Go to Post they just build excellence and silently wait to deploy their creation on the unsuspecting masses - Koko Ed [more]
Home
Go Back   Chief Delphi > Technical > Programming > Java
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rating: Thread Rating: 2 votes, 5.00 average. Display Modes
  #1   Spotlight this post!  
Unread 10-02-2013, 17:59
shawn.schwartz's Avatar
shawn.schwartz shawn.schwartz is offline
Lead Systems Engineer
FRC #0004 (Team 4 ELEMENT)
Team Role: Programmer
 
Join Date: Feb 2012
Rookie Year: 2011
Location: Lake Balboa, CA
Posts: 27
shawn.schwartz is an unknown quantity at this point
WPILIB Camera Code Implementation HELP

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.

Code:
/*
 * 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[i] = new Scores();
                    
                    scores[i].rectangularity = scoreRectangularity(report);
                    scores[i].aspectRatioOuter = scoreAspectRatio(filteredImage,report, i, true);
                    scores[i].aspectRatioInner = scoreAspectRatio(filteredImage, report, i, false);
                    scores[i].xEdge = scoreXEdge(thresholdImage, report);
                    scores[i].yEdge = scoreYEdge(thresholdImage, report);

                    if(scoreCompare(scores[i], 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[i], 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[i].rectangularity + "ARinner: " + scores[i].aspectRatioInner);
                    System.out.println("ARouter: " + scores[i].aspectRatioOuter + "xEdge: " + scores[i].xEdge + "yEdge: " + scores[i].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[i] 
                   && columnAverages[i] < xMax[i*(XMAXSIZE-1)/columnAverages.length]){
                        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[i] 
                   && rowAverages[i] < yMax[i*(YMAXSIZE-1)/rowAverages.length]){
                        total++;
                }
        }
        total = 100*total/(rowAverages.length);
        return total;
    }
}
__________________
Shawn Schwartz
Programming/Head of Website & Media, Team 4 ELEMENT
team4element.com
"Innovation distinguishes between a leader and a follower." -Steve Jobs
Reply With Quote
  #2   Spotlight this post!  
Unread 10-02-2013, 18:57
Ginto8's Avatar
Ginto8 Ginto8 is offline
Programming Lead
AKA: Joe Doyle
FRC #2729 (Storm)
Team Role: Programmer
 
Join Date: Oct 2010
Rookie Year: 2010
Location: Marlton, NJ
Posts: 174
Ginto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of light
Re: WPILIB Camera Code Implementation HELP

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.
__________________
I code stuff.
Reply With Quote
  #3   Spotlight this post!  
Unread 10-02-2013, 19:37
shawn.schwartz's Avatar
shawn.schwartz shawn.schwartz is offline
Lead Systems Engineer
FRC #0004 (Team 4 ELEMENT)
Team Role: Programmer
 
Join Date: Feb 2012
Rookie Year: 2011
Location: Lake Balboa, CA
Posts: 27
shawn.schwartz is an unknown quantity at this point
Re: WPILIB Camera Code Implementation HELP

Quote:
Originally Posted by Ginto8 View Post
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.
Thank you so much. Is there anyway you could provide any sample methods/code with a simple motor implementation? Thank you again.
__________________
Shawn Schwartz
Programming/Head of Website & Media, Team 4 ELEMENT
team4element.com
"Innovation distinguishes between a leader and a follower." -Steve Jobs
Reply With Quote
  #4   Spotlight this post!  
Unread 10-02-2013, 20:06
Ginto8's Avatar
Ginto8 Ginto8 is offline
Programming Lead
AKA: Joe Doyle
FRC #2729 (Storm)
Team Role: Programmer
 
Join Date: Oct 2010
Rookie Year: 2010
Location: Marlton, NJ
Posts: 174
Ginto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of light
Re: WPILIB Camera Code Implementation HELP

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.
__________________
I code stuff.
Reply With Quote
  #5   Spotlight this post!  
Unread 10-02-2013, 20:57
shawn.schwartz's Avatar
shawn.schwartz shawn.schwartz is offline
Lead Systems Engineer
FRC #0004 (Team 4 ELEMENT)
Team Role: Programmer
 
Join Date: Feb 2012
Rookie Year: 2011
Location: Lake Balboa, CA
Posts: 27
shawn.schwartz is an unknown quantity at this point
Re: WPILIB Camera Code Implementation HELP

Quote:
Originally Posted by Ginto8 View Post
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.
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.
__________________
Shawn Schwartz
Programming/Head of Website & Media, Team 4 ELEMENT
team4element.com
"Innovation distinguishes between a leader and a follower." -Steve Jobs
Reply With Quote
  #6   Spotlight this post!  
Unread 10-02-2013, 22:09
Ginto8's Avatar
Ginto8 Ginto8 is offline
Programming Lead
AKA: Joe Doyle
FRC #2729 (Storm)
Team Role: Programmer
 
Join Date: Oct 2010
Rookie Year: 2010
Location: Marlton, NJ
Posts: 174
Ginto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of lightGinto8 is a glorious beacon of light
Re: WPILIB Camera Code Implementation HELP

Reading through it, it seems that this chunk is where all the actual information is extracted:
Code:
                    if(scoreCompare(scores[i], 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[i], 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:
Code:
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[i], false))
    {
        return new ImageResults(true,report.center_mass_x_normalized,report.center_mass_y_normalized,computeDistance(thresholdImage,report,i,false));
    } else if (scoreCompare(scores[i], 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:
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:
Code:
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.
__________________
I code stuff.
Reply With Quote
Reply


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 12:54.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi