Thread created automatically to discuss a document in CD-Media.
Team 341 Vision System Code
by: Jared341
SmartDashboard Widget used by FIRST Team 341 for automatic targeting during the 2012 competition season.
Now that the 2012 FIRST Championships are a (painful) memory, Team 341 is releasing its laptop-based vision system code to all teams so that they can learn from it and/or use it in offseason competitions.
DaisyCV.zip (82.6 KB)
Team341TestImages.zip (189 KB)
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/listReleases/projects.smartdashboard/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!
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.
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.
1323 cannot thank Jared and 341 for helping us with vision. By champs we were one of the best shooters in our division.
-RC
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.
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 :).
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.
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
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);
}
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.
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);
}
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 
Ok, fine.
return(((angle%360)+360)%360);
I feel like at this point the original code was easier to understand from an outside perspective.
On another note, this is AWESOME! It’s nice to see another team using Java to program, especially when it’s done so well. Our camera tracking system is currently only used in auton to track after grabbing the third ball off the co-op bridge. It’s very rudimentary code so that it can be run on the cRio (we had issues with a bad camera that made it impossible to test any sort of laptop based vision code). This is very interesting, and I’m excited to go over it in more detail later.
Thanks for posting this, Jared. I took a look at your dashboard on the field at Champs and was very impressed. It seems incredibly useful to have the vision system draw its idea of target state on top of the real image.
I suspect if vision is a part of upcoming games, we will probably use a solution very similar to this. This year we wrote all of our vision code on top of WPILib/NIVision to run on the cRIO. In the end we got this to work pretty well, but development and debugging was a bit of a pain compared to your system.
Very cool! I just finished taking an online data structures course from IIT, so it was really awesome to see the implementation of the TreeMap! Just so that I’m sure I understand, it is used to quickly find an rpm based on your distance, right? What are the advantages (if any) of using this instead of plugging your data points into an excel graph and getting a polynomial best fit equation to transform your distance into an rpm?