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

效果