opencv python 巡线 最小二乘法
findline.py
函数返回theta为拟合直线与y轴负半轴夹角角度，单位为度，左偏为负，右偏为正;intercept_x_line为拟合直线和x轴交点的横坐标与x=320的偏离值，左偏为负，右偏为正。
上面的坐标系为opencv像素坐标系。
import cv2
import numpy as np
import time
cap = cv2.VideoCapture(0) # 若为USB摄像头改为1
def nothing(x):
pass
WindowName = 'Approx'
cv2.namedWindow(WindowName, cv2.WINDOW_AUTOSIZE)
cv2.createTrackbar('threshold', WindowName, 0, 255, nothing)
def pixel_threshold(frame_line, threshold_min, group_y, min_pixel, flag_drawline, flag_color):
ret_val, img_bin_line = cv2.threshold(frame_line, threshold_min, 255, cv2.THRESH_BINARY)
# 双阈值
# img_bin_line = cv2.erode(img_bin_line, None, iterations=4)
# img_bin_line = cv2.dilate(img_bin_line, None, iterations=4)
# ret1, img_bin1 = cv2.threshold(frame_line, 148, 255, cv2.THRESH_BINARY_INV)
# img_bin_line = cv2.bitwise_and(img_bin1, img_bin_line)
coordinate = np.zeros((6, group_y), dtype=np.float)
count_coordinate = 0
if flag_color == 0:
gray_value = 0
else:
gray_value = 255
# 以group_y为步进，取每一行白色像素的中心坐标
for i in range(0, 480, int(480 / group_y)):
color = img_bin_line[i]
# 下面两个255改为0可巡黑线
white_count = np.sum(color == gray_value)
white_index = np.where(color == gray_value)
if white_count >= min_pixel:
center = (white_index[0][white_count - 1] + white_index[0][0]) / 2
white_width = white_index[0][white_count - 1] - white_index[0][0]
coordinate[2][count_coordinate] = center
coordinate[3][count_coordinate] = i
coordinate[5][count_coordinate] = white_width
count_coordinate += 1
# 3σ准则去除异常值，主要是去除零散的反光，反光严重这种方法不可行
for j in range(2):
std1 = np.std(coordinate[5][0:count_coordinate], ddof=1)
mean1 = np.mean(coordinate[5][0:count_coordinate])
left = mean1 - std1
right = mean1 + std1
count_c = 0
# print("left:", left, "right:", right)
for i in range(0, count_coordinate, 1):
if left <= coordinate[5][i] <= right:
coordinate[0][count_c] = coordinate[2][i]
coordinate[1][count_c] = coordinate[3][i]
coordinate[4][count_c] = coordinate[5][i]
count_c += 1
coordinate[2] = coordinate[0]
coordinate[3] = coordinate[1]
coordinate[5] = coordinate[4]
count_coordinate = count_c
coordinate[0] = coordinate[2]
coordinate[1] = coordinate[3]
# 最小二乘法拟合直线
if count_coordinate >= 1:
ave_x = np.sum(coordinate[0][0:count_coordinate]) / count_coordinate
ave_y = np.sum(coordinate[1][0:count_coordinate]) / count_coordinate
over = np.sum(coordinate[0][0:count_coordinate] * coordinate[1][0:count_coordinate]) - \
count_coordinate * ave_x * ave_y
square = np.sum(coordinate[0][0:count_coordinate] * coordinate[0][0:count_coordinate]) - \
count_coordinate * ave_x * ave_x
if square != 0:
slope = over / square
if slope != 0:
intercept_x_line = ave_x - ave_y / slope
else:
slope = 1.79e308
intercept_x_line = 320.0
else:
slope = 1.79e308
intercept_x_line = 320.0
if square != 0:
theta_line = np.arctan(slope) / np.pi * 180
else:
theta_line = -90
if theta_line > 0:
theta_line = theta_line - 90
else:
theta_line = 90 + theta_line
else:
slope = 1.79e308
theta_line = 0.0
intercept_x_line = 320.0
# 画辅助线
if flag_drawline:
state_cor = 0
draw_x0 = 0
draw_y0 = 0
draw_x1 = 0
draw_y1 = 0
step = 10
for i in range(count_coordinate):
cv2.circle(frame_line, (int(coordinate[0][i]), int(coordinate[1][i])), 2, 0, -1)
cv2.line(img_bin_line, (0, int(coordinate[1][i])), (640, int(coordinate[1][i])), 255, 1)
cv2.line(frame_line, (0, int(coordinate[1][i])), (640, int(coordinate[1][i])), 255, 1)
for i in range(0, 481, step):
if theta_line != 0:
draw_cor_x = i / slope + intercept_x_line
else:
draw_x0 = draw_x1 = 320
draw_y0 = 0
draw_y1 = 480
break
if state_cor == 0:
if 0 <= draw_cor_x <= 640:
draw_x0 = draw_cor_x
draw_y0 = i
state_cor = 1
elif state_cor == 1:
if draw_cor_x < 0 or draw_cor_x > 640 or i == 480:
draw_x1 = (i - step) / slope + intercept_x_line
draw_y1 = i - step
state_cor = 0
else:
state_cor = 0
cv2.line(frame_line, (int(draw_x0), int(draw_y0)), (int(draw_x1), int(draw_y1)), (0, 0, 0), 2)
img_level = np.hstack((img_bin_line, frame_line))
cv2.namedWindow("img_level_line", cv2.WINDOW_GUI_NORMAL)
cv2.imshow("img_level_line", img_level)
intercept_x_line = (intercept_x_line - 320)
if intercept_x_line >= 180.0:
intercept_x_line = 180.0
elif intercept_x_line <= -180.0:
intercept_x_line = -180.0
return theta_line, intercept_x_line
if __name__ == "__main__":
t_num = 0.0
count = 0.0
while True:
t_start = time.time()
ret, frame = cap.read()
frame = cv2.GaussianBlur(frame, (3, 3), 0)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# frame = cv2.imread('./00.jpg')
threshold_i = cv2.getTrackbarPos('threshold', WindowName)
# 图片大小为640 * 480，大部分摄像头都可
# frame_line为灰度图，threshold_min为阈值，大于白色，小于黑色，group_y为分组，越大取得行数越多越细致，
# min_pixel为最小宽度，超过这个宽度的白线中心点才会参与线性拟合，flag_drawline = 1画辅助线，否则不画
# flag_color = 1巡白线，否则黑线
theta, intercept_x = pixel_threshold(frame_line=gray,
threshold_min=threshold_i,
group_y=48,
min_pixel=3,
flag_drawline=1,
flag_color=1
)
# print(theta, intercept_x, flag_turn)
# 按q退出
if cv2.waitKey(1) & 0xFF == ord('q'):
cap.release()
cv2.destroyAllWindows()
break
t_num += time.time() - t_start
count += 1
if t_num >= 1:
# 每隔一秒打印帧率
print('ZL:', count)
t_num = 0
count = 0
效果
