Angle Calculation from image of goal

[Jared Russell]

Originally Posted by TheOtherGuy

For example, if the camera is 90° horizontal FoV and the resolution is 640 horizontally, then a simple equation would be

angle = (goalx - 640/2) * 90/640

This can be a decent enough approximation, but there is a more correct way to do this conversion:

horizontal_angle_to_goal = atan((goal_x - center_x) / focal_length_pixels)

where:
focal_length_pixels = 
.5 * image_width_pixels / tan(horizontal_field_of_view / 2)

The idea of a focal length is a little unintuitive at first, but is explained here: https://en.wikipedia.org/wiki/Angle_of_view

Typically, unless you calibrated your camera to compensate for manufacturing imperfections (total overkill for FRC):

center_x = (image_width_pixels / 2 - .5)

The -.5 compensates for the fact that if there are an even number of columns/rows in your image, the center is actually on the border between two of them (and we start counting rows/cols from 0 typically).

Note that these equations do give slightly different answers! (See attached image...red is the correct equation, blue is the approximate linear one)

Also note that this angle is relative to the camera...you need to whip out some more trig depending on the angle of the camera mount relative to its robot.