Code for this project can be found here.

The idea was to use OpenCV and Python, with a Linux OS, to sense the distance to a beacon (with known coordinates) using stereo vision. After calculating the distance to two beacons, the intersection of circles centered about the beacon's known locations with radii equal to the calculated distances would be used to localize the robot. Knowing the (x,y) coordinates of the robot within the constructed coordinate frame (constructed using the beacons), the position of the robot would be found and appropriate correction to keep the robot on course would be sent to the motors.

Here is a video of the Python software which finds a beacon (red and blue circles on paper) in the left and right camera, and outputs the difference in horizontal pixels. Take note that no calculations take place unless the beacon is properly oriented. This was a necessary feature to get rid of all of the detected blobs that are not beacons, i.e noise rejection.

To find the Hue values of the circles that were printed out, I used the program pick_and_track.py found here. The color tracking algorithm I use is based on the multi_color_two_object.py example found in the same place. The circle intersection algorithm is based on analysis performed in this document:

It should also be noted that in the video, the left and right camera feeds are "warped" to get rid of lens distortion. This is accomplished by using camera calibration code and a calibration image which can be printed out here.

In the end, the FLPR was not able to localize using stereo vision (especially not over large distances). Basic computer vision was successfully used to make corrections to the FLPR trajectory when moving in a straight line towards a beacon, but this is a much much simpler problem to solve. The simple control of the robot was implemented using a PID on horizontal pixel location of the beacon centroid. The size of the beacon was determined by the width of the beacon's bounding rectangle, which was then used to determine distance. The video below shows active correction while moving in a straight line towards the beacon, but there were still some bugs to work out on the turn :).