paper: Team 341 Vision System Code

[Jared Russell]

Attached is a zip file containing everything you need to run FIRST Team 341's laptop-based vision system. We are very proud of this software, as it propelled us to 4 Regional/District Championships and the Curie Division Finals. Every shot our robot attempted in Rebound Rumble was completely controlled by this code.

To get it up and running on your computer (which can be Windows, Linux, or Mac OSX), you first should install the WPI SmartDashboard using the installer which you can find here: http://firstforge.wpi.edu/sf/frs/do/.../frs.installer

Note that for Linux or OSX builds (or if you just want to use more recent libraries than are included in the SmartDashboard installer on Windows), you will need to do the installation by hand. The libraries you will need are OpenCV (http://opencv.willowgarage.com/wiki/), JavaCV (http://code.google.com/p/javacv/), and FFMPEG (http://ffmpeg.org/). Make sure that the OpenCV and JavaCV versions you install are compatible! Meaning the same version number of OpenCV, and the same architecture (x86 or x86-64). You then need to make sure the paths to the OpenCV libraries are in your system path.

To use the widget with the Axis camera, you should configure your camera to allow anonymous viewing and make sure it is connected directly to your wireless bridge (not the cRIO). The IP must also be set by right clicking on the SmartDashboard widget. For best results, you will also want to ensure that your camera has a very fast exposure time (we used 1/120s). The FIRST control system documentation and vision whitepaper go over how to do this.

You can also run the widget in a "stand alone" fashion, as the class has a Java main method that you can use to load images from disk and display the processed results. Sample images are included.

Our vision system uses OpenCV library functions to successively filter the raw camera image and find the vision targets. The highest target detected is then used to compute range and bearing information. With a statically mounted camera, we use inverse trigonometry to find the distance to the target based on the height in the image. Bearing is computed by adding the offset of the target horizontally in the camera frame to the heading we are getting back from the robot. This lets us use a closed loop controller on the robot to use the gyro to "aim". If your robot has a turret, you could instead send the turret angle back to the SmartDashboard and get the same results. The computed range is used to find an optimal shooter RPM, which also gets sent back to the robot. We interpolate between points in our "range-to-RPM" lookup table in order to provide smooth outputs to our shooter wheel speed control loop.

Thanks to Brad Miller, WPI, Kevin O'Connor, and the FRC Control System team for making OpenCV-based vision processing a reality in 2012!

[Brandon Holley]

Thanks for publishing this. Your tracking was absolutely deadly all season. I'm curious to see how our final algorithm stacks up against your code.

[JamesTerm]

Thanks so much for posting this... looking forward to reviewing this. It was nice to meet you... BTW I have worked with FFmpeg professionally since 2006, so if you ever have any issues with it, perhaps I can help.

[R.C.]

1323 cannot thank Jared and 341 for helping us with vision. By champs we were one of the best shooters in our division.

-RC

[rainbowdash]

Thanks for the code!

Great job with your camera tracking, Miss Daisy! It was exciting watching all your matches! I know everyone in the chat room was very shocked and sad when you guys got eliminated.

Good luck at your off- season competitions!

Your tracking system was legendary this year, 341. Heh... One of our running jokes was that your team had an aimbot.

My nerdy friends always called out "HAAAAAXXXX!!1!" when you were on the field. You're pretty good when just playing the game is cheating compared to lesser robots.

[Ziv]

This is really cool! I'm looking forward to taking a close look at this at a non-12AM time, but a quick skim left me hungry for more.

We used a much more primitive system to find targets. After even very lenient RGB thresholding, we found that ignoring small particles and filtering the remaining ones for high normalized moment of inertia was sufficient. However, we didn't use any measurements other than the x coordinate of the center of the targets. Your code will no doubt be studied by a great many people, hopefully including all of 125's trainee programmers .

[hodgepodge]

It's great to see how others approached the vision processing. I wrote the vision code for team 118 this year and from a quick glance, your algorithm is quite similar.

Our code ran onboard on a BeagleBone running Linux and straight OpenCV, written in C++. libCurl was used to capture images and a UDP socket spewed angle and distance data to the cRio.

[BradAMiller]

Jared -

Thanks so much for posting your code. It was good meeting you at the regional and amazing watching your teams robot do it's stuff. This will go a long way to helping teams learn about computer vision processing and raise the bar for future competitions. Expect to see better documentation and improvements for next season.

And thanks to Joe Grinstead and Greg Granito for helping develop the code over last summer at WPI. They did a great job with the camera code and network tables extensions to WPILib.

Brad

[sebflippers]

Thanks for the code. I believe lines 322-334:

private double boundAngle0to360Degrees(double angle)
    {
        // Naive algorithm
        while(angle >= 360.0)
        {
            angle -= 360.0;
        }
        while(angle < 0.0)
        {
            angle += 360.0;
        }
        return angle;
    }

could be replaced with

private double boundAngle0to360Degrees(double angle)
    {
        return(abs(angle)%360.0);
    }

[Ziv]

Originally Posted by sebflippers

Thanks for the code. I believe lines 322-334 [snipped] could be replaced with

private double boundAngle0to360Degrees(double angle)
    {
        return(abs(angle)%360.0);
    }

Consider the angle -90. Daisy's code returns 270, but yours returns 90. I think you're looking for something more like this:

private double boundAngle0to360Degrees(double angle)
    {
        double ret = abs(angle)%360.0;
        if(angle < 0.0)
        {
            ret = -ret;
        }
        return(ret);
    }

[sebflippers]

Originally Posted by Ziv

Consider the angle -90. Daisy's code returns 270, but yours returns 90. I think you're looking for something more like this:

private double boundAngle0to360Degrees(double angle)
    {
        double ret = abs(angle)%360.0;
        if(angle < 0.0)
        {
            ret = -ret;
        }
        return(ret);
    }

right.

[sebflippers]

done. This one kind of defeats the purpose of making your code simpler, though.

	public static double boundAngle0to360Degrees(double angle)
    {
        return(angle > 0.0? angle % 360.0 : 360.0*(1 + (Math.abs((int)angle)/360))+angle);
    }

[Gray Adams]

Originally Posted by sebflippers

done. This one kind of defeats the purpose of making your code simpler, though.

	public static double boundAngle0to360Degrees(double angle)
    {
        return(angle > 0.0? angle % 360.0 : 360.0*(1 + (Math.abs((int)angle)/360))+angle);
    }

Why not just...

 public static double boundAngle0to360Degrees(double angle)
    {
        return((angle+360)%360);
    }

[Jared Russell]

Originally Posted by Gray Adams

Why not just...

 public static double boundAngle0to360Degrees(double angle)
    {
        return((angle+360)%360);
    }

What if angle is equal to -361?

It's awesome that you guys are analyzing the code and you have already taught me something new (that Java's "%" operator works on floating point values...as a primarily C++ guy, I have it burned into my brain that thou shalt use "fmod" for floating point modulus). But if this is the part of the code that engenders the most discussion, then I'm a bit disappointed