WPILIB Camera Code Implementation HELP

shawn.schwartz · February 10, 2013, 5:59pm

Hello CD,

Can anyone please provide help or an explanation of how to implement the sample Vision Code for tracking the Targets in this year’s game? For example, how can we take the provided “scoring methods” and use them to make a motor run faster, or rotate an arm at an angle? Thank you very much.

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.usfirst.frc4.Team4Ascent.subsystems;

import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.LinearAverages;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVision.Rect;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.image.RGBImage;

/**
 *
 * @author robotics4
 */
public class Camera{
    //Constants from WPI
    final int XMAXSIZE = 24;
    final int XMINSIZE = 24;
    final int YMAXSIZE = 24;
    final int YMINSIZE = 48;
    final double xMax] = {1, 1, 1, 1, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, 1, 1, 1, 1};
    final double xMin] = {.4, .6, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, .1, 0.6, 0};
    final double yMax] = {1, 1, 1, 1, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, .5, 1, 1, 1, 1};
    final double yMin] = {.4, .6, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05,
								.05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05, .05,
								.05, .05, .6, 0};
    final int RECTANGULARITY_LIMIT = 60;
    final int ASPECT_RATIO_LIMIT = 75;
    final int X_EDGE_LIMIT = 40;
    final int Y_EDGE_LIMIT = 60;
    
    final int X_IMAGE_RES = 320;          //X Image resolution in pixels, should be 160, 320 or 640
//    final double VIEW_ANGLE = 43.5;       //Axis 206 camera
    final double VIEW_ANGLE = 48;       //Axis M1011 camera
    
    AxisCamera camera;          // the axis camera object (connected to the switch)
    CriteriaCollection cc;      // the criteria for doing the particle filter operation
    
    public class Scores {
        double rectangularity;
        double aspectRatioInner;
        double aspectRatioOuter;
        double xEdge;
        double yEdge;
    }

    public void init() {
        cc = new CriteriaCollection();
        cc.addCriteria(MeasurementType.IMAQ_MT_AREA, 500, 65535, false);
    }
    public void auton(){
        try {
            //ColorImage image = camera.getImage();     // comment if using stored images
            ColorImage image;                           // next 2 lines read image from flash on cRIO
            image = new RGBImage("/testImage.jpg");		// get the sample image from the cRIO flash
            BinaryImage thresholdImage = image.thresholdHSV(60, 100, 90, 255, 20, 255);   // keep only red objects
            //thresholdImage.write("/threshold.bmp");
            BinaryImage convexHullImage = thresholdImage.convexHull(false);          // fill in occluded rectangles
            //convexHullImage.write("/convexHull.bmp");
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           //filteredImage.write("/filteredImage.bmp");
            //filteredImage.write("/filteredImage.bmp");
            
            Scores scores] = new Scores[filteredImage.getNumberParticles()];
                for (int i = 0; i < scores.length; i++) {
                    ParticleAnalysisReport report = filteredImage.getParticleAnalysisReport(i);
                    scores* = new Scores();
                    
                    scores*.rectangularity = scoreRectangularity(report);
                    scores*.aspectRatioOuter = scoreAspectRatio(filteredImage,report, i, true);
                    scores*.aspectRatioInner = scoreAspectRatio(filteredImage, report, i, false);
                    scores*.xEdge = scoreXEdge(thresholdImage, report);
                    scores*.yEdge = scoreYEdge(thresholdImage, report);

                    if(scoreCompare(scores*, false))
                    {
                        System.out.println("particle: " + i + "is a High Goal  centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
			System.out.println("Distance: " + computeDistance(thresholdImage, report, i, false));
                    } else if (scoreCompare(scores*, true)) {
			System.out.println("particle: " + i + "is a Middle Goal  centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
			System.out.println("Distance: " + computeDistance(thresholdImage, report, i, true));
                    } else {
                        System.out.println("particle: " + i + "is not a goal  centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
                    }
                    System.out.println("rect: " + scores*.rectangularity + "ARinner: " + scores*.aspectRatioInner);
                    System.out.println("ARouter: " + scores*.aspectRatioOuter + "xEdge: " + scores*.xEdge + "yEdge: " + scores*.yEdge);	
                    filteredImage.free();
                    convexHullImage.free();
                    thresholdImage.free();
                    image.free();    
                }
        } catch (NIVisionException ex) {
            ex.printStackTrace();
        }
    }
    double computeDistance (BinaryImage image, ParticleAnalysisReport report, int particleNumber, boolean outer) throws NIVisionException {
            double rectShort, height;
            int targetHeight;

            rectShort = NIVision.MeasureParticle(image.image, particleNumber, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
            //using the smaller of the estimated rectangle short side and the bounding rectangle height results in better performance
            //on skewed rectangles
            height = Math.min(report.boundingRectHeight, rectShort);
            targetHeight = outer ? 29 : 21;

            return X_IMAGE_RES * targetHeight / (height * 12 * 2 * Math.tan(VIEW_ANGLE*Math.PI/(180*2)));
    }
    public double scoreAspectRatio(BinaryImage image, ParticleAnalysisReport report, int particleNumber, boolean outer) throws NIVisionException
    {
        double rectLong, rectShort, aspectRatio, idealAspectRatio;

        rectLong = NIVision.MeasureParticle(image.image, particleNumber, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
        rectShort = NIVision.MeasureParticle(image.image, particleNumber, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
        idealAspectRatio = outer ? (62/29) : (62/20);	//Dimensions of goal opening + 4 inches on all 4 sides for reflective tape
	
        //Divide width by height to measure aspect ratio
        if(report.boundingRectWidth > report.boundingRectHeight){
            //particle is wider than it is tall, divide long by short
            aspectRatio = 100*(1-Math.abs((1-((rectLong/rectShort)/idealAspectRatio))));
        } else {
            //particle is taller than it is wide, divide short by long
                aspectRatio = 100*(1-Math.abs((1-((rectShort/rectLong)/idealAspectRatio))));
        }
	return (Math.max(0, Math.min(aspectRatio, 100.0)));		//force to be in range 0-100
    }
    
    /**
     * Compares scores to defined limits and returns true if the particle appears to be a target
     * 
     * @param scores The structure containing the scores to compare
     * @param outer True if the particle should be treated as an outer target, false to treat it as a center target
     * 
     * @return True if the particle meets all limits, false otherwise
     */
    boolean scoreCompare(Scores scores, boolean outer){
            boolean isTarget = true;

            isTarget &= scores.rectangularity > RECTANGULARITY_LIMIT;
            if(outer){
                    isTarget &= scores.aspectRatioOuter > ASPECT_RATIO_LIMIT;
            } else {
                    isTarget &= scores.aspectRatioInner > ASPECT_RATIO_LIMIT;
            }
            isTarget &= scores.xEdge > X_EDGE_LIMIT;
            isTarget &= scores.yEdge > Y_EDGE_LIMIT;

            return isTarget;
    }
    
    /**
     * Computes a score (0-100) estimating how rectangular the particle is by comparing the area of the particle
     * to the area of the bounding box surrounding it. A perfect rectangle would cover the entire bounding box.
     * 
     * @param report The Particle Analysis Report for the particle to score
     * @return The rectangularity score (0-100)
     */
    double scoreRectangularity(ParticleAnalysisReport report){
            if(report.boundingRectWidth*report.boundingRectHeight !=0){
                    return 100*report.particleArea/(report.boundingRectWidth*report.boundingRectHeight);
            } else {
                    return 0;
            }	
    }
    
    /**
     * Computes a score based on the match between a template profile and the particle profile in the X direction. This method uses the
     * the column averages and the profile defined at the top of the sample to look for the solid vertical edges with
     * a hollow center.
     * 
     * @param image The image to use, should be the image before the convex hull is performed
     * @param report The Particle Analysis Report for the particle
     * 
     * @return The X Edge Score (0-100)
     */
    public double scoreXEdge(BinaryImage image, ParticleAnalysisReport report) throws NIVisionException
    {
        double total = 0;
        LinearAverages averages;
        
        Rect rect = new Rect(report.boundingRectTop, report.boundingRectLeft, report.boundingRectHeight, report.boundingRectWidth);
        averages = NIVision.getLinearAverages(image.image, LinearAverages.LinearAveragesMode.IMAQ_COLUMN_AVERAGES, rect);
        float columnAverages] = averages.getColumnAverages();
        for(int i=0; i < (columnAverages.length); i++){
                if(xMin(i*(XMINSIZE-1)/columnAverages.length)] < columnAverages* 
                   && columnAverages* < xMax*){
                        total++;
                }
        }
        total = 100*total/(columnAverages.length);
        return total;
    }
    
    /**
	 * Computes a score based on the match between a template profile and the particle profile in the Y direction. This method uses the
	 * the row averages and the profile defined at the top of the sample to look for the solid horizontal edges with
	 * a hollow center
	 * 
	 * @param image The image to use, should be the image before the convex hull is performed
	 * @param report The Particle Analysis Report for the particle
	 * 
	 * @return The Y Edge score (0-100)
	 *
    */
    public double scoreYEdge(BinaryImage image, ParticleAnalysisReport report) throws NIVisionException
    {
        double total = 0;
        LinearAverages averages;
        
        Rect rect = new Rect(report.boundingRectTop, report.boundingRectLeft, report.boundingRectHeight, report.boundingRectWidth);
        averages = NIVision.getLinearAverages(image.image, LinearAverages.LinearAveragesMode.IMAQ_ROW_AVERAGES, rect);
        float rowAverages] = averages.getRowAverages();
        for(int i=0; i < (rowAverages.length); i++){
                if(yMin(i*(YMINSIZE-1)/rowAverages.length)] < rowAverages* 
                   && rowAverages* < yMax*){
                        total++;
                }
        }
        total = 100*total/(rowAverages.length);
        return total;
    }
}

Ginto8 · February 10, 2013, 6:57pm

Once you have the locations of the targets’ centers, there’s a lot of information that can be extracted and used:

You can use the horizontal and vertical field-of-view angles to find the relative horizontal and vertical angles of each target
You can use these relative angles to determine the setpoints for PID loops controlling the turn/tilt of your shooter – I’d recommend a gyro for turn feedback and an accelerometer for tilt feedback.
*]The angles, along with the angle and height of your camera and the height of the targets, can be used to determine how far away the target is. This can then be plugged into a look-up table populated by experimental results to choose the best speed.

shawn.schwartz · February 10, 2013, 7:37pm

Thank you so much. Is there anyway you could provide any sample methods/code with a simple motor implementation? Thank you again.

Ginto8 · February 10, 2013, 8:06pm

I’m not quite sure what you mean by “simple motor implementation.” A shooter wheel control? In that case, you’ll want to look either into a PID control or a Bang-Bang control (http://www.chiefdelphi.com/media/papers/2663). An example of turning toward the target? If you don’t have a turret, you can use a PIDCommand or something similar with the output set to something like drive.tankdrive(output,-output) and the input set to something like gyro.getAngle() (assuming drive is a RobotDrive and gyro is a Gyro). Tilting gets a little more complicated because you probably need to use an accelerometer, which only measures angle indirectly.

shawn.schwartz · February 10, 2013, 8:57pm

What I meant was that with the methods provided in the above code from WPI, how can these methods be called to make a robot change its angle or move its motors at a certain speed, depending on the output of camera data from the methods? Thanks.

Ginto8 · February 10, 2013, 10:09pm

Reading through it, it seems that this chunk is where all the actual information is extracted:

                    if(scoreCompare(scores*, false))
                    {
                        System.out.println("particle: " + i + "is a High Goal  centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
			System.out.println("Distance: " + computeDistance(thresholdImage, report, i, false));
                    } else if (scoreCompare(scores*, true)) {
			System.out.println("particle: " + i + "is a Middle Goal  centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
			System.out.println("Distance: " + computeDistance(thresholdImage, report, i, true));
                    } else {
                        System.out.println("particle: " + i + "is not a goal  centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
                    }

If you want access to that data elsewhere, you can either return it from the function, or assign it to member variables in the class. An example of how to return it:

public class ImageResults {
    public boolean isHighGoal;
    public double centerX,centerY,distance;

    public ImageResults(...) { ... } // just pass in data and initialize the members with this
}

public ImageResults processImage() { // a clearer name than auton
    // .. snip ..
    if(scoreCompare(scores*, false))
    {
        return new ImageResults(true,report.center_mass_x_normalized,report.center_mass_y_normalized,computeDistance(thresholdImage,report,i,false));
    } else if (scoreCompare(scores*, true)) {
        return new ImageResults(false,report.center_mass_x_normalized,report.center_mass_y_normalized,computeDistance(thresholdImage,report,i,false));
    } else {
        return null;
    }
}

Then elsewhere in code:

ImageResults results = camera.processImage();
if(results == null) {
    return; // no valid target found
}
double horizAngle = results.centerX*cameraHFOV/2; // centerX is normalized, so -1 is left, 1 is right. Multiply by cameraHFOV/2, and you get the horizontal angle of the target relative to the camera
new PIDTurn(gyro.getAngle()+horizAngle).start();

PIDTurn is assumed to be a class extending PIDCommand, taking an angle setpoint in its constructor. An (incomplete) example implementation would be:

class PIDTurn extends PIDCommand {
    // ...
    public PIDTurn(double setpoint) {
        super(kP,kI,kD);
        setSetpoint(setpoint);
        requires(driveTrain);
        // ...
    }

    protected double returnPIDInput() {
        return driveTrain.gyro.getAngle();
    }
    protected void usePIDOutput(double output) {
        driveTrain.tankDrive(output,-output);
    }
    // ...
}

Just as a caveat, this code will not work on its own, and may not work at all. I’m trying to get the theory across so that you can write (and understand) your own code.

In fact, reading over my code again, I’m pretty sure it will create a memory leak if inserted directly into your code. The free() calls in auton() are important.****

system · January 5, 2020, 6:29am