通过ROS控制真实机械臂(16) --- 视觉抓取之find_object实现物体识别

视觉抓取中非常重要的一个部分就是对抓取物体的识别，无论是二维图像还是三维点云，在ROS中都可以找到对应的功能包，本次测试的是能对物体进行快速识别，甚至定位的find_object包，该功能包是基于模板匹配算法（包括彩色图像或点云匹配）。ubuntu14.04+indigo+usb摄像头/kinect相机/Zed mini相机。

本文参考： https://github.com/introlab/find-object

一、下载安装(两种方式)：

1. Install安装：

# ROS Kinetic:
 $ sudo apt-get install ros-kinetic-find-object-2d
# ROS Jade:
 $ sudo apt-get install ros-jade-find-object-2d
# ROS Indigo:
 $ sudo apt-get install ros-indigo-find-object-2d
# ROS Hydro:
 $ sudo apt-get install ros-hydro-find-object-2d

2. 源码安装：

# Install ROS Groovy/Hydro/Indigo/Jade/Kinetic (catkin build):
 $ cd ~/catkin_ws
 $ git clone https://github.com/introlab/find-object.git src/find_object_2d
 $ catkin_make

# Install ROS Fuerte (in a directory of your "ROS_PACKAGE_PATH"):
 $ svn checkout -r176 http://find-object.googlecode.com/svn/trunk/ros-pkg/find_object_2d
 $ rosmake find_object_2d

二、简单启动测试：

1. USB摄像头

 # &表示后台运行
 $ roscore &

 # 启动摄像机节点
 $ rosrun uvc_camera uvc_camera_node &
 # 或者启动摄像机launch文件，我更倾向这种
 $ roslaunch usb_cam usb_cam-test.launch

 $ rosrun find_object_2d find_object_2d image:=image_raw
 # 通过launch启动话题需要修改
 $ rosrun find_object_2d find_object_2d image:=/usb_cam/image_raw 

2. KInect摄像头

 # 启动Kinect摄像机节点
 $ rosrun freenect_launch freenect.launch

 # 查看话题类型
 $ rostopic list
 
 # 双目相机同理，订阅对应的二维图像话题即可
 $ rosrun find_object_2d find_object_2d image:=/camera/rgb/image_color
 
 # 如果使用选择成/camera/rgb/image_raw会报错
  find_object_ros: Encoding "bayer_grbg8" detected. Supported image encodings are bgr8 and rgb8...

三、基于二维图像识别和检测物体

在出现的界面左侧空白栏中鼠标右击选择Add obejct from scene 从场景中选取待检测物体，讲想要识别的物体置于场景当中，点击take picture，然后左上角可以选择拍照的具体的区域或者角点，next之后确认无误end即可。

此时左侧空白处已经添加了一个待识别物体，当我们讲该物体放到摄像头的视角当中的时候，待识别物体就会背框选出来。

find_object包中还有一个节点print_objects_detected可以用来确定物体的位置,当没有检测处待测物体的时候，显示的是No object detected，当待识别物体出现在视野当中的时候，就可以看到终端输出的物体位置信息。

源码：
printf("Object %d detected, Qt corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n",
                    id,
                    qtTopLeft.x(), qtTopLeft.y(),
                    qtTopRight.x(), qtTopRight.y(),
                    qtBottomLeft.x(), qtBottomLeft.y(),
                    qtBottomRight.x(), qtBottomRight.y());

从源码可以看出，数据还不是特别容易观察，但该信息也通过话题/objects发布，编程的是订阅话题即可获取数据。通过topic查看：

$ rostopic echo /objects

输出结果说明：
Objects detected formatted as [objectId1, objectWidth, objectHeight, h11, h12, h13, h21, h22, h23, h31, h32, h33, objectId2...] where h## is a 3x3 homography matrix (h31 = dx and h32 = dy, see QTransform).

输出结果：
layout: 
  dim: []
  data_offset: 0
data: [4.0, 289.0, 271.0, 1.8393758535385132, 0.0202195942401886, 0.00017601421859581023, 0.4901503920555115, 2.0912954807281494, 0.00150511774700135, 125.54476928710938, 102.35379791259766, 1.0]


# 对应的结果来看 此处物体的宽和高都是为289.0和271.0，大致是真实物体的宽和高的一半，我觉得可能描述的是识别物体的中心坐标，也可能是配置和标定不准确，有待后期测试

四、基于三维深度识别和检测物体

和usb单目摄像头不同的是，find_object还专门提供了一个find_object_3d节点，专为kinect或者zed双目之类的深度相机准备的，可以通过在匹配目标后识别目标中心的深度信息输出目标的三维坐标。

1. 启动深度相机

# kinect2
roslaunch kinect2_bridge kinect2_bridge,launch

# realsesnse D414/435
roslaunch realsense2_camera rs_camera.launch

# zed mini相机
roslaunch zed_wrapper zedm.launch

2. 启动find_object_3d节点

 # 双目相机
 $ roslaunch find_object_2d find_object_3d.launch
 # 或
 $ roslaunch find_object_2d find_object_3d_kinect2.launch
 # 或
 $ roslaunch find_object_2d find_object_3d_zed.launch

由于kinect一代和二代有点区别，如下是我用kinect和zed的launch文件

3.话题确定(如果launch文件发布的话题不同，还需要手动映射参数)

​# find_object_3d节点接收的话题
topic：rgb/image_rect_color (message_type: sensor_msgs/Image) //对齐后的彩色图像
topic：rgb/camera_info (message_type: sensor_msgs/CameraInfo) //相机标定信息
topic：registered_depth/image_raw (message_type: sensor_msgs/Image)//深度图像​


# find_object_3d节点发布的话题
topic：objects (message_type: std_msgs/Float32MultiArray)    // 同二维
topic：objectsStamped (message_type: find_object_2d/ObjectsStamped)//带标签的对象物体

4 调整参数，选择特征算子及算法(参考知乎大神：https://zhuanlan.zhihu.com/p/71603204)

可以选择SURF、SIFT、ORB、FAST等一系列算法，观察目标物体上的特征点数量及效果，以及处理的流畅程度，衡量选择合适的算法。通过菜单View - Parameters调出参数栏选项卡进行调整。此外如果我们是使用find_object_3d，可以打开rviz可视化工具，看到目标的TF坐标已经被发出来了（这里需要正确的配置相机的TF坐标系），此外此节点还发布了目标中心坐标的图像，可以通过rqt_image_view查看 /image_with_objects。整体来说这个功能包安装是相当方便的，不过因为是模板匹配，所以局限性比较大，在目标物体未正对摄像头的情况下识别成功率不高，而且想要得到良好的匹配效果需要认真的调节参数，旋转合适的算法，另外输出的tf中心点坐标大致准确，但是姿态就不是很靠谱了。

$ rosrun rviz rviz  (for visualisation purpose: set "fixed_frame" to "/camera_link" and add TF display)