I've been working on a line-following robot using computer vision for a while now and have finished the code that plots a quadratic in the centre of a line like so:
I want to use the values of a and b from the expanded form of the parabola (ax^2 + bx + c) and how far the mid point of the line is away from the center of the screen. However, I've never used PID before and could use some help in implementing it or even if I should. I want the PID controller to control the turn of the robot; how much it has to rotate to keep the quadratic as straight as possible.
The code works by converting the image to grayscale, blurring and thresholding it, then skeletonizing it to get the center pixels of the line. I then perform a variety of methods to eliminate some noise from the line. Once I have all the averaged pixels I fit a curve over it and plot that on the image.
Here is the code:
import numpy as np
import cv2
from skimage.morphology import medial_axis
import math
from scipy.optimize import curve_fit
cap = cv2.VideoCapture(2)
w = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
def drawLines(Line, outlier_threshold):
# grabs height, width and color of an image
# filters the image to make sure there are only pixels on the screen that are close to each other (like a line)
filtered_pixel_coords = []
prev_point = None
for pixel in Line:
x, y = pixel[0]
if prev_point is not None:
prev_x, prev_y = prev_point
distance = math.sqrt((x - prev_x) ** 2 + (y - prev_y) ** 2)
if distance <= outlier_threshold:
# forgets about any pixels on the edge cause sometimes there are a lot of them and that is bad
if 10 < x < w - 10 and 10 < y < h - 10:
filtered_pixel_coords.append([x, y])
else:
filtered_pixel_coords.append([x, y])
prev_point = [x, y]
averaged_coords = []
avg = [0, 0]
count = 0
# averages out every 50 pixels for smoother line
for i, coord in enumerate(filtered_pixel_coords):
avg[0] += coord[0]
avg[1] += coord[1]
count += 1
if count == 50 or i == len(filtered_pixel_coords) - 1:
avg[0] //= count
avg[1] //= count
averaged_coords.append(avg.copy())
avg = [0, 0]
count = 0
return averaged_coords
def objective(x, a, b, c):
# quadratic for PID values
return a * x + b * x**2 + c
while True:
ret, img = cap.read()
if not ret:
break
# converts image to grayscale (black and white), blurs the image and only grabs white pixels (no asking how)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
blurred = cv2.medianBlur(gray, 9)
_, thresholded = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
# creates contours (boxes) around the white pixels
contours,_= cv2.findContours(thresholded,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)
for contour in contours:
# contour = cv2.approxPolyDP(contour, 0.005 * cv2.arcLength(contour, True), True)
cv2.drawContours(img, [contour], -1, (0, 255, 0), -1)
# get everything that is zero on the white pixel only image (grabs all pixels of the line)
inverted_thresholded = cv2.bitwise_not(thresholded)
# gets the middle of the line by eroding it before it is only a pixel big. !!FORGOT WHAT DISTANCE DOES HERE!!
skeleton, dist = medial_axis(inverted_thresholded, return_distance=True)
dist_on_skel = dist * skeleton
skeleton = (255 * dist_on_skel).astype(np.uint8)
# grabs all pixels that werent eroded
Line = cv2.findNonZero(skeleton)
averaged = drawLines(Line, 30)
x_coords = [coord[0] for coord in averaged]
y_coords = [coord[1] for coord in averaged]
try:
popt, _ = curve_fit(objective, y_coords, x_coords)
# summarize the parameter values
a, b, c = popt
y_line = np.arange(min(y_coords), max(y_coords), 1)
# calculate the output for the range
x_line = objective(y_line, a, b, c)
# plots the line to show on the image
for i in range(len(y_line) - 1):
pt1 = (int(x_line[i]), int(y_line[i]))
pt2 = (int(x_line[i + 1]), int(y_line[i + 1]))
cv2.line(img, pt1, pt2, (0, 0, 255), 2)
except:
continue
cv2.imshow('Image', img)
cv2.imshow('Skeletonized', skeleton)
if cv2.waitKey(100) == 27:
break
cap.release()
cv2.destroyAllWindows()
Thanks for the input in advance.
