交通灯识别

校准

1.启动gazebo节点

roslaunch turtlebot3_gazebo turtlebot3_autorace_2020.launch

2.打开相机内标定

roslaunch turtlebot3_autorace_camera intrinsic_camera_calibration.launch

3.启动相机外标定

roslaunch turtlebot3_autorace_camera extrinsic_camera_calibration.launch  

4.启动交通灯检测校准节点

roslaunch turtlebot3_autorace_detect detect_traffic_light.launch mode:=calibration

• 默认情况下，gazebo初始加载是不带交通灯模块的，需要额外加载
• 额外加载交通灯模块是快速切换红黄绿灯，切换时候较快，不足以满足校准，所以需要修改切换灯的时间
• 新终端，修改turtlebot3_autorace_2020/turtlebot3_autorace_core/nodes下的core_node_mission文件
• 校准黄灯时，同样在切换绿灯的步骤前，添加time sleep() 函数设置切换绿灯的等待时间

5.加载其它gazebo控件

roslaunch turtlebot3_autorace_core turtlebot3_autorace_mission.launch

6.打开可视化

rqt

• 点击rqt左上角菜单栏Plugins -> Cisualization -> Image view，依次添加三个窗口且分别订阅/detect/image_red_light、/detect/image_yellow_light、/detect/image_green_light和/detect/image_traffic_light四个主题

7.打开rqt_reconfigure工具

rosrun rqt_reconfigure rqt_reconfigure

• 点击detect_traffic_lane后调整参数来过滤红色、黄色和绿色，实际需要的过滤效果可以参考上图
• 新终端，将调整好的值写入turtlebot3_autorace_detect/param/traffic_light/的traffic_light.yaml文件中

---
detect:
  traffic_light:
    red:
      hue_l: 0
      hue_h: 26
      saturation_l: 239
      saturation_h: 255
      lightness_l: 123
      lightness_h: 250
    yellow:
      hue_l: 19
      hue_h: 33
      saturation_l: 237
      saturation_h: 255
      lightness_l: 231
      lightness_h: 255
    green:
      hue_l: 40
      hue_h: 113
      saturation_l: 210
      saturation_h: 255
      lightness_l: 131
      lightness_h: 255

测试

1.关闭所有终端,打开gazebo

roslaunch turtlebot3_gazebo turtlebot3_autorace_2020.launch

2.启动相机内标定

roslaunch turtlebot3_autorace_camera intrinsic_camera_calibration.launch

3.启动交通灯任务程序

roslaunch turtlebot3_autorace_core turtlebot3_autorace_core.launch mission:=traffic_light

4.设置decided_mode为3

rostopic pub -1 /core/decided_mode std_msgs/UInt8 "data: 3"

5.加载gazebo任务

roslaunch turtlebot3_autorace_core turtlebot3_autorace_mission.launch

说明:

1.turtlebot3_autorace_core.launch会启动两个节点:traffic_light__core_node_controller以及traffic_light__core_mode_decider

2.执行了rostopic pub -1 /core/decided_mode std_msgs/UInt8 "data: 3",则traffic_light__core_node_controller节点接收这个topic并将它的self.current_mode字段设置为3

3.在traffic_light__core_node_controller节点的fnControlNode方法中执行self.current_mode == self.CurrentMode.traffic_light.value的操作,会启动detect_lane.launch、detect_traffic_light.launch以及turtlebot3_autorace_control_lane.launch

4.detect_traffic_light节点检测出结果会发布/detect/traffic_light话题

5.traffic_light__core_mode_decider接收到/detect/traffic_light话题

发布/core/decided_mode让traffic_light_core_controller处理(mode为2)

6.traffic_light_core_controller收到mode为2之后启动跟随

斑点(Blob)检测

斑点(Blob)的定义: 图像特征点检测包括角点和斑点,斑点是指二维图像中和周围颜色有颜色差异和灰度差异的区域,因为斑点代表的是一个区域,所以其相对于单纯的角点,具有更好的稳定性和更好的抗干扰能力.斑点通常是指与周围有着颜色和灰度差别的区域。在实际地图中，往往存在着大量这样的斑点，如一颗树是一个斑点，一块草地是一个斑点，一栋房子也可以是一个斑点。由于斑点代表的是一个区域，相比单纯的角点，它的稳定性要好，抗噪声能力要强，所以它在图像配准上扮演了很重要的角色。同时有时图像中的斑点也是我们关心的区域，比如在医学与生物领域，我们需要从一些X光照片或细胞显微照片中提取一些具有特殊意义的斑点的位置或数量

SimpleBlobDetector_create的原理:

1. 阈值：通过使用以minThreshold开始的阈值对源图像进行阈值处理，将源图像转换为多个二进制图像。这些阈值以thresholdStep递增，直到maxThreshold。因此，第一个阈值为minThreshold，第二个阈值为minThreshold + thresholdStep，第三个阈值为minThreshold + 2 x thresholdStep，依此类推；

2. 分组：在每个二进制图像中，连接的白色像素被分组在一起。我们称这些二进制blob；

3. 合并：计算二进制图像中二进制斑点的中心，并合并比minDistBetweenBlob更近的斑点；

4. 中心和半径计算：计算并返回新合并的Blob的中心和半径。

SimpleBlobDetector_create可按颜色，大小和形状来过滤斑点类型：

1. 按颜色：首先需要设置filterByColor =True。设置blobColor = 0可选择较暗的blob，blobColor = 255可以选择较浅的blob。

2. 按大小：可以通过设置参数filterByArea = 1以及minArea和maxArea的适当值来基于大小过滤blob。例如。设置minArea = 100将滤除所有少于100个像素的斑点。

3. 按圆度：这只是测量斑点距圆的距离。例如。正六边形的圆度比正方形高。要按圆度过滤，请设置filterByCircularity =1。然后为minCircularity和maxCircularity设置适当的值。圆度定义为（）。圆的为圆度为1，正方形的圆度为PI/4，依此类推。

4. 按凸性：凸度定义为（斑点的面积/凸包的面积）。现在，形状的“凸包”是最紧密的凸形，它完全包围了该形状，用不严谨的话来讲，给定二维平面上的点集，凸包就是将最外层的点连接起来构成的凸多边形，它能包含点集中所有的点。直观感受上，凸性越高则里面“奇怪的部分”越少。要按凸度过滤，需设置filterByConvexity = true，minConvexity、maxConvexity应该属于[0,1]，而且maxConvexity> minConvexity。

5. 按惯性比：这个词汇比较抽象。我们需要知道Ratio可以衡量形状的伸长程度。简单来说。对于圆，此值是1，对于椭圆，它在0到1之间，对于直线，它是0。按惯性比过滤，设置filterByInertia = true，并设置minInertiaRatio、maxInertiaRatio同样属于[0,1]并且maxConvexity> minConvexity。

参数说明

SimpleBlobDetector::Params::Params()
{
    thresholdStep = 10;    //二值化的阈值步长，即公式1的t
    minThreshold = 50;   //二值化的起始阈值，即公式1的T1
    maxThreshold = 220;    //二值化的终止阈值，即公式1的T2
    //重复的最小次数，只有属于灰度图像斑点的那些二值图像斑点数量大于该值时，该灰度图像斑点才被认为是特征点
    minRepeatability = 2;   
    //最小的斑点距离，不同二值图像的斑点间距离小于该值时，被认为是同一个位置的斑点，否则是不同位置上的斑点
    minDistBetweenBlobs = 10;

    filterByColor = true;    //斑点颜色的限制变量
    blobColor = 0;    //表示只提取黑色斑点；如果该变量为255，表示只提取白色斑点

    filterByArea = true;    //斑点面积的限制变量
    minArea = 25;    //斑点的最小面积
    maxArea = 5000;    //斑点的最大面积

    filterByCircularity = false;    //斑点圆度的限制变量，默认是不限制
    minCircularity = 0.8f;    //斑点的最小圆度
    //斑点的最大圆度，所能表示的float类型的最大值
    maxCircularity = std::numeric_limits<float>::max();

    filterByInertia = true;    //斑点惯性率的限制变量
    minInertiaRatio = 0.1f;    //斑点的最小惯性率
    maxInertiaRatio = std::numeric_limits<float>::max();    //斑点的最大惯性率

    filterByConvexity = true;    //斑点凸度的限制变量
    minConvexity = 0.95f;    //斑点的最小凸度
    maxConvexity = std::numeric_limits<float>::max();    //斑点的最大凸度
}

检测案例:

# -*- coding:utf-8 -*-
import cv2
import numpy as np

# Read image
im = cv2.imread("img/shapes.jpg", cv2.IMREAD_GRAYSCALE)

# 设置SimpleBlobDetector_Params参数
params = cv2.SimpleBlobDetector_Params()
# 改变阈值
params.minThreshold = 10
params.maxThreshold = 200
# 通过面积滤波
params.filterByArea = True
params.minArea = 1500
# 通过圆度滤波
params.filterByCircularity = True
params.minCircularity = 0.1
# 通过凸度滤波
params.filterByConvexity = True
params.minConvexity = 0.87
# 通过惯性比滤波
params.filterByInertia = True
params.minInertiaRatio = 0.01
# 创建一个检测器并使用默认参数
detector = cv2.SimpleBlobDetector_create(params)
# 检测blobs.
key_points = detector.detect(im)

# 绘制blob的红点
draw_image = cv2.drawKeypoints(im, key_points, np.array([]), (0, 0, 255),
                               cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

# Show blobs
cv2.imshow("key_points", draw_image)
cv2.waitKey(0)

交通灯检测

# -*- coding:utf-8 -*-
import cv2 as cv
import numpy as np

Hue_l = 40
Hue_h = 113
Saturation_l = 210
Saturation_h = 255
Lightness_l = 131
Lightness_h = 255
ori_img = None

def fnMaskGreenTrafficLight(cv_image):
    # 图像
    image = np.copy(cv_image)

    # 转换成HSV颜色
    hsv = cv.cvtColor(image, cv.COLOR_BGR2HSV)

    # 绿色HSV颜色区间
    lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
    upper_green = np.array([Hue_h, Saturation_h, Lightness_h])

    # 绿色掩膜
    mask = cv.inRange(hsv, lower_green, upper_green)

    # 保留图像中绿色颜色
    res = cv.bitwise_and(image, image, mask=mask)

    mask = cv.bitwise_not(mask)

    return mask

def fnFindCircleOfTraficLight(img):
    # 设置SimpleBlobDetector_Params参数
    params = cv.SimpleBlobDetector_Params()
    # 改变阈值
    params.minThreshold = 10
    params.maxThreshold = 255
    # 通过面积滤波
    params.filterByArea = True
    params.minArea = 50
    params.minArea = 600
    # 通过圆度滤波
    params.filterByCircularity = True
    params.minCircularity = 0.4
    # 通过凸度滤波
    params.filterByConvexity = True
    params.minConvexity = 0.6
    # 通过惯性比滤波
    # params.filterByInertia = True
    # params.minInertiaRatio = 0.01
    # 创建一个检测器并使用默认参数
    detector = cv.SimpleBlobDetector_create(params)
    # 检测blobs.
    keypts = detector.detect(img)
    print(len(keypts))
    # 绘制blob的红点
    draw_image = cv.drawKeypoints(img, keypts, np.array([]), (0, 0, 255),
                                  cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

    col1 = 180
    col2 = 270
    col3 = 305

    low1 = 50
    low2 = 170
    low3 = 170

    # # if detected more than 1 light
    for i in range(len(keypts)):

        point_col = int(keypts[i].pt[0])
        point_low = int(keypts[i].pt[1])
        if point_col > col1 and point_col < col2 and point_low > low1 and point_low < low2:
            # if find_color == 'green':
            #     status = 1
            # elif find_color == 'yellow':
            #     status = 2
            # elif find_color == 'red':
            #     status = 3
            pass
        elif point_col > col2 and point_col < col3 and point_low > low1 and point_low < low3:
            # if find_color == 'red':
            #     status = 4
            # elif find_color == 'green':
            #     status = 5
            pass
        else:
            status = 6

    # Show blobs
    cv.imshow("key_points", draw_image)
    cv.waitKey(0)

def main():
    # Read image
    ori_img = cv.imread("img/trafic_light_green.png")
    # ori_img = cv.imread("img/shapes.jpg")
    # 获取绿色
    cv_image_mask = fnMaskGreenTrafficLight(ori_img)
    # 高斯滤波
    cv_image_mask = cv.GaussianBlur(cv_image_mask, (5, 5), 0)
    fnFindCircleOfTraficLight(cv_image_mask)



if __name__ == '__main__':
    main()