Tower Tracker 1.0

[Mr. Rick]

Originally Posted by Fauge7

Team 3019 graciously presents their vision tracking program for everybody to use and borrow! Now instead of dreaming about the perils of computer vision YOU can bring your team the joy of having a robot that is capable of tracking the target with ease! No more Grip crashes or weird deploys, can calculate fun things such as: distance to target, and angle to target! to be used to auto align and auto aim!

if you are going to modify the code, all i ask is give me and my team credit in a comment at the top of the code and comment your suggestions and or your praise!

to install:

• download opencv 3.1 from here
• download Network table 3.0 jar
• make a new project in eclipse
• make opencv and networktables added as a user library to the build path of your new project
• copy opencv_ffmpeg310_64.dll from C:\Path\to\opencv\build\bin to C:\Windows\System32
• add the code in a class that is named TowerTracker.java
• when your ready to export
• export the .jar file as a runnable jar
• move the .jar to a folder similar to this
• run the .jar with "java -jar c:\Path\to\TowerTracker.jar" on a command prompt window

the code is just an example of what it can do, i can add network table stuff soon but i thought i would publish it first!
github link

want to see an example of what it can output?
here you go!

how it works: using an axis camera or a mjpeg streamer you can use a stream of a webcam to process images using an opencv program that runs on the driver station computer. This program can be modified to run on a coprocessor and directly input to the roborio for even better results because network tables can only go at 10hz vs the camera stream which is 30hz...this program can easily be ported over to c++ and python and would probably run better with those as c++ and python are way more supported then java with opencv.

Fauge7, thanks so much for taking the time to write this out! We are trying to use your solution but there are a few errors in your github code. I fixed a couple of them (for example, line 87 and a few merge artifacts left over near the bottom), but I'm still getting the following error when running the jar file in the command line:

Exception in thread "main" java.lang.NullPointerException at org.usfirst.frc.team5407.robot.TowerTracker.main(TowerTracker.java:107)

We are totally new to java, or any language for that matter. Does anyone have working java code they would be willing to share?? Thanks!

[Fauge7]

Yes, I messed up the github code but luckly one of my team members has the code on his github. https://github.com/Aventek/TowerTracker3019Modified

Hopefully this helps, pm me if you have any questions!

[E, Palmer]

This may be a dumb question. But could someone please explain to me the "fun" math i am not really sure how its doing what it is doing.....

[euhlmann]

Originally Posted by E, Palmer

This may be a dumb question. But could someone please explain to me the "fun" math i am not really sure how its doing what it is doing.....

I can try

y = rec.br().y + rec.height / 2;
y= -((2 * (y / matOriginal.height())) - 1);

These two lines are a bit confusing (lesson to be learned here: don't write spaghetti code in a public code demo ). Let's rearrange the operations

double half_image_height = matOriginal.height() / 2;
double pixel_y = half_image_height - (rec.br().y + rec.height / 2);
y = pixel_y / half_image_height;

Unless I'm missing something (which is likely), this doesn't seem quite right. Notice the rec.br().y+rec.height()/2. I would change that to rec.br().y-rec.height()/2 so it finds the middle y-coordinate of the bounding rectangle.

With that change, this operation makes a bit more sense. The point is to calculate the offset in normalized coordinates from the middle of the target to the horizon. The horizon is assumed to be at (matOriginal.height()/2). First, it subtracts the y-coordinate of the middle of the target from half of the image size in pixels. This results in the pixel offset between the horizon line on the image to the target y-coordinate. Then, the entire thing is normalized by dividing it by half the pixel image height.

distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) / Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180);

This section is more clear right off the bat.

There are two parts. First, the angle from the horizon to the target is approximated using the small angle approximation.

(y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180

The small angle approximation states that sin(x) = x for small values of x (in radians; in practice anything up to about 30 degrees), so they used linear scaling to calculate the angle rather than the more precise

angle = arcsin( pixel_y * sin(VERTICAL_FOV/2.0) / half_image_height )

(Remember that y is actually pixel_y/half_image_height)
Now the camera may be tilted relative to the field, meaning that its local "horizon" isn't the same, so the offset CAMERA_ANGLE is added, for the angle that the camera is tilted relative to the actual horizon.

Now the final part is to calculate the distance using the known height of the target and this calculated angle to the horizon.

h is the distance from the camera's height to the target's height (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT), so tan(alpha) = h/d => d = h/tan(alpha)

I hope this explains it.

[E, Palmer]

I am very likely working on a faulty understanding of FOV but wouldn't this be much simpler?

double AngleToHalfScreen = Vertical_FOV/2;
      
        double OffsetFromMiddle = Math.abs(targetY - pixelHeight/2);

        double FractionofOffset = OffsetFromMiddle/(pixelHeight/2);

        FinalAngle =  AngleToHalfScreen+(AngleToHalfScreen*FractionofOffset);

This is way to simple to be right.

Thank you so much for taking the time to explain this.