NJUPT第二次积分赛小结与视觉部分开源

1|0NJUPT第二次积分赛小结与视觉部分开源

跟队友连肝一周多积分赛，写了一堆屎山，总算是今天完赛了。结果也还行，80分到手。其实题目是全做完了的，但验收时我nt了没操作好导致丢了不少分，而且整个控制流程也都基于一堆bug和屎山做的，所以其实能做成这样我也很满意了，下次积分赛再战，只要国赛不出bug就行。

先放题目吧，如图：

基础部分：

基础1：小车从1号区域出发，巡线到2号区域（15分）

基础2：在基础1的基础上，小车在2号区域成功转直角弯，并走到3号区域。（15分）

基础3：在基础2的基础上，小车从3号区域成功走到4号位置。（10分）

基础4：在基础3的基础上，小车停车成功，在框内，不压到边界线。（10分）

提高部分：

小车以正方形和圆心区分左右移动，其中正方形指示小车向左移动，圆形指示小车向右移动。在移动时，如果移动侧存在道路，为变道，不存在道路为避障。其中避障从指定方向（识别的指示方向）绕过障碍，然后立即再次回到之前的道路上。即存在道路即继续按照道路行驶，如果不存在则超过障碍需要切回道路。距离越短越好。

例如，小车从3区域走到A障碍物处，发现为方形障碍物，需要往左移动，所以变道到虚线道路上，继续沿着虚线行驶到B位置，看到B为圆形障碍物，则小车往右侧移动，变道到实线道路上，继续沿着实线前进，看到C处为方形障碍物，从障碍物右侧避障再立刻回到实线道路。继续往前走，由于道路上不经过D处障碍物，所以不做处理。（D为隔壁道路的指示牌，与当前道路无关，视为干扰项，随机在小车此刻相邻道路的某一位置）。

注：所有障碍物颜色均为红色，一共有且仅有三个需要做出反应的障碍物。

发挥1：小车每成功路过一个障碍物，并执行相应动作获得10分，共30分。（路过干扰障碍物不额外加分）

发挥2：小车在每次变道/避障时，需要提前进行声光提示，变完道需要关闭声光提示。注：声光提示必须体现处左转和右转区别。（5分）

发挥3：小车成功走到4区域并完成停车。（10分）

发挥部分：

自由发挥出额外特色功能。（5分）

注：禁止使用麦克纳姆轮小车。

当初看到题目的时候，没觉得有多难，但真正入手时，是真的感觉到了题目的恶意。。。

我们队用的是树莓派4B 4GB + STM32401CCU6，一个USB摄像头，一个mpu6050（屁用没有），一个oled，一个TB6612，还有车板（拿平衡车板子改的，机械结构很逆天。）

先说出我们队做题时的几个困难点：

1、线好细，2mm线径，灰度巡线的队伍直接寄。还好我和我做控制的队友都没用过灰度，所以整套传感器基本是纯摄像头（mpu6050只能算是摆设，几套控制全程开环）

2、场地数字干扰。数字和线离得很近，干扰蛮大的，图像部分方案选取不好的话容易寄。

3、场地地形干扰。从1区域到2区域时车子左边噪声干扰较大，因为紧挨桌子，同时圆弧顶部离墙特别近，我们队的车又比较宽，如果不做一些处理的话巡线会直接撞墙，同时墙与地板之间的夹线对图像部分也有一定干扰。

4、障碍物大小不同。障碍物做的太过随便，都是拿胶把红色卡纸贴快递盒子上，高度大小都不太一样，甚至还有卷边。两个圆方识别我也是调了不少时间。

5、控制方案是真不好做。四个动作，两个变道，两个避障。在做变道的时候试了不少方案，一开始是准备mpu转90度走一段然后再转90度就开始寻线，但是后来由于车子形态，pid控制，mpu零漂，代码屎山堆积等多方面因素影响，我们最终选择直接开环。。。然后程序直接少了上百行，可读性大大增强。至于变道怎么个开环法，那还得是车子强大的巡线功能，从一条线上转个角度就能直接巡到另一条线上😋然后就是避障，还是靠的巡线，转个角度走个圆弧就切进线了😋

6、通信。我们用的串口，因为最常用。至于其他乱七八糟的通信协议用的都不怎么熟练，所以直接选用串口，但由于第一次进行STM32与树莓派交互通信，bug频出，有一个bug找了整整一天，最后还是一个有经验的学长过来指导的。。。真的很感谢那位学长！红豆泥阿里嘎多！

废话不多说，直接上图像部分代码。这是我封装后的函数，所以要用的话直接复制就能用。

1|0bgr转hsv

def TrackBar_Init():
    cv2.namedWindow('hsv_binary')
    cv2.createTrackbar('hmin', 'hsv_binary', 0, 179, call_back)
    cv2.createTrackbar('hmax', 'hsv_binary', 179, 179, call_back)
    cv2.createTrackbar('smin', 'hsv_binary', 0, 255, call_back)
    cv2.createTrackbar('smax', 'hsv_binary', 255, 255, call_back)
    cv2.createTrackbar('vmin', 'hsv_binary', 130, 255, call_back)
    cv2.createTrackbar('vmax', 'hsv_binary', 255, 255, call_back)

def getHSV(image):
    # 1 get trackbar's value
    hmin = cv2.getTrackbarPos('hmin', 'hsv_binary')
    hmax = cv2.getTrackbarPos('hmax', 'hsv_binary')
    smin = cv2.getTrackbarPos('smin', 'hsv_binary')
    smax = cv2.getTrackbarPos('smax', 'hsv_binary')
    vmin = cv2.getTrackbarPos('vmin', 'hsv_binary')
    vmax = cv2.getTrackbarPos('vmax', 'hsv_binary')
    cv2.imshow("hsv_binary", win)
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    h, s, v = cv2.split(hsv)
    h_binary = cv2.inRange(np.array(h), np.array(hmin), np.array(hmax))
    s_binary = cv2.inRange(np.array(s), np.array(smin), np.array(smax))
    v_binary = cv2.inRange(np.array(v), np.array(vmin), np.array(vmax))
    binary = 255 - cv2.bitwise_and(h_binary, cv2.bitwise_and(s_binary, v_binary))
    return binary

1|0逆时针90度旋转摄像头读取的图像：

def rotateAntiClock_90(img):
    img = cv2.flip(cv2.transpose(img), 0)
    return img

1|0图像处理

def Image_Processing():
    global frame, binary, dst, blur
    ret, frame = camera.read()
    frame = rotateAntiClock_90(frame)  # 逆时针旋转90度
    binary = getHSV(frame)
    blur = cv2.GaussianBlur(binary, (9, 9), 0)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
    dst = cv2.morphologyEx(blur, cv2.MORPH_CLOSE, kernel)
    cv2.imshow('Dilate', dst)  # dst：处理后的图像

1|0巡线（实线）

def Find_RealLine(src):  # 从左扫线
    dst = src
    direc1_x = 0
    direc1_y = 0
    diffCRL = 0
    filtRate = 1 / 20
    dirLine = int(dst.shape[0]*9/13)  # 扫线
    dot_x = []
    # Canny边缘检测
    dst_canny = cv2.Canny(dst, 190, 250)
    h, w = dst_canny.shape[0:2]
    dst_canny = dst_canny[:, 0 + int(filtRate*w):int(w - w*filtRate)]  # 提取ROI
    # cv2.imshow('canny', dst_canny)
    # 找基准点，利用与运算掩模
    dst_mask_line = np.zeros(dst_canny.shape, np.uint8)
    dst_mask_line[dirLine, :] = 255  # 图像尺寸：640x480
    dotImg = cv2.bitwise_and(dst_mask_line, dst_canny)  # 使用按位与找交点
    # 从左往右找第一第二个交点，取中点即为基准点
    for i in range(0, dotImg.shape[1]):
        if dotImg[dirLine, i] == 255:
            dot_x.append(i)
    try:  # 防止没有找到交点导致列表内无数据的情况
        x1 = dot_x[0]
        x2 = dot_x[1]
        # 基准点坐标
        direc1_x = int((x1 + x2) / 2)
        direc1_y = dirLine
        print((direc1_x, direc1_y))
        # 计算偏差值
        diffCRL = direc1_x - int(1/2*dst_canny.shape[1])
        print(diffCRL)
    except:
        pass
    return diffCRL

1|0巡线（虚线）

def Find_DottedLine(img):  # 巡虚线：利用图像矩（好用！）
    # img = cv2.Canny(img, 50, 150)
    area = 0
    difDotLine = 0
    h, w = img.shape[0:2]
    img = img[int(3/5*h):h, :]  # 提取ROI
    m = cv2.moments(img)
    try:
        x = int(m['m10'] / m['m00'])
        y = int(m['m01'] / m['m00'])
        area = int(m['m00'])
        difDotLine = int(x - 1 / 2 * img.shape[1])
        # cv2.circle(frame, (x, y + int(2/3*h)), 7, (0, 255, 0), 4)
        cv2.imshow('dstline', img)
    except:
        call_back()
    return area, difDotLine

1|0检测近似水平的线

def findHorizonLine(src):  # 检测水平直线
    flag = 0
    src = src[int(1/2 * src.shape[0]):src.shape[0], :]  # 提取ROI
    src_canny = cv2.Canny(src, 50, 150)
    lines = cv2.HoughLinesP(src_canny, 1, np.pi / 180, 70, minLineLength=40, maxLineGap=10)
    try:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1 + 1e-6)
            if abs(slope) > 0.3:  # 滤掉斜率大于0.3的线
                continue
            else:
                print("Found Horizon Line")
                flag = 1
            # cv2.line(frame, (x1, y1+int(1/2 * src.shape[0])), (x2, y2+int(1/2 * src.shape[0])), (0, 0, 255), 2)
    except:
        call_back()
    return flag

1|0检测近似垂直的线

def findVerticalLine(src):
    flag = 0
    src = src[int(src.shape[0]*2/3):src.shape[0], :]
    lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=150, maxLineGap=5)
    try:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (x2 - x1) / (y2 - y1 + 1e-6)
            if abs(slope) > 0.3:
                continue
            else:
                print("Found vertical Line")
                flag = 1
            # cv2.line(frame, (x1, y1 + int(src.shape[0]*2/3)), (x2, y2 + int(src.shape[0]*2/3)), (0, 0, 255), 2)
    except:
        call_back()
    return flag

1|0串口发送消息

def comPrint(_string):
    message = _string
    ser.write(message.encode())

1|0串口接收单字节消息（能接收，但有bug）

def getMessage():
    if ser.inWaiting():
        string = ser.read(1)  # 监听消息
    return string

1|0识别矩形

def getRect(img):
    # global frame
    shape = 0
    centerPoint = (0, 0)
    area = 0
    _, contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # openCV不同版本有区别
    for obj in contours:
        area = cv2.contourArea(obj)  # 计算轮廓内区域的面积
        perimeter = cv2.arcLength(obj, True)  # 计算轮廓周长
        approx = cv2.approxPolyDP(obj, 0.02 * perimeter, True)  # 获取轮廓角点坐标
        CornerNum = len(approx)  # 轮廓角点的数量
        x, y, w, h = cv2.boundingRect(approx)  # 获取坐标值和宽度、高度
        centerPoint = (int(x+(1/2)*w), int(y+(1/2)*w))
        if CornerNum == 4:
            shape = 4
            # cv2.circle(frame, centerPoint, 3, (0,0,255), -1)
    return shape, centerPoint, area

1|0识别并提取圆形

def getCircle(src):  # src:处理过的图像
    signalCircle = 0
    circle = []
    edges = cv2.Canny(src, 128, 255)
    cv2.imshow('canny', edges)
    # 检测图像形状
    # edges = edges[int(edges.shape[0] * 1 / 10):edges.shape[0], :]
    circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 300,
                               param1=13, param2=26, minRadius=50,
                               maxRadius=200)
    # 确保至少发现一个圆
    if circles is not None:
        # 进行取整操作
        print("Circle here")
        edges = np.zeros(edges.shape, np.uint8)
        circles = np.round(circles[0, :]).astype("int")
        signalCircle = 1
        # 循环遍历所有的坐标和半径
        # 绘制结果
        for (x, y, r) in circles:
            cv2.circle(frame, (x, y), r, (255, 0, 0), 4)
            cv2.rectangle(frame, (x - 5, y - 5), (x + 5, y + 5), (255, 128, 0), -1)
            circle.append((x,y,r))
    return signalCircle, circle

1|0提取红色部分

def getRedImg(img):
    grid_HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    # H、S、V范围一：
    lower1 = np.array([0, 43, 46])
    upper1 = np.array([10, 255, 255])
    mask1 = cv2.inRange(grid_HSV, lower1, upper1)  # mask1 为二值图像
    # H、S、V范围二：
    lower2 = np.array([156, 43, 46])
    upper2 = np.array([180, 255, 255])
    mask2 = cv2.inRange(grid_HSV, lower2, upper2)
    # 将两个二值图像结果 相加
    mask3 = mask1 + mask2
    # 去除噪声
    kernel = np.ones((5, 5), np.uint8)
    dst3 = cv2.morphologyEx(mask3, cv2.MORPH_OPEN, kernel)
    dst3 = cv2.medianBlur(dst3, 29)
    return dst3

1|0全套工程代码（一堆屎山）

import numpy as np
import cv2
import serial
import time
from numpy import *
import RPi.GPIO as GPIO


ser = serial.Serial("/dev/ttyAMA0", 9600)
ser.timeout = 5
if not ser.isOpen:
    ser.open()  # 打开串口

width, height = 160, 120
camera = cv2.VideoCapture(0)
camera.set(100, width)
camera.set(80, height)
camera.set(cv2.CAP_PROP_FPS, 10)
win = np.zeros((300, 512, 3), np.uint8)
dst = np.zeros((width, height), np.uint8)
blur = np.zeros((width, height), np.uint8)
direc1_x = 0
direc1_y = 0
diffCDL = 0
diffCRL = 0
modeFlag = 0
horizonFlag = 0
verticalFlag = 0
followReaLineFlag = 1
followDottedLineFlag = 1
findDotHorLineFlag = 0
rectFlag = 0
circleFlag = 0
whichLine = 1  # 1:实线，2：虚线，0：中间位置
stopFlag = 0
openFlag = 0
ticks = 0
tickFlag = 0
printFlag = 0
turnFlag = 0
newModeFlag = 0


def call_back(*arg):
    pass


def TrackBar_Init():
    # 1 create windows
    cv2.namedWindow('hsl_binary')
    # 2 Create Trackbar
    cv2.createTrackbar('hmin', 'hsl_binary', 0, 179, call_back)
    cv2.createTrackbar('hmax', 'hsl_binary', 179, 179, call_back)
    cv2.createTrackbar('smin', 'hsl_binary', 0, 255, call_back)
    cv2.createTrackbar('smax', 'hsl_binary', 255, 255, call_back)
    cv2.createTrackbar('vmin', 'hsl_binary', 130, 255, call_back)
    cv2.createTrackbar('vmax', 'hsl_binary', 255, 255, call_back)


def Init():
    TrackBar_Init()
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(5, GPIO.IN)
    GPIO.setup(5, GPIO.IN, pull_up_down=GPIO.PUD_UP)  # 可以调整上下拉状态


def rotateAntiClock_90(img):
    img = cv2.flip(cv2.transpose(img), 0)
    return img


# 在HSV色彩空间下得到二值图
def Get_HSV(image):
    # 1 get trackbar's value
    hmin = cv2.getTrackbarPos('hmin', 'hsl_binary')
    hmax = cv2.getTrackbarPos('hmax', 'hsl_binary')
    smin = cv2.getTrackbarPos('smin', 'hsl_binary')
    smax = cv2.getTrackbarPos('smax', 'hsl_binary')
    vmin = cv2.getTrackbarPos('vmin', 'hsl_binary')  # 128
    vmax = cv2.getTrackbarPos('vmax', 'hsl_binary')
    cv2.imshow("hsl_binary", win)
    # 2 to HSV
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    # cv2.imshow('hsv', hsv)
    h, s, v = cv2.split(hsv)

    # 3 set threshold (binary image)
    # if value in (min, max):white; otherwise:black
    h_binary = cv2.inRange(np.array(h), np.array(hmin), np.array(hmax))
    s_binary = cv2.inRange(np.array(s), np.array(smin), np.array(smax))
    v_binary = cv2.inRange(np.array(v), np.array(vmin), np.array(vmax))

    # 4 get binary（对H、S、V三个通道分别与操作）
    binary = 255 - cv2.bitwise_and(h_binary, cv2.bitwise_and(s_binary, v_binary))

    # 5 Show
    # cv2.imshow('binary', binary)

    return binary


# 图像处理
def Image_Processing():
    global frame, binary, dst, blur
    ret, frame = camera.read()
    frame = rotateAntiClock_90(frame)  # 逆时针旋转90度
    binary = Get_HSV(frame)
    blur = cv2.GaussianBlur(binary, (9, 9), 0)
    # cv2.imshow('blur', blur)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
    dst = cv2.morphologyEx(blur, cv2.MORPH_CLOSE, kernel)
    cv2.imshow('Dilate', dst)  # dst：处理后的图像


def Find_RealLine():  # 从左扫线
    global dst, direc1_x, direc1_y, diffCRL, frame
    dirLine = int(dst.shape[0]*9/13)
    dot_x = []
    # Canny边缘检测
    dst_canny = cv2.Canny(dst, 190, 250)  
    h, w = dst_canny.shape[0:2]
    filtRate = 1/20
    dst_canny = dst_canny[:, 0 + int(filtRate*w):int(w - w*filtRate)]
    # cv2.imshow('canny', dst_canny)
    # 找基准点，利用与运算掩模
    dst_mask_line = np.zeros(dst_canny.shape, np.uint8)
    dst_mask_line[dirLine, :] = 255  # 图像尺寸：640x480
    dotImg = cv2.bitwise_and(dst_mask_line, dst_canny)  # 使用按位与找交点

    # 从左往右找第一第二个交点，取中点即为基准点
    for i in range(0, dotImg.shape[1]):
        if dotImg[dirLine, i] == 255:
            dot_x.append(i)
    try:  # 防止没有找到交点导致列表内无数据的情况
        x1 = dot_x[0]
        x2 = dot_x[1]
        # 基准点坐标
        direc1_x = int((x1 + x2) / 2)
        direc1_y = dirLine

        # 画出基准点
        frame = cv2.circle(frame, (direc1_x + int(filtRate*w), direc1_y), 5, (0, 0, 255), 2)
        dst_canny = cv2.line(dst_canny, (dirLine, 0), (dirLine, dst_canny.shape[0]), (255, 255, 255), 2)
        dst_canny = cv2.cvtColor(dst_canny, cv2.COLOR_GRAY2BGR)
        # cv2.imshow('dst_canny', dst_canny)
        print((direc1_x, direc1_y))

        # 计算偏差值
        diffCRL = direc1_x - int(1/2*dst_canny.shape[1])
        print(diffCRL)
    except:
        pass


def findHorizonLine(src):  # 检测水平直线
    global modeFlag, horizonFlag, frame
    flag = 0
    src = src[int(1/2 * src.shape[0]):src.shape[0], :]
    dst_y = cv2.Canny(src, 50, 150)
    # cv2.imshow('dst_y', dst_y)
    lines = cv2.HoughLinesP(dst_y, 1, np.pi / 180, 70, minLineLength=40, maxLineGap=10)
    # dsty_bgr = cv2.cvtColor(dst_y, cv2.COLOR_GRAY2BGR)
    try:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1 + 1e-6)
            if abs(slope) > 0.3:
                continue
            else:
                print("Found Line")
                flag = 1
                horizonFlag = 1
            cv2.line(frame, (x1, y1+int(1/2 * src.shape[0])), (x2, y2+int(1/2 * src.shape[0])), (0, 0, 255), 2)
    except:
        call_back()
    # cv2.imshow('dsty_line', dsty_bgr)
    return flag


def findVerticalLine(src):
    global modeFlag, frame
    flag = 0
    src = src[int(src.shape[0]*2/3):src.shape[0], :]
    lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=150, maxLineGap=5)
    try:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (x2 - x1) / (y2 - y1 + 1e-6)
            if abs(slope) > 0.45:
                continue
            else:
                print("Found vertical Line")
                flag = 1
            cv2.line(frame, (x1, y1 + int(src.shape[0]*2/3)), (x2, y2 + int(src.shape[0]*2/3)), (0, 0, 255), 2)
    except:
        call_back()
    return flag


def findChangeLine(src):
    global modeFlag, horizonFlag, frame
    # src = cv2.Canny(src, 50, 150)
    flag = 0
    src = src[int(1 / 3 * src.shape[0]):src.shape[0], 0:int(src.shape[1] * 5/6)]
    # cv2.imshow('HorDotLine', src)
    lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=40, maxLineGap=120)
    try:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1 + 1e-6)
            if 1/100 <= abs(slope) <= 2:
                print("Found Change Line")
                flag = 1
                # horizonFlag = 1
            else:
                continue
            cv2.line(frame, (x1, y1+int(1 / 2 * src.shape[0])), (x2, y2+int(1 / 2 * src.shape[0])), (0, 0, 255), 2)
    except:
        call_back()
    return flag


def findChangeRLine(src):
    global modeFlag
    # src = cv2.Canny(src, 50, 150)
    flag = 0
    src = src[int(1 / 4 * src.shape[0]):src.shape[0], :]
    # cv2.imshow('HorDotLine', src)
    lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=50, maxLineGap=20)
    try:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1 + 1e-6)
            if 1/100 <= abs(slope) <= 2:
                print("Found Change Line")
                flag = 1
                # horizonFlag = 1
            else:
                continue
            # cv2.line(frame, (x1, y1+int(1 / 2 * src.shape[0])), (x2, y2+int(1 / 2 * src.shape[0])), (0, 0, 255), 2)
    except:
        call_back()
    return flag


def Find_DottedLine(img):  # 巡虚线：利用图像矩（好用！）
    global diffCRL, frame
    # img = cv2.Canny(img, 50, 150)
    area = 0
    difDotLine = 0
    h, w = img.shape[0:2]
    img = img[int(3/5*h):h, :]
    m = cv2.moments(img)
    try:
        x = int(m['m10'] / m['m00'])
        y = int(m['m01'] / m['m00'])
        area = int(m['m00'])
        difDotLine = int(x - 1 / 2 * img.shape[1])
        img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
        cv2.circle(frame, (x, y + int(2/3*h)), 7, (0, 255, 0), 4)
        cv2.imshow('dstline', img)
    except:
        call_back()
    cv2.imshow("DotLine", img)
    return area, difDotLine


# cv2.fitLine(points, distType, param, reps, aeps[, line ]) -> Line
def findDottedLine3(img):  # 巡虚线方案3：利用cv2.fitLine方法
    cnt = []  # 点集
    minVal = 1  # 区间下限
    maxVal = 255  # 区间上限
    # 由于拟合出的直线几乎垂直，故使用 x = m * y + n 型直线
    h, w = img.shape[0:2]
    img = img[int(2 / 3 * h):h, :]  # 提取ROI，滤去边缘噪声
    for i in range(img.shape[0]):
        for j in range(img.shape[1]):
            if minVal <= img[i, j] <= maxVal:
                cnt.append([j, i])
    cnt = np.array(cnt)             # cnt 必须为矩阵形式，且表示[x,y]坐标
    [vx, vy, x, y] = cv2.fitLine(cnt, cv2.DIST_L2, 0, 0.01, 0.01)
    # 其中：(vx,vy)为直线的方向向量，(x,y)为直线上的一个点
    img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)  # 用来画线的BGR图
    try:      # 防止全黑图等情况
        m = vx / vy
        n = x - m * y
        pt1 = (int(m * (y - 1000) + n), int(y - 1000))
        pt2 = (int(m * (y + 1000) + n), int(y + 1000))
        cv2.line(img_bgr, pt1, pt2, (0, 255, 0), 3)
        cv2.imshow('DottedLine', img_bgr)
    except:
        pass


def comPrint(_string):
    message = _string
    ser.write(message.encode())
    # ser.write(b"10%d" % diffCRL)


def getRect(img):
    global frame
    shape = 0
    centerPoint = (0, 0)
    area = 0
    imgContour = img.copy()
    _, contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    for obj in contours:
        area = cv2.contourArea(obj)  # 计算轮廓内区域的面积
        cv2.drawContours(imgContour, obj, -1, (255, 0, 0), 4)  # 绘制轮廓线
        perimeter = cv2.arcLength(obj, True)  # 计算轮廓周长
        approx = cv2.approxPolyDP(obj, 0.02 * perimeter, True)  # 获取轮廓角点坐标
        CornerNum = len(approx)  # 轮廓角点的数量
        x, y, w, h = cv2.boundingRect(approx)  # 获取坐标值和宽度、高度
        centerPoint = (int(x+(1/2)*w), int(y+(1/2)*w))
        if CornerNum == 4:
            shape = 4
            cv2.circle(frame, centerPoint, 3, (0,0,255), -1)
    return shape, centerPoint, area, imgContour


def findRed():
    global frame
    signalRect = 0        # 检测矩形标志位
    signalCirlcle = 0     # 检测圆形标志位
    img = frame
    # cv2.imshow('ord', img)
    grid_RGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
    # 从RGB色彩空间转换到HSV色彩空间
    grid_HSV = cv2.cvtColor(grid_RGB, cv2.COLOR_RGB2HSV)
    # H、S、V范围一：
    lower1 = np.array([0, 43, 46])
    upper1 = np.array([10, 255, 255])
    mask1 = cv2.inRange(grid_HSV, lower1, upper1)  # mask1 为二值图像
    # H、S、V范围二：
    lower2 = np.array([156, 43, 46])
    upper2 = np.array([180, 255, 255])
    mask2 = cv2.inRange(grid_HSV, lower2, upper2)
    # 将两个二值图像结果 相加
    mask3 = mask1 + mask2
    # 去除噪声
    kernel = np.ones((5, 5), np.uint8)
    dst3 = cv2.morphologyEx(mask3, cv2.MORPH_OPEN, kernel)
    dst3 = cv2.medianBlur(dst3, 29)
    # dst3 = dst3[:, int(dst3.shape[1]*1/10):dst3.shape[1]]
    # 结果显示
    # cv2.imshow("dst", dst3)
    edges = cv2.Canny(dst3, 128, 255)
    cv2.imshow('canny', edges)
    # 检测图像形状
    edges = edges[int(edges.shape[0] * 1/10):edges.shape[0], :]
    circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 300,
                               param1=13, param2=26, minRadius=50,
                               maxRadius=200)
    # dst3 = cv2.cvtColor(dst3, cv2.COLOR_GRAY2BGR)
    # 确保至少发现一个圆
    if circles is not None:
        # 进行取整操作
        print("Circle here")
        edges = np.zeros(edges.shape, np.uint8)
        circles = np.round(circles[0, :]).astype("int")
        signalCirlcle = 1
        # 循环遍历所有的坐标和半径
        for (x, y, r) in circles:
            # 绘制结果
            cv2.circle(frame, (x + int(dst3.shape[1]*1/5), y), r, (255, 0, 0), 4)
            # cv2.rectangle(frame, (x - 5, y - 5), (x + 5, y + 5), (255, 128, 0), -1)
    shape, centerPoint, area, _ = getRect(edges)
    #  检测矩形：
    if shape >= 2 and area >= 2500:
        print("There is a Rectangular")
        edges = np.zeros(edges.shape, np.uint8)
        cv2.circle(frame, centerPoint, 3, (255,0,0), 3)
        signalRect = 1
    # cv2.imshow('dst3', dst3)
    # edges = np.zeros(edges.shape(), np.uint8)
    del edges
    return signalRect, signalCirlcle


if __name__ == '__main__':
    Init()
    modeFlag = 2
    stopFlag = 2
    turnFlag = 0
    while True:
        Image_Processing()
        # Find_RealLine()
        # modeFlag = 10
        # if findHorizonDotLine(frame):
        #     print("findHorizon Line")
        # _, diffCDL = Find_DottedLine(dst)
        # if findChangeLine(dst) == 1:
        #     print("Chnage!")
        # findVerticalLine(dst)
        # comPrint("14\r\n")
        # if ser.read(1) == b'A':
        #     print("A")
        # comPrint("10%d\r\n" % diffCDL)
        # comPrint("10%d\r\n" % diffCRL)
        # findRed()
        # findHorizonLine(dst)
        # comPrint("13\r\n")
        # string = ser.read(1)
        # print(string)
        if modeFlag == 0:             # 出发走直线
            comPrint("17\r\n")        # 开环走直线模式
            if findHorizonLine(dst):  # 如果看到水平线
                modeFlag = 1          # 切换
                # 模式
        if modeFlag == 1:             # 右转90度模式
            turnFlag = 1
            horizonFlag = 0
            comPrint("12\r\n")        # 让车子转弯
            print("turn RIGHT, STM32!")
            # string = ser.read(1)      # 读取来自stm32的消息
            # print(string)
            # if string == b'A':        # 如果收到"A"
            #     modeFlag = 2          # 切换模式
            #     print(modeFlag)
            verticalFlag = findVerticalLine(dst)
            if tickFlag == 0:
                ticks = int(time.time())
                tickFlag = 1
            if verticalFlag == 1 and int(time.time()) - ticks >= 5:
                print("Find Vertical Line")
                followReaLineFlag = 1
                modeFlag = 2
                horizonFlag = 0
                tickFlag = 0
        if modeFlag == 2:             # 巡实线模式，同时检测障碍物与停车线
            whichLine = 1  # 实线
            tickFlag = 0
            openFlag = 1
            if followReaLineFlag == 1:
                Find_RealLine()
                # _,diffCDL = Find_DottedLine()
                comPrint("10%d\r\n" % diffCRL)  # 发给stm32偏移量
                print("10%d\n" % diffCRL)
                rectFlag, circleFlag = findRed()
                horizonFlag = findHorizonLine(dst)
                if horizonFlag == 1 and stopFlag >= 2:  # 经过多少障碍物停车
                    modeFlag = 8     # 实线停车模式
            if rectFlag == 1:
                followReaLineFlag = 0
                modeFlag = 3  # 向左变道
                # stopFlag = stopFlag + 1
            if circleFlag == 1:
                followReaLineFlag = 0
                modeFlag = 6  # 向右避障
                # stopFlag = stopFlag + 1
                # 给stm32发变道或避障指令
                # 如果读到来自stm32的动作完毕指令，就开始巡虚线或实线，并将形状flag置为0
            if turnFlag == 0:
                if findHorizonLine(dst):
                    horizonFlag = 1
                if horizonFlag == 1:
                    tickFlag = 0
                    modeFlag = 1
        if modeFlag == 7:  # 巡虚线模式
            whichLine = 2
            tickFlag = 0
            openFlag = 1
            if followDottedLineFlag == 1:
                print("Find Dot Line")
                _, diffCDL = Find_DottedLine(dst)
                comPrint("10%d\r\n" % diffCDL)  # 发给stm32偏移量
                rectFlag, circleFlag = findRed()
                horizonFlag = findHorizonLine(dst)
                if horizonFlag == 1 and stopFlag >= 2:
                    modeFlag = 9  # 虚线停车模式
            if rectFlag == 1:
                followDottedLineFlag = 0
                modeFlag = 5  # 向左避障
            if circleFlag == 1:
                followDottedLineFlag = 0
                modeFlag = 4  # 向右变道
            # 此处再加一个停车位的判断


        if modeFlag == 3:  # 向左变道模式
            whichLine = 0
            if tickFlag == 0:
                ticks = int(time.time())
                tickFlag = 1
                openFlag = 1
            if int(time.time()) - ticks >= 2 and openFlag == 1:
                stopFlag = stopFlag + 1
                openFlag = 0
            comPrint("13\r\n")  # 向左变道指令
            if ser.inWaiting():
                string = ser.read(1)  # 监听消息
                if string == b'A':  # 收到stm32的动作完毕指令
                    modeFlag = 7  # 直接进入巡线
                    print("Following Dot Line")
                    string = None
                    followDottedLineFlag= 1
                    rectFlag = 0
                    circleFlag = 0
                    # ser.flushInput()  # 清除输入缓冲区数据

        if modeFlag == 4:  # 向右变道模式
            whichLine = 0
            if tickFlag == 0:
                ticks = int(time.time())
                tickFlag = 1
                openFlag = 1
            if int(time.time()) - ticks >= 2 and openFlag == 1:
                stopFlag = stopFlag + 1
                openFlag = 0
            if printFlag == 0:
                comPrint("15\r\n")  # 向右变道指令
                printFlag = 1
            if ser.inWaiting():
                string = ser.read(1)  # 监听消息
                if string == b'B':  # 收到stm32的动作完毕指令
                    print("B")
                    modeFlag = 2
                    followReaLineFlag = 1
                    rectFlag = 0
                    circleFlag = 0
                    printFlag = 1
                    # ser.flushInput()  # 清除输入缓冲区数据
        if modeFlag == 5:  # 向左避障模式
            whichLine = 0
            if tickFlag == 0:
                ticks = int(time.time())
                tickFlag = 1
                openFlag = 1
            if int(time.time()) - ticks >= 2 and openFlag == 1:
                stopFlag = stopFlag + 1
                openFlag = 0
            comPrint("14\r\n")  # 向左避障指令
            if ser.inWaiting():
                string = ser.read(1)  # 监听消息
                if string == b'C':  # 收到stm32的动作完毕指令
                    print("left avoid")
                    # if findChangeLine(dst):
                    #     print("142\r\n")
                    #     comPrint("142\r\n")
                    time.sleep(2)
                    modeFlag = 7
                    followDottedLineFlag = 1
                    rectFlag = 0
                    circleFlag = 0
                    # ser.flushInput()  # 清除输入缓冲区数据
        if modeFlag == 6:  # 向右避障模式
            if tickFlag == 0:
                ticks = int(time.time())
                tickFlag = 1
                openFlag = 1
            if int(time.time()) - ticks >= 2 and openFlag == 1:
                stopFlag = stopFlag + 1
                openFlag = 0
            comPrint("16\r\n")  # 向右避障指令
            whichLine = 0
            if ser.inWaiting():
                string = ser.read(1)  # 监听消息
                if string == b'D':  # 收到stm32的动作完毕指令
                    string = None
                    print("right avoid")
                    if findChangeRLine(dst) == 1:
                        comPrint("163\r\n")
                        print("Turn right, STM32!")
                        modeFlag = 2
                        followReaLineFlag = 1
                        rectFlag = 0
                        circleFlag = 0
                        # ser.flushInput()  # 清除输入缓冲区数据
                    # serial.flushInput()  # 清除输入缓冲区数据

        if modeFlag == 8:       # 实线停车
            comPrint("11\r\n")
            print("STOP AT REAL LINE")
            time.sleep(10)
            if findHorizonLine(dst):
                comPrint("18\r\n")
                print("18")
                modeFlag = 10
        if modeFlag == 9:       # 虚线停车
            comPrint("19\r\n")
            print("STOP AT DOT LINE")
            time.sleep(15)
            # if findHorizonLine(dst):
            #     comPrint("19\r\n")
            #     print("18")
            #     modeFlag = 10
            modeFlag = 10
        cv2.imshow('frame', frame)
        if ser.inWaiting():
            string = ser.read(1)  # 监听消息
            if string == b'E':
                print("We can stop")
                stopFlag = 3
        print("stop flag : %d" % stopFlag)
        print("mode : %d" % modeFlag)
        if modeFlag == 10:
            newModeFlag = int(input("please change mode:"))
            if newModeFlag == 1:
                print("change mode")
                stopFlag = 0
                modeFlag = 2
        # getKeyVal()
        # if GPIO.input(3):
        #     print("HIGH VALUE")
        # GPIO.wait_for_edge(3, GPIO.RISING)
        # print("HIGH!")
        # if GPIO.input(5) == GPIO.HIGH:
        #     print("HIGH VALUE!")

        if cv2.waitKey(1) == ord('q'):
            cv2.destroyAllWindows()
            break

因为自己比较懒，有些函数没怎么测试，反正思路在那，看着改改就好。

__EOF__

本文作者：Asaka
本文链接：https://www.cnblogs.com/Asaka-QianXiang/p/17344045.html
关于博主：评论和私信会在第一时间回复。或者直接私信我。
版权声明：本博客所有文章除特别声明外，均采用 BY-NC-SA 许可协议。转载请注明出处！
声援博主：如果您觉得文章对您有帮助，可以点击文章右下角【推荐】一下。您的鼓励是博主的最大动力！

posted @ 2023-04-22 21:25 Akasa 阅读(111) 评论(0) 编辑收藏举报

刷新页面返回顶部

登录后才能查看或发表评论，立即登录或者逛逛博客园首页

相关博文：

· opencv-python笔记（1）

· NJUPT第一次积分赛

· Python OpenCV3 计算机视觉秘籍：6~9

· 智能车学习笔记/备忘录乱写 | 地平线篇

· opencv的学习笔记

阅读排行：
· 震惊！C++程序真的从main开始吗？99%的程序员都答错了
· 【硬核科普】Trae如何「偷看」你的代码？零基础破解AI编程运行原理
· 单元测试从入门到精通
· 上周热点回顾（3.3-3.9）
· winform 绘制太阳，地球，月球运作规律

公告

NJUPT第二次积分赛小结与视觉部分开源

发表于 2023-04-22 21:25阅读：111评论：0推荐：0

图像处理笔记

关注

跳至底部

昵称： Akasa
园龄： 2年2个月
粉丝： 23
关注： 3

+加关注

Asaka浅香

NJUPT第二次积分赛小结与视觉部分开源

1|0NJUPT第二次积分赛小结与视觉部分开源

1|0bgr转hsv

1|0逆时针90度旋转摄像头读取的图像：

1|0图像处理

1|0巡线（实线）

1|0巡线（虚线）

1|0检测近似水平的线

1|0检测近似垂直的线

1|0串口发送消息

1|0串口接收单字节消息（能接收，但有bug）

1|0识别矩形

1|0识别并提取圆形

1|0提取红色部分

1|0全套工程代码（一堆屎山）

公告

Asaka

NJUPT第二次积分赛小结与视觉部分开源

搜索

常用链接

随笔分类

随笔档案

阅读排行榜

评论排行榜

推荐排行榜

最新评论