深度摄像机实现物体识别和三维定位（ros+tensorflow+乐视深度摄像机）

---

前言

本文的目的在于记录我自己在使用乐视深度摄像机实现物体识别定位功能的步骤，以防以后忘记一些细节，tensorflow安装在虚拟环境中比较好，参考古月的教程，本人使用的版本为1.14.0

---

一、摄像机驱动安装和标定

以下两种方法均可实现摄像机的运行：
1.修改astrapro.launch文件中product的产品号
2.修改astra.launch文件

参考文章：
方法1参考链接
方法2参考链接

摄像机标定：
参考文章

二、代码解析

主要参考代码：ros_object_detection_3d
改代码使用的tensorflow的物体检测API进行物体识别
测距离部分的代码：

def get_object_depth(self, obj_struct, cv_image, depth_image, i):
  
            #
            X_SCALING = 1.333
            Y_SCALING = 0.75
            bbox = obj_struct.bbox
            top_left     = (int((bbox.center.x-(bbox.size_x/2))*X_SCALING)), \
                           (int((bbox.center.y-(bbox.size_y/2))*Y_SCALING))
            bottom_right = (int((bbox.center.x+(bbox.size_x/2))*X_SCALING)), \
                           (int((bbox.center.y+(bbox.size_y/2))*Y_SCALING))
	    center       = (int(bbox.center.x*X_SCALING)),(int(bbox.center.y*Y_SCALING)) #define center

	    # get average depth
	    RECT_ROI_DIST = 15
            # 
            if ( center[0]<RECT_ROI_DIST ):
                center[0] = RECT_ROI_DIST
            elif ( center[0]+RECT_ROI_DIST > IMAGE_WIDTH ):
                center[0] = IMAGE_WIDTH-RECT_ROI_DIST
            if ( center[1]<RECT_ROI_DIST ):
                center[1] = RECT_ROI_DIST
            elif ( center[1]+RECT_ROI_DIST > IMAGE_HEIGHT ):
                center[1] = IMAGE_HEIGHT-RECT_ROI_DIST
            #
            rect_roi_corners = [( center[0]-RECT_ROI_DIST, center[1]-RECT_ROI_DIST ), \
                                ( center[0]+RECT_ROI_DIST, center[1]+RECT_ROI_DIST)] 
	    rect_roi = depth_image[center[0]-RECT_ROI_DIST:center[0]+RECT_ROI_DIST, \
                                   center[1]-RECT_ROI_DIST:center[1]+RECT_ROI_DIST]
	    try:
                avg_depth = int(np.median(rect_roi))
            except:
                avg_depth = np.mean(rect_roi)
                pass
           
            avg_depth = depth_image[center[1],center[0]]#box的中心为depth点

   
	    if DEBUG_MODE == 1:

                print('Object: '+str(i)+': '+str(center[0])+','+str(center[1])) 

                # write depth of object on images  
                cv2.putText(cv_image, str((avg_depth)), center, cv2.FONT_HERSHEY_SIMPLEX, \
                            1.0, (255,255,255), lineType=cv2.LINE_AA)
                cv2.putText(depth_image, str((avg_depth)), center, cv2.FONT_HERSHEY_SIMPLEX, \
                            1.0, (255,255,255), lineType=cv2.LINE_AA)

		# put coords on image 
                #cv2.putText(cv_image, str(top_left), top_left, cv2.FONT_HERSHEY_SIMPLEX, \ 
                #             1.0, (255,255,255), lineType=cv2.LINE_AA)
                #cv2.putText(cv_image, str(bottom_right), bottom_right, cv2.FONT_HERSHEY_SIMPLEX, \
                #             1.0, (255,255,255), lineType=cv2.LINE_AA)
                #cv2.putText(cv_image, str(bbox), (100,100), cv2.FONT_HERSHEY_SIMPLEX, \ 
                #             1.0, (255,255,255), lineType=cv2.LINE_AA)
	        #print(str(bbox))

                # draw bboxes on images
                cv2.rectangle(cv_image, top_left, bottom_right, (0,0,0), thickness=4)
                cv2.rectangle(cv_image, rect_roi_corners[0], rect_roi_corners[1], (0,0,0), thickness=4)
                cv2.circle(cv_image, center, 2, (255,255,255), -4)
                cv2.rectangle(depth_image, top_left, bottom_right, (0,0,0), thickness=4)
                cv2.rectangle(depth_image, rect_roi_corners[0], rect_roi_corners[1], (0,0,0), thickness=4)
                cv2.circle(depth_image, center, 2, (255,255,255), -4)

                # draw line from center of image to center of bbox
                image_center = (IMAGE_WIDTH/2, IMAGE_HEIGHT/2)
                cv2.circle(cv_image, image_center, 2, (255,255,255), -4)
                #
                cv2.line(cv_image, image_center, center, (255,0,0), 3)
                # center depth 
                center_depth = depth_image[image_center[1],image_center[0]]
                cv2.putText(cv_image, str((center_depth)), image_center, cv2.FONT_HERSHEY_SIMPLEX, \
                            1.0, (255,255,255), lineType=cv2.LINE_AA)
           
                signed_height = center[1] - IMAGE_HEIGHT/2 
                signed_width  = center[0] - IMAGE_WIDTH/2
                mid_height = (center[1]+(IMAGE_HEIGHT/2))/2
                mid_width  = (center[0]+(IMAGE_WIDTH/2))/2
                cv2.line(cv_image, (image_center[0],center[1]), image_center, (255,0,0), 3)
                cv2.line(cv_image, (image_center[0],center[1]), center, (255,0,0), 3)
         
                #cv2.putText(cv_image, 'u='+str((signed_height)), (mid_height,mid_width), \
                #             cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,255,255), lineType=cv2.LINE_AA) 
                #cv2.putText(cv_image, 'v='+str((signed_width)), (mid_height,mid_width), \
                #             cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,255,255), lineType=cv2.LINE_AA) 


            #if avg_depth == 0:
                #print('Object'+str(i)+'detected, but depth reads 0')
                 
 
	    return avg_depth, center, cv_image

遇到问题：
callback没有被触发，在stackoverflow上找到了解决办法

 self.depth_image_sub = message_filters.Subscriber('/camera/depth_registered/image_raw', Image)
        self.color_image_sub = message_filters.Subscriber('/camera/rgb/image_raw', Image)
        ts = message_filters.TimeSynchronizer([self.depth_image_sub, self.color_image_sub], 10)
        ts.registerCallback(self.processing_cb)

将message_filters.TimeSynchronizer这段改为ApproximateTimeSynchronizer,具体用法参考Roswiki的messagefilter部分

三、运行

安装好驱动，可以roslaunch摄像机
对于乐视摄像机:

roslaunch astra_camera astrapro.launch

然后launch detector

roslaunch tensorflow_object_detector launch_file_name

也可以自己写一个launch file来launch以上两个node

四、测试

物体1:cup
深度相机显示距离：985mm
手动测量：约为990mm

物体2:bottle
深度相机显示距离：580mm
手动测试距离：约为590mm

手动测量误差十分大，故1cm左右的误差实属正常，有机会将使用更科学的方法来测量精度。

本文代码下载