One of the things I’ve seen on Chief Delphi and other FIRST forums is teams trying to start using vision. With all the emphasis Dean put on Machine Learning and AI it’s hard to imagine many teams without some form of it. This post will be a short guide on how to get started with some basic vision using some simple python and a processor. My team uses the Jetson Nano, but this tiny project should work with a Raspberry Pi 4.
To start off, you should always know the theory behind the code before you actually try and write the code, this enables you to understand what you are doing and change it to what you want. Distance calculation is not super difficult. We won’t even be using Tensorflow today. The first thing you want to know is the perceived focal length of your camera/webcam. Most teams use the Microsoft camera FIRST choice provides, or maybe the Pixy 2 camera. Whatever camera you use the equation is the same. F = (P x D) / W. Let’s say we want to be able to detect the distance of the robot to the low goal. The width of the low goal is around 85 cm. That’s our width / W. Next we need to find D. D stands for Distance. To do this your team has to have a field element replica of the low goal. Once that is finished, stand two feet away from the low goal and take a picture. We will use this picture later to “calibrate” our calculator. Next we find P, the apparent width in pixels, which will be calculated in our program. Finally, from this equation we can create the distance equation, D = (W x F) / P. We use the first to grab F, and then use F to find D at any point.
Now that the boring theory is over, lets get programming!
This will be in Python since OpenCV works very well with it, but feel free to convert it to another language, or PM me for help if there’s something you want to change.
Let’s start. First create a new python file, name it whatever you want, my team has a bad habit of using names that don’t match the code, but I would highly recommend against that. First import the required libraries, we will be using imutils, numpy, and cv2.
import imutils
import numpy
from imutils import paths
import cv2
If you don’t have these packages run sudo apt-get install cv2, imutils, numpy in the terminal of the rasbian os. (Or whatever OS you are using).
Next we need to find the object. We do this by writing a function that takes a singular argument ‘image’ that converts it to grayscale, blurs it, and detects the edges. Then we find the contour (outline) that represents the low goal. After we find all the contours, the final line (17) finds the contour with the largest area which most likely is the low goal. In this example, most of the time the low goal will be the biggest contour, but there are other things you can use to lower the error rate. For now this should do. Now that we have the contour of the low goal, we return the bounding box of the low goal that gives us the x,y cordinates, width and height in pixels
def find_marker(image):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (5, 5) , 0)
edged = cv2.Canny(gray , 35, 125)
cnts = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
c = max(cnts, key = cv2.contourArea)
return cv2.minAreaRect(c)
Next we write a function that will calculate the distance between the camera and the low goal using the triangle similairty equation above.
def distance_to_camera(knownWidth, focalLength, perWidth):
return (knownWidth * focalLength) / perWidth
Next we need to compute the focal length to use in the distance equation. We do this by using the image we took at the beginning as a calibration image. First declare two variables, they should be the distance you took the picture from (I think above I said do two feet),
and the width of the low goal, which is around 86 cm. Next call the image you took by using cv2’s imread function, and input the name of your file. then call the find_marker function you made earlier and store it in marker. Finally, find the focal length by using the focal length equation at the beginning, using marker as P, KNOWN_DISTANCE as D, and KNOWN_WIDTH as W.
KNOWN_DISTANCE = 24.0
KNOWN_WIDTH = 86.0
image = cv2.imread("nameofimage.png")
marker = find_marker(image)
focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
It’s the time for the final step (You all are probably thinking it’s about time! ) The final goal is to use the functions and calibration we have done in a live webcam feed. Using cv2, this is entirely possible, although I don’t recommend doing it through the RIO, as stated above a RasPi or better is recommended.
First we must setup our webcam using cv2. Use 0 as the default for your camera. Next we grab the captured images, (ret is a boolean variable that will return true if the image is available). Next we find the marker and store it in marker, the find the distance to camera and store it in cm. Next we set up the output by drawing the bounding box. We then contour the video feed, put text displaying the distance in cm, and finally outputting the video feed. We then set a waitkey that says hey if the user presses q then quit the program and destroy the window.
cam = cv2.VideoCapture(0)
ret, video = cam.read()
marker = find_marker(video)
cm = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
box = cv2.cv.BoxPoints(marker) if imutils.is_cv2() else cv2.boxPoints(marker)
box = np.int0(box)
cv2.drawContours(video, [box], -1, (0, 255, 0), 2)
cv2.putText(image, "% cm" % (cm), (video.shape[1] - 200, video.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 255, 0), 3)
cv2.imshow("video", video)
if cv2.waitKey(25) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
And that;s it you guys! I hope you all learned something or took a new idea from this. This is only the very basic version of distance detection our team uses (Can’t give it all away before comp). If you have any questions on modifying this or ho to get the output to go to your computer from the RasPi feel free to PM me. there are tons of resources out there on things like this so please go spend an hour gathering new ideas and creating amazing programs! Feel free to take this and transform it into something even better. That’s why FRC is so great, the sharing of ideas and community is absolutely amazing. Good luck in the 2020 season everyone, I hope to see you there!