航天物流线上赛交通灯识别部分

发布于 2021-04-22  40 次阅读


本质上是基于对于组委会提供的代码进行优化,由于对于所有都有改动,这里全部贴出,版本为openCV3

# -*- coding:utf-8 _*-
"""
@author:YuTao
@time: 2019/07/01
"""
import matplotlib
matplotlib.use('Agg')
import numpy as np
import cv2
import matplotlib.pyplot as plt
from _02CalculatePositon import *
from _03TrafficLight import *

np.set_printoptions(suppress=True, precision=4)
font = cv2.FONT_HERSHEY_SIMPLEX

# 载入参数,载入地图
Frame = 1      # 从第1500帧开始读取视频
Video = cv2.VideoCapture('1.mp4')
Video.set(1, Frame)


while 1:
    # 逐帧读取
    ret, Img = Video.read()
    if ret == True:
        Frame = Frame + 1
        # %% 由Marker计算小车的相对位置,同时得到marker图像区域
        CamPosition, MarkerROI, distance, Img = DealMarker(Img, Frame)  # CamPosition:(x,y,z)

        # %% 实现交通灯颜色检测
        LightColors = TrafficLight(MarkerROI, Img)  # LightColors:1-'Red', 2-'Yellow', 3-'Green'
        if distance != None:
            cv2.putText(Img, 'camPos'+str(CamPosition), (10,30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 255), 3) # 粉色
            # 显示距离和角度
            cv2.putText(Img, 'distance:' + str(round(distance, 4)) + str('m'), (0, 60), font, 1, (0, 255, 0), 2,cv2.LINE_AA)
        print('Frame:', Frame)
        print(CamPosition, LightColors)     # 输出小车位置和交通灯颜色
        Img = cv2.resize(Img, (int(Img.shape[1] / 2), int(Img.shape[0] / 2)))
        cv2.imshow('Video', Img)
        key = cv2.waitKey(5)
        if key != -1:
            exit()
    else:
        break
print('End')
Video.release()

#-*- coding:utf-8 _*-
"""
@author:YuTao
@time: 2019/07/01
"""
import matplotlib
matplotlib.use('Agg')
import numpy as np
import cv2
import cv2.aruco as aruco
tot = 0
CamPosition=None 
MarkerROI=None 
distance=None
dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)  # 编码点的类型,与生成的时候对应
# K = np.array([[228.82, 0, 601.32],
              # [0, 228.59, 363.39],
              # [0, 0, 1]])
# Dis = np.array([0.00490, 0.00105, 0.00012, 0.00047])
K = np.array([[303.55312379,   0.    ,     325.86194854],
              [  0.         ,301.67536832, 223.77253055],
              [  0.          , 0.         ,  1.        ]]
)
Dis = np.array([-3.51044971e-01 , 1.81596870e-01, -2.12578263e-04, -8.09476809e-04,-5.97380810e-02])
EMap = np.loadtxt('EMapForTrafficLight.txt')
MinusY = np.array([[0], [-1], [0.]])  # 相机坐标系Y轴负方向为小车方向
font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below)
parameters =  aruco.DetectorParameters_create()



#纠正相机畸变
def img_undistort(img, mtx, dist):
    if img is None:
        print("img is empty. ERROR!")
    else:
        h,w = img.shape[:2]
        newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(h,w),0,(h,w)) # 自由比例参数
        dis = cv2.undistort(img, mtx, dist, None, newcameramtx)
        # 根据前面ROI区域裁剪图片
        x,y,w,h = roi
        #dis = dis[y:y+h, x:x+int(1.4*w)]
        return dis,newcameramtx

def img_undistort1(img, mtx, dist):
    if img is None:
        print("img is empty. ERROR!")
    else:
        h,w = img.shape[:2]
        dis = cv2.undistort(img, mtx, dist, None, mtx)
        return dis


def aruco_check(Corners,IDs):
    new_IDs = []
    new_Corners = []
    for i, element in enumerate(IDs):
        if element < 2:
            new_IDs.append(element)
            new_Corners.append(Corners[i])
    new_IDs = np.array(new_IDs)
    return new_Corners, new_IDs


def DealMarker(Img,frame):
    #CamPosition = None 
    #MarkerROI = None
    undistort_img = Img
    #distance = None
    global CamPosition, MarkerROI, distance
    global tot 
    #undistort_img,_ = img_undistort(Img, K, Dis) #纠正相机畸变
    gray = cv2.cvtColor(undistort_img, cv2.COLOR_BGR2GRAY)
    ret, th1 = cv2.threshold(gray,100,255,cv2.THRESH_BINARY)
    Corners, IDs, _ = aruco.detectMarkers(gray, dict,parameters=parameters)
    corners_float = np.float32(Corners)
    temp = corners_float.reshape(-1,2)
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
    cv2.cornerSubPix(gray,temp,(5,5),(-1,-1),criteria)
    rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners_float, 0.2, K, Dis)
    ID = np.array(IDs)
    if IDs is None:
        tot = tot + 1
    if (tot > 20):
        CamPosition = None
        MarkerROI = None
        distance = None
    if IDs is not None:  # 如果检测点
        tot = 0     
        if (len(IDs) == 2):
            print("IDS len "+str(len(IDs)))
            Corners,IDs = aruco_check(Corners,IDs)   # 修正错误的点
            Error_detect = False;
            Point3D = np.empty((0, 3))
            Point2D = np.empty((0, 2))
            for i, Corner in enumerate(Corners):
                Point2D = np.vstack((Point2D, Corner.reshape(-1, 2)))
                ID = IDs.flatten()[i]
                if ID>2:
                    Error_detect = True;
                    break
                Point3D = np.vstack((Point3D, np.hstack((EMap[ID, 3:].reshape(-1, 2), np.zeros((4, 1))))))
                if (Error_detect == False):
                    aruco.drawDetectedMarkers(undistort_img,Corners)
                    aruco.drawAxis(undistort_img, K, Dis, rvec[i, :, :], tvec[i, :, :], 0.1)
                    CamPosition = np.mean(tvec, axis=0)[0]
                    distance = np.mean(tvec, axis=0)[0][2]
            MarkerROI = np.hstack((np.min(Point2D,axis=0),np.max(Point2D,axis=0))).astype(np.int)  # xmin,ymin,xmax,ymax
    #else: ##### DRAW "NO IDS" #####
    #   cv2.putText(undistort_img, "No Ids", (0,123), font, 1, (0,0,255),2,cv2.LINE_AA)
    return CamPosition, MarkerROI, distance, undistort_img


# -*- coding:utf-8 _*-
"""
@author:YuTao
@time: 2019/07/01
"""
import matplotlib
matplotlib.use('Agg')
import cv2
import numpy as np
ColorsName = ('', 'Red', 'Yellow', 'Green')

np.set_printoptions(threshold=np.inf)
t = 0
lastColor = 0
Color = 0
def detectColor(image): 
    hsv_img = cv2.cvtColor(image,cv2.COLOR_BGR2HSV)
    cv2.imshow('hsv', hsv_img)
    red_min = np.array([0,0,100])
    red_max = np.array([15,255,255])
    red_min2 = np.array([125,0,100])
    red_max2 = np.array([180,255,255])

    yellow_min = np.array([15,0,100])
    yellow_max = np.array([35,255,255])

    green_min = np.array([35,0,100])
    green_max = np.array([125,255,255])

    red_thresh = cv2.inRange(hsv_img,red_min,red_max)+cv2.inRange(hsv_img,red_min2,red_max2)
    yellow_thresh = cv2.inRange(hsv_img,yellow_min,yellow_max)
    green_thresh = cv2.inRange(hsv_img,green_min,green_max)

    red_blur = cv2.medianBlur(red_thresh,5)
    yellow_blur = cv2.medianBlur(yellow_thresh,5)
    green_blur = cv2.medianBlur(green_thresh,5)
    red = cv2.countNonZero(red_blur)
    yellow = cv2.countNonZero(yellow_blur)
    green = cv2.countNonZero(green_blur)
    print('red', red)
    print('yellow', yellow)
    print('green', green)
    lightColor = max(red,yellow,green)
    if lightColor > 0:
        if lightColor == red:
            return 1
        elif lightColor == yellow:
            return 2
        elif lightColor == green:
            return 3
    else:
        return 0

def TrafficLight(MarkerROI, Img):
    LightColors = []
    global t, lastColor, Color
    if MarkerROI is not None:  # 如果检测到Marker,CamPosition和MarkerROI就不是None
        W = MarkerROI[2] - MarkerROI[0]
        H = MarkerROI[3] - MarkerROI[1]
        MinY = max(MarkerROI[1] - int(2.2 * H), 0)
        MaxY = min(MarkerROI[3] - H, Img.shape[0])
        if MaxY <= MinY + 5:
            return LightColors
        LightImg = Img[MinY:MaxY, MarkerROI[0]:MarkerROI[2], :]  # 提取交通灯的小块区域图像

        # 提取亮点中心轮廓
        LightImgGray = cv2.cvtColor(LightImg, cv2.COLOR_BGR2GRAY)

        th, MaskImg = cv2.threshold(LightImgGray, 200, 255, cv2.THRESH_TOZERO)
        MaskImg = cv2.morphologyEx(MaskImg, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))
        _ , contours, hierarchy = cv2.findContours(MaskImg, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        sel_contours = []
        index = None
        contour = None
        last = 0
        # 根据面积筛选轮廓
        for index1, contour1 in enumerate(contours):
            if (cv2.contourArea(contour1) > last):
                index = index1
                contour = contour1
                last = cv2.contourArea(contour1)
        if (contour is not None):
            Area = cv2.contourArea(contour)
            Hull = cv2.convexHull(contour, False)
            HullArea = cv2.contourArea(Hull)
            if Area > 0 and Area < 2000 and Area / HullArea > 0.4:
                sel_contours.append(contour)
                # 形态学提取外轮廓区域
                MaskImg = np.zeros_like(LightImgGray)
                cv2.drawContours(MaskImg, [contour], -1, 255, cv2.FILLED)
                kernel = np.ones((int(H / 8), int(H / 8)), np.uint8)
                dilation = cv2.dilate(MaskImg, kernel, iterations=1)  # 膨胀
                MaskImg = dilation
                cv2.imshow('mask', MaskImg)
                MaskImg = cv2.cvtColor(MaskImg, cv2.COLOR_GRAY2BGR)
                OutSide = LightImg & MaskImg
                cv2.imshow('gray', OutSide)
                C = detectColor(OutSide)
                print(C)
                print(lastColor)
                if C != lastColor:
                    t = 0
                if C == lastColor:
                    t = t + 1
                if t > 5:
                    Color = C
                if C != 0:
                    lastColor = C   
                print(t)            
        print(ColorsName[Color])
        cv2.putText(Img, ColorsName[Color], (10, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 3)
        cv2.imshow('LightImg', LightImg)
        cv2.waitKey(15)
    return LightColors

1、交通灯识别方案设计目标
方案需要拥有对于相机的标定能力,其目标为能够在较远的范围外准确识别交通灯的信息和交通灯的变化,并且保证在摄像头移动时,识别仍然能够有较高的准确度和稳定性。
同时根据Aruco标签确定相对位置并且输出交通灯相对位置的距离值。
2、技术难点
首先,要在运动中进行交通灯区块的处理有一定的难度,对于形态学和霍夫圆环检测在运动时都存在丢失目标的可能性,那么要如何减少丢失目标所带来的影响是一个技术难点。
其次,对于交通灯的颜色识别在距离较远切摄像头像素较低时可能导致识别失败或者识别错误,确定合理的符合摄像头素质的颜色识别算法,并且通过一定的调试确定识别阈值也是一个技术难点。
3、交通灯识别基本方法
对于一个包含交通灯的情况,识别交通灯的方法基本上大同小异,首先是抓取出包含交通灯的小区块,再对这一个小区块进行处理得到精确的待识别区块,最后将区块中的颜色放入识别算法中。
在组委会提供的代码中,使用的是形态学方法通过分析轮廓和膨胀来得出具体所需要识别的交通灯区块,再在BGR空间下对颜色和标准色的差距进行判断。在实际的调试过程中,发现采用BGR颜色空间识别时,即使将阈值设定得较为宽松,仍然会有较多的识别失败的情况。于是此处我们采用了基于HSV颜色空间的颜色识别算法。HSV采用色调(H),饱和度(S),明度(V)来表示颜色,其具有更平衡的色差感知[5],在实际应用中可以发现其对于颜色的识别更加精确。

识别的交通灯区块(左),处理后得到的HSV空间中的待检测交通灯(右)
在实际操作中,由于信号灯的中心位置为明度和饱和度都非常高的白色,识别时会导致识别困难,同时因为摄像头精度较低,在远距离时其只能检测出个位数的目标像素点,这就要求设置合理的阈值。
如果我们将上述的HSV图像输出,可以得到opencv在转化后得到的HSV空间图像数组,在进行了比对后,我们发现,将red的值设定为[0~15+125~180,0~255, 100~255],green的值设定为[35~125,0~255,100~255],yellow的值设定为[15~35,0~255, 100~255]可以最大限度的满足给出的测试视频。

HSV颜色空间中各个颜色的区域划分
我们的方案在保证了设定的阈值可以使得极少部分识别错误的情况被忽略的同时,使得检测的信号灯信息在具有一定的可信度后长期保留,以提高稳定程度。我们采用了连续三帧识别到同一种型号灯颜色后就保留这种信号灯颜色的方法,在测试中发现以连续三帧为信息可信的依据,既能满足快速识别的要求,并且有非常高的稳定程度,信号一经识别,在系统中的信号信息就能一直存在,不会中断,方便了对于以后的运动控制。同样的,在二维码识别的方面,我们也采用了类似的稳定方案,如果出现短暂的识别不稳定,程序会直接沿用上一次的数据进行识别,这种方案在直道和慢速转弯的情况下可以很大程度上提升识别的稳定性。
在给定的测试视频中,我们的方案中给出的算法能够从视频的第510帧开始就稳定锁定颜色为绿色,而原算法需要在1000帧后才能开始识别到绿色,在1600帧后才能较为稳定的识别出绿色。

成功进行远距离识别的开始帧数
而对于红灯,我们的方案可以在最开始就识别到目标,并且在转弯的过程中能够很好的保持对于交通灯的识别,直到第一个二维码完全离开相机拍摄的画面为止。

在转弯状态下对交通灯的识别
本方案对颜色识别的范围、阈值等参数都有简便的更改参数方式,可以满足对于实际上的调试要求。
而对于交通灯区块的处理,在尝试了形态学方案和霍夫圆环检测后,发现采用形态学方案进行交通灯的提取更加稳定,在450帧就可以捕获到目标,于是在方案中保留了形态学方案。同时,我们将原本方案中的检测改为直接捕获面积最大的区域作为待识别区域。


「雪霁融雾月,冰消凝夜雨」