TB3_Autorace之路标检测

发表于 | 分类于

利用SIFT检测器对交通标志牌进行匹配检测，控制TB3完成相应的任务动作！
0

谦卑并不意味着多顾他人少顾自己，
也不意味着承认自己是个无能之辈，
是意味着从根本上把自己置之度外。

订阅话题

同车道线检测《TB3_Autorace之信号灯检测》程序一样，我们这里接收未经透视变换的原始图像信息/detect/image_input

1
2
3
4
5
6
7
8
9
self.sub_image_type = "raw" 
self.pub_image_type = "compressed" 

    if self.sub_image_type == "compressed":
        # subscribes compressed image
        self.sub_image_original = rospy.Subscriber('/detect/image_input/compressed', CompressedImage, self.cbFindTrafficSign, queue_size = 1)
    elif self.sub_image_type == "raw":
        # subscribes raw image
        self.sub_image_original = rospy.Subscriber('/detect/image_input', Image, self.cbFindTrafficSign, queue_size = 1)

发布话题

发布检测到的路标信息，用以控制TB3机器人执行相应的动作。

1
2
3
4
5
6
7
8
self.pub_traffic_sign = rospy.Publisher('/detect/traffic_sign', UInt8, queue_size=1)

if self.pub_image_type == "compressed":
    # publishes traffic sign image in compressed type
    self.pub_image_traffic_sign = rospy.Publisher('/detect/image_output/compressed', CompressedImage, queue_size = 1)
elif self.pub_image_type == "raw":
    # publishes traffic sign image in raw type
    self.pub_image_traffic_sign = rospy.Publisher('/detect/image_output', Image, queue_size = 1)

设定检测状态

这里我们利用python的枚举操作将路标检测封装为几个不同状态，包括停车、泊车、隧道三个路标信息。根据不同路标指示执行相应的动作。

1
2
3
4
self.TrafficSign = Enum('TrafficSign', 'divide '
                                       'stop '
                                       'parking '
                                       'tunnel')

特征点匹配

初始化一个SIFT检测器，读取存储的路标信息，利用SIFT提取图片的关键点和特征符，通过FlannBasedMatcher算法（二维特征点匹配）进行近似匹配得到self.flann值。代码封装如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
def fnPreproc(self):
    self.sift = cv2.xfeatures2d.SIFT_create()

    dir_path = os.path.dirname(os.path.realpath(__file__))
    dir_path = dir_path.replace('turtlebot3_autorace_detect/nodes', 'turtlebot3_autorace_detect/')
    dir_path += 'file/detect_sign/'

    self.img2 = cv2.imread(dir_path + 'stop.png',0)         # Image1
    self.img3 = cv2.imread(dir_path + 'parking.png',0)      # Image2
    self.img4 = cv2.imread(dir_path + 'tunnel.png',0)       # Image3

    self.kp2, self.des2 = self.sift.detectAndCompute(self.img2,None)
    self.kp3, self.des3 = self.sift.detectAndCompute(self.img3,None)
    self.kp4, self.des4 = self.sift.detectAndCompute(self.img4,None)

    FLANN_INDEX_KDTREE = 0
    index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
    search_params = dict(checks = 50)   #指定递归遍历的次数。值越高结果越准确，但是消耗的时间也越多

    self.flann = cv2.FlannBasedMatcher(index_params, search_params)

均方差计算

计算单应性矩阵的均方差用以判断是否寻找到目标。代码封装如下：

1
2
3
4
5
6
def fnCalcMSE(self, arr1, arr2):
        squared_diff = (arr1 - arr2) ** 2
        sum = np.sum(squared_diff)
        num_all = arr1.shape[0] * arr1.shape[1] #cv_image_input and 2 should have same shape
        err = sum / num_all
        return err

路标检测

将帧率设置成10fps，将图像格式转换成OpenCV格式，设定最小匹配数来决定是否开始寻找目标，设定最小均方差值。利用Flann单应性检测器对图像中的信息和存储的图像信息进行匹配，返回一个训练集和询问集。我们创建一个数组来存储匹配后得到的训练集成员，如果匹配得到的训练集成员个数大于设定的最小匹配数，就开始寻找目标。
提取匹配的对应点的坐标信息,计算3x3映射矩阵(单应性矩阵H)对图像进行矫正，并计算矩阵的均方差，若均方差小于设定的均方差值，则说明图像匹配成功，检测到路标。将匹配上的特征点连接并标识在图像上，发布交测到的路标信息。代码封装如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
def cbFindTrafficSign(self, image_msg):
     if self.counter % 3 != 0:
         self.counter += 1
         return
     else:
         self.counter = 1

     if self.sub_image_type == "compressed":
         np_arr = np.fromstring(image_msg.data, np.uint8)
         cv_image_input = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
     elif self.sub_image_type == "raw":
         cv_image_input = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")

     MIN_MATCH_COUNT = 9   
     MIN_MSE_DECISION = 50000

     # find the keypoints and descriptors with SIFT
     kp1, des1 = self.sift.detectAndCompute(cv_image_input,None)

     matches2 = self.flann.knnMatch(des1,self.des2,k=2)
     matches3 = self.flann.knnMatch(des1,self.des3,k=2)
     matches4 = self.flann.knnMatch(des1,self.des4,k=2)

     image_out_num = 1

     good2 = []
     for m,n in matches2:
         if m.distance < 0.7*n.distance:
             good2.append(m)

     if len(good2)>MIN_MATCH_COUNT:

         src_pts = np.float32([kp1[m.queryIdx].pt for m in good2]).reshape(-1, 1, 2)
         dst_pts = np.float32([self.kp2[m.trainIdx].pt for m in good2]).reshape(-1, 1, 2)
         M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
         matchesMask2 = mask.ravel().tolist()

         mse = self.fnCalcMSE(src_pts, dst_pts)
         if mse < MIN_MSE_DECISION:
             msg_sign = UInt8()
             msg_sign.data = self.TrafficSign.stop.value

             self.pub_traffic_sign.publish(msg_sign)

             rospy.loginfo("TrafficSign 2")

             image_out_num = 2

     else:
         matchesMask2 = None

     if image_out_num == 1:
         if self.pub_image_type == "compressed":
             # publishes traffic sign image in compressed type
             self.pub_image_traffic_sign.publish(self.cvBridge.cv2_to_compressed_imgmsg(cv_image_input, "jpg"))

         elif self.pub_image_type == "raw":
             # publishes traffic sign image in raw type
             self.pub_image_traffic_sign.publish(self.cvBridge.cv2_to_imgmsg(cv_image_input, "bgr8"))

     elif image_out_num == 2:
         draw_params2 = dict(matchColor = (0,0,255), # draw matches in green color
                         singlePointColor = None,
                         matchesMask = matchesMask2, # draw only inliers(内对)
                         flags = 2)

         final2 = cv2.drawMatches(cv_image_input,kp1,self.img2,self.kp2,good2,None,**draw_params2)

         if self.pub_image_type == "compressed":
             self.pub_image_traffic_sign.publish(self.cvBridge.cv2_to_compressed_imgmsg(final2, "jpg"))

         elif self.pub_image_type == "raw":
             self.pub_image_traffic_sign.publish(self.cvBridge.cv2_to_imgmsg(final2, "bgr8"))

1

至此，已完成所有交通标志牌检测任务！

附

完整程序源代码：https://github.com/sun-coke/TB3_Autorace

-------------本文结束 感谢阅读-------------
0%