一起做ROS-DEMO系列 (2):基于find_object_2d的目标模板匹配识别

• 接上文，我们希望机器人能够更加智能一点，抓住我们想要的任何东西，而不是通过贴标签（ar_makrer）或者简单的颜色过滤分割（比如固定识别某纯色物体）来进行目标物体的识别。所以我们打算采用其他的方法来进行目标的识别识别。
• 目前我知道的比较好上手的方法有两种，一是使用模板匹配（包括彩色图像或点云匹配），二是使用最近大热的机器学习来做（比如yolo等，以后会具体介绍）。那么这次我会讲一讲基于彩色图像的模板匹配，因为目前ROS上现有的find_object_2d功能包能够让我们在不需要了解具体实现原理的情况下，快速上手此功能，比较适合任务的迅速部署。
• 参考资料：
  http://wiki.ros.org/find_object_2d
  https://github.com/introlab/find-object/tree/kinetic-devel
  http://introlab.github.io/find-object/
  

---

---

1、find_object_2d 功能包安装

直接悬着对应的ros版本进行安装即可，其他版本安装参见官方说明：https://github.com/introlab/find-object/tree/kinetic-devel

# 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、功能包的可视化使用

该功能包有两个节点find_object_2d及find_object_3d，其中find_object_3d是为kinect之类的深度相机准备的，可以通过在匹配目标后识别目标中心的深度信息输出目标的三维坐标。

2.1 、find_object_2d

启动过程：

 $ roscore 
 $ 启动你的相机节点，输出图像话题
 $ rosrun find_object_2d find_object_2d image:=image_raw

注意 image:=image_raw，这是个话题重映射，这里的image_raw要修改为你的相机输出的图像话题。

• launch文件写法：

<launch>
	<!-- Nodes -->
	<node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen">
		<remap from="image" to="image"/>
		<param name="gui" value="false" type="bool"/>
		<param name="objects_path" value="~/objects" type="str"/>
		<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
	</node>
</launch

2.2 、find_object_3d

• 启动过程：（由于需要调用多个参数，所以这里采用launch文件配置参数，启动节点）

• 1、启动深度相机，如：

# kinect2
roslaunch kinect2_bridge kinect2_bridge,launch

# realsesnse D414/435
roslaunch realsense2_camera rs_camera.launch

• 2、启动find_object_3d识别：

<launch>
	<!-- Example finding 3D poses of the objects detected -->
	<!-- $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true -->

	<!-- Which image resolution: sd, qhd, hd -->
	<arg name="resolution" default="qhd" />
	
	<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
		<param name="gui" value="true" type="bool"/>
		<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
		<param name="subscribe_depth" value="true" type="bool"/>
		<param name="objects_path" value="" type="str"/>
		<param name="object_prefix" value="object" type="str"/>
		
		<remap from="rgb/image_rect_color" to="/kinect2/$(arg resolution)/image_color_rect"/>
		<remap from="depth_registered/image_raw" to="/kinect2/$(arg resolution)/image_depth_rect"/>
		<remap from="depth_registered/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
	</node>
</launch>

---

3、具体使用

不管是启动2d、3d哪个节点，都会有一个QT的可视化界面。

3.1 调整参数，选择特征算子及算法

可以选择SURF、SIFT、ORB、FAST等一系列算法，观察目标物体上的特征点数量及效果，以及处理的流畅程度，衡量选择合适的算法。通过菜单View - Parameters调出参数栏选项卡进行调整。
如图：

3.2 选择匹配的对象，进行匹配

在左侧栏右键选择添加新的目标，通过框选方式选择目标。

之后就可以看到目标物体被成功框选了。

3.3 查阅相关的话题

3.3.1 节点接收的话题

//find_object_2d节点使用
topic：image (message_type: sensor_msgs/Image) 

//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)//深度图像

3.3.2 节点发布的话题

topic：objects (message_type: std_msgs/Float32MultiArray)
//话题内容按照以下方式排列 [objectId1, objectWidth, objectHeight, h11, h12, h13, h21, h22, h23, h31, h32, h33, objectId2...] 
 //hxx 是 3x3 矩阵 (h31 = dx and h32 = dy, see QTransform)

topic：objectsStamped (message_type: find_object_2d/ObjectsStamped)
//带标签的对象物体

• 注意：此外如果我们是使用find_object_3d，可以打开rviz可视化工具，看到目标的TF坐标已经被发出来了（这里需要正确的配置相机的TF坐标系）。
• 话题 /objects 的h矩阵并不利于理解，这里官方还提供了另外一个节点供我们借鉴：
  rosrun find_object_2d print_objects_detected image:=/camera/color/image_raw
• 观察该节点代码可以看到此节点会处理h矩阵，输出目标id及边框的坐标。源码看这里

此节点的输出：
Object 3 detected, Qt corners at (786.949707,-11.180605) (1497.147252,-69.618191) (813.221906,428.768064) (1425.264906,463.355441)

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());

此外此节点还发布了目标中心坐标的图像，可以通过rqt_image_view查看 /image_with_objects。

3.3 保存匹配的模板及配置文件

点击左上角的File，保存匹配模板及配置，并放置到自己对应的目录下,如我都存到config文件夹下，使用launch文件正确的读取他们。

<launch>
	<!-- My finding 3D poses of the objects detected -->
	<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
		<param name="gui" value="true" type="bool"/>

		<param name="settings_path" value="~/dhrobot_ws/src/dhrobot_demo/config/find_object_2d.ini" type="str"/>
		<param name="subscribe_depth" value="true" type="bool"/>
		<param name="objects_path" value="~/dhrobot_ws/src/dhrobot_demo/config" type="str"/>
		<param name="object_prefix" value="object" type="str"/>

		<remap from="rgb/image_rect_color" to="/camera/color/image_raw"/>
		<remap from="depth_registered/image_raw" to="/camera/depth/image_rect_raw"/>
		<remap from="depth_registered/camera_info" to="/camera/depth/camera_info"/>
	</node>
</launch>

4、将识别的坐标点发送给机械臂

在使用find_object_3d时，我们可以直接获得目标物体的tf坐标，因此可以使用ros自带的tf转换直接查询机械臂基座标到物体的tf关系，并发送给机械臂：
参考这里:http://docs.ros.org/lunar/api/tf/html/c++/classtf_1_1TransformListener.html
http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29

tf::StampedTransform transform;
  25     try{
  26       listener.lookupTransform("/arm_base_link", "/object_1",  
  27                                ros::Time(0), transform);
  28     }
  29     catch (tf::TransformException ex){
  30       ROS_ERROR("%s",ex.what());
  31       ros::Duration(1.0).sleep();
  32     }
  33 

５、功能包效果评价

• 整体来说这个功能包安装是相当方便的，不过因为是模板匹配，所以局限性比较大，在目标物体未正对摄像头的情况下识别成功率不高，而且想要得到良好的匹配效果需要认真的调节参数，旋转合适的算法，另外输出的tf中心点坐标大致准确，但是姿态就不是很靠谱了。