Below is my current code where I’ve attempted to incorporate distance tracking in a colour detection for the colour blue in a live stream directly from the picamera and tracking it.
My University project is about designing small bots that have to locate an object in an area and retrieve it back to base. Our approach is to colour code the desired object for detection where it can track it as it moves and then be able to calculate distance to the colour as it gets closer using the picamera itself. When the code is run, the colour blue can be detected and tracked fine, and a distance measurement is output at the bottom corner of the frame. However, this measurement randomly fluctuates a lot and doesn't seem to be measuring to the blue marker as it doesn't respond appropriately when I move the object closer/further away from the camera.
So the issue must be that the colour marker hasn't been referenced properly in the script so that it knows to measure to that once it has used the still image "cam1.jpg" to calibrate itself (calculate focal length from) and I'm not sure on how to do this correctly. I do know that both markers (the image and the colour) have to be referenced in the same def find_marker() function but not entirely sure how. Moreover, focalLength's value depends on marker[1][0] which gets updated in the loop but focalLength doesn't which I think is where the main issue lies. I tried adding the line where I define focalLength into the loop but when When I run the script, a constant value of 0.42ft is output to the bottom of the frame meaning it's not getting updated properly and I don't know how to go about doing this due to my limited python knowledge. Below is my script where I've attempted this.
How do I reference FocalLength in the script so that its value gets updated properly in the loop?
I've been stuck on this for the past few days now and I'm still new to Python so any help/guidance would be greatly appreciated! This is as far as I've managed to get with the help of pyimagesearch's articles and online forums trying to adapt and merge pyimagesearche's blogposts for colour detection and measuring distance for my Raspberry Pi.
SPECS: Raspberry PI Zero W, python 2.7, OpenCV 3.2 on Mac via VNC – Here’s my code:
My code:
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import cv2
import numpy as np
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 50
camera.hflip = True
rawCapture = PiRGBArray(camera, size=(640, 480))
time.sleep(0.1)
camera.capture(rawCapture, format='bgr')
image = rawCapture.array
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
edged = cv2.Canny(gray, 35, 125)
def find_marker(image):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
edged = cv2.Canny(gray, 35, 125)
lower = np.array([76,31,4],dtype="uint8")
upper = np.array([210,90,70], dtype="uint8")
rawCapture.truncate(0)
(_, cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
c = max(cnts, key = cv2.contourArea)
return cv2.minAreaRect(c)
def distance_to_camera(knownWidth, focalLength, perWidth):
return (knownWidth * focalLength) / perWidth
KNOWN_DISTANCE = 5.0
KNOWN_WIDTH = 2.0
IMAGE_PATHS = ['cam1.jpg']
image = cv2.imread(IMAGE_PATHS[0])
marker = find_marker(image)
focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
image = frame.array
blur = cv2.blur(image, (3,3))
marker = find_marker(image)
lower = np.array([76,31,4],dtype="uint8")
upper = np.array([210,90,70], dtype="uint8")
thresh = cv2.inRange(blur, lower, upper)
thresh2 = thresh.copy()
image, contours,hierarchy = cv2.findContours(thresh,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
max_area = 0
best_cnt = 1
for cnt in contours:
image = frame.array
area = cv2.contourArea(cnt)
if area > max_area:
max_area = area
best_cnt = cnt
M = cv2.moments(best_cnt)
cx,cy = int(M['m10']/M['m00']), int(M['m01']/M['m00'])
cv2.circle(blur,(cx,cy),10,(0,0,255),-1)
cv2.putText(blur, "%.2fft" % (inches / 12),
(image.shape[1] - 200, image.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
2.0, (0, 255, 0), 3)
cv2.imshow("Frame", blur)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
if key == ord("q"):
break
Thanks in advance!