代码解析参考

LineMod算法已经是比较成熟的算法，在Ubuntu里直接作为一个算法包嵌入进去，可以直接下载用。

运行环境：Ubuntu16.04+ROS Kinetic+PCL

http://campar.in.tum.de/pub/hinterstoisser2011linemod/hinterstoisser2011linemod.pdf

https://blog.csdn.net/zmdsjtu/article/details/79933822

https://blog.csdn.net/zmdsjtu/article/details/79933822

https://blog.csdn.net/haithink/article/details/88396670

这个是LineMod的算法总结，也可以辅助来看Patch_lineMod的源代码，其中有两篇是源码级带着看代码，对于理解代码还不错。

前两篇已经从原理、公式、图示、算法优缺点等探讨了LineMod算法，本篇主要是从工程角度，回顾一下怎么复现LineMod算法以及得到可乐罐的6d位姿。

1.参考

  我最开始采用PCL进行目标物体检测分割，后面发现了LineMod算法而且是集成在Ubuntu系统里面当做一个模块的，使用很方便。本篇主要是使用Intel ZR300深度相机通过采集到的电云数据识别物体标签、6D位姿，并通过TF坐标转换等把6D位姿送到机械臂进行抓取。原始LineMod模块直接安装即可，可乐罐需要训练一下，另外可以自己添加3D模型，按照第一二篇提到的如AutoCAD自己做或者直接有现成的网站可以下物体的模板。

运行环境：Ubuntu16.04+ROS Kinetic+PCL+ORK
参考这两篇文章，虽然是针对Kinetic或者Xbox不完全适用，但是只需要小部分代码更改，配置一下就可以用了。
http://wg-perception.github.io/ork_tutorials/tutorial03/tutorial.html#setup-the-working-environment(ORK官网 这里有完整详细的步骤)

2.复现

 1.配置ZR300深度相机
  下载realsense适用Intel ZR300深度相机的例程包（http://wiki.ros.org/realsense_camera），官网下载并安装，选择branch(indigo-devel)。作为替代openni2(适用Kinetic)的启动代码，可以得到实时图像的RGBD图像并在rviz显示，这里的-b （分支名）要添加，不然下载下来的是realsense2_camera 新版本的在kinetic编译不通。(注意ZR300要接USB3.0接口！血淋淋的教训0.0)
#git clone -b indigo-devel https://github.com/intel-ros/realsense.git
#roslaunch realsense_camera zr300_nodelet_rgbd.launch
#rviz

  这里补充一句，SR300 ZR300等都是Intel Realsense的深度相机，只不过他们的景深不一样，识别范围精度和距离不一样。并不是优劣，而是各自的擅长地方不一样，所以需要根据具体的任务确定适合使用哪种相机。

---------------------------------------------------------------------

关于如何使用和配置zr300深度相机:

（1）在Intel官网就有安装该款相机的教程。https://software.intel.com/zh-cn/realsense/zr300/get-started

（2）设定时间（可设可不设）建议用网络时间设置
（3）设定储存库，将英特尔® 实感™ 软件储存库添加至 Ubuntu* 设备的 package manager（程序包管理器）

$ echo 'deb "http://realsense-alm-public.s3.amazonaws.com/apt-repo" xenial main' | sudo tee /etc/apt/sources.list.d/realsense-latest.list
$ sudo apt-key adv --keyserver keys.gnupg.net --recv-key D6FB2970
$ sudo apt update

（4）安装英特尔® 实感™ 摄像头访问实用程序，获得高级摄像头控制，如反差和曝光设置。

$ sudo apt install librealsense-utils
$ cpp-capture

注意ZR300要插在USB3.0上

（5）安装运行时和样本，此安装允许运行所有 librealsense 中间件样本。

安装运行时后，就应能看到摄像头输出，并可运行英特尔® 实感™ 中间件样本，例如人体跟踪、对象库及同时定位和地图绘制 (SLAM)。

(6)安装开发工具，此步骤安装物体识别、人体跟踪和 SLAM 库至设备。

$ sudo apt install librealsense-samples beignet-opencl-icd

$ sudo apt install build-essentials git cmake libjpeg-dev libopencv-dev
$ git clone http://github.com/IntelRealSense/realsense_samples
$ cd realsense_samples
$ mkdir build
$ cd build
$ cmake
$ make

(7)$cd catkin_ws/进入该文件夹进行cmake,$catkin_make。

关于这部分没有写很细，一些bug的解决方法可以参考在csdn上的一篇(109条消息) IntelRealSense ZR300 For ROS相关 系列第一篇_羊羊羊机器人的博客-CSDN博客

------------------------------------------------------------------------------------------------------------------------------------

这里注意另一个点，ros下运行的包要装到~/catkin_ws/src目录下编译运行。

这里的话题名为：depth_registered

发布的RGBD信息为：

Rgb_frame_id:’camera_rgb_frame’

Rgb_image_topic:’/camera/rgb/image_rect_color’

Rgb_camera_info:’/camera/rgb/camera_info’

Depth_frame_id:’camera_depth_info’

Depth_image_topic:’/camera/depth_registered/sw_registered/image_rect’

Depth_camera_info:’/camera/depth_registered/sw_registered/camera_info’

注意类似这样的错误不能正常启动的话可以重新插拔realsense摄像头，或者按下面所说的在rqt_reconfigure重新打钩enable就可以了。

#rosrun rqt_reconfigure rqt_reconfigure
这行代码即可查看运行状态，重新打钩刷新等。
注意：在测试中要经常#sudo apt-get update 以及 #rospack profile，前者更新包以避免不必要的麻烦，后者是ros有时候会找不到文件的位置导致无法tap。（参考：https://answers.ros.org/question/281477/ioerror-errno-13-permission-denied-rosrosdepsourcescacheindex/）

2.安装LineMod模块

#git clone http://github.com/wg-perception/object_recognition_msgs
#git clone http://github.com/wg-perception/object_recognition_ros
#git clone http://github.com/wg-perception/object_recognition_ros_visualization
#cd ../ && catkin_make
#git clone http://github.com/wg-perception/object_recognition_core
#git clone http://github.com/wg-perception/linemod
#git clone http://github.com/wg-perception/ork_renderer
#cd ../ && catkin_make

如果之前有配置过Ros，在src目录直接#cm即可。在这个过程汇总可以挨个下载后即可编译，解决了出现的问题后再下载其他的继续编译，以此发现其中哪个包存在问题。

碰到的某bug:

@fatal error: GL/osmesa.h: 沒有此一檔案或目錄#include <GL/osmesa.h>。

解决：#sudo apt-get install libosmesa6-dev 安装缺少的GL/osmesa.h


3.修改配置文件

 3.1查找需要改的节点信息

之前参考的文章所使用的例子的环境是kinetic深度相机\baxter，所以在realsenseZR300需要重新配置文件，需找到realsense_camera摄像头驱动包对应发布的话题名。这里有个查找方法，即将limemod涉及到的所有依赖包加到sublime，就可以大批量直接寻找要找的含旧话题名的文件。

#rosnode cleanup命令可清除无关此次项目开发的节点

查看节点信息、图节点信息:
#rosnode info /object_recognition_server

查看realsense给的例子需要订阅的主题为：
Subscriptions:
    * /camera/depth_registered/camera_info [unknown type]
    * /camera/depth_registered/image_raw [unknown type]
    * /camera/rgb/camera_info [sensor_msgs/CameraInfo]
    * /camera/rgb/image_rect_color [sensor_msgs/Image]

查看相关主题:
#rostopic echo /camera/depth_registered/sw_registered/camera_info
#rostopic echo /camera/depth_registered/image_raw
#rostopic echo /camera/rgb/camera_info
#rostopic echo /camera/rgb/image_rect_color

利用上面的命令即可查看例程订阅的数据(前提已roslauch了realsense_camera zr300_nodelet_rgb)，可以看到如果例子realsense跑起来，要用到的这四个话题是否有数据，是否成功发布。用#rqt_graph 用节点图形的方式来查看运行的节点信息。

3.2让detection程序（也就是linemod例程）订阅深度相机realsense发布的话题。

将用到的包linemod、ros、ork、core、msgs等（新安装的包）拖入sublime中便于编辑。搜索linemod订阅的的四个话题：（这里之前zr300_nodelet_rgbd.launch节点发布的话题已经修改并注释掉了）

修改位于linemod\conf\目录下的detection.ros.ork文件

 在~/catkin_ws/src/linemod/conf目录下的detection

      # rgb_frame_id: 'camera_rgb_optical_frame'
     # rgb_image_topic: '/camera/rgb/image_rect_color'
     # rgb_camera_info: '/camera/rgb/camera_info'
     # depth_frame_id: 'camera_depth_optical_frame'
     # depth_image_topic: '/camera/depth_registered/image_raw'
     # depth_camera_info: '/camera/depth_registered/camera_info'

  修改为：
     #rgb_frame_id: 'camera_rgb_frame'
     #rgb_image_topic: '/camera/rgb/image_rect_color'
     #rgb_camera_info: '/camera/rgb/camera_info'
     #depth_frame_id: 'camera_depth_frame'
     #depth_image_topic: '/camera/depth_registered/sw_registered/image_rect'
     #depth_camera_info: '/camera/depth_registered/sw_registered/camera_info'
   这样linemod模块就可以找到rgb图像的数据以及D的数据，即可以处理RGBD三维图像数据。

3.3 导入可乐罐模型并训练

安装CouchDB（用来建立物体辨识的工具），这里ORK tutorials已提供可乐罐3D模型（coke.stl），接着上传物体及其mesh文件。在自己的资料库可查看是否新增物体，记录id（训练其他物体模型同理）。

这部分的代码：
#sudo apt-get install couchdb

#git clone https://github.com/wg-perception/ork_tutorials
#cm  (在src目录下cmake)

#rosrun object_recognition_core object_add.py -n "coke " -d "A universal can of coke"

（http://localhost:5984/_utils/database.html?object_recognition/_design/objects/_view/by_object_name）

#rosrun object_recognition_core mesh_add.py <YOUR_OBJECT_ID> <path to ork_tutorials/data/coke.stl>
实际为：
#rosrun object_recognition_core mesh_add.py 0be612246c9b0a00baaa4adefb0009eb /home/yyang/catkin_ws/src/ork_tutorials/data/coke.stl --commit
（PS: 若rosrun object_recognition_core object_add.py -n这条命令添加不了可在命令后加--commit）

添加后如图所示在浏览器可看到自己添加的模型信息。

---------------------------------------------------------

关于添加模型：

注意添加模型要保证模板是完整的，另外就是要注意文件大小，和系统提供的coke.stl同计算量度即可。如果文件太大训练时会卡死，而且也识别不出来物体。

关于删除模型：

在http://localhost:5984/_utils/database.html?object_recognition/_design%2Fobjects%2F_view%2Fby_object_name 有delete database直接删除对应的物体模型再重新训练就可以了。

#rosrun object_recognition_core training -c `rospack find object_recognition_linemod`/conf/training.ork

3.4 训练模型并测试

#rosrun object_recognition_core training -c `rospack find object_recognition_linemod`/conf/training.ork

训练完即可进行开始试验测试detection。

#roslaunch realsense_camera zr300_nodelet_rgbd.launch
（如果之前已经launch过就不需要了）
#rosrun object_recognition_core detection -c `rospack find object_recognition_linemod`/conf/detection.ros.ork
#rviz

可乐罐对应的模型有一些偏置,实际场景中6D位姿是正确的，只是白色模型没有和物体重合。运行成功检测到物体则会输出：

PS: 若出现不了点云图或者运行过程中没有了点云图只有罐体以及坐标就：

#rosrun rqt_reconfigure rqt_reconfigure在下拉菜单中的driver中重新点击启动enable_depth（打钩）。

3.5 获取6D位姿信息

#rostopic list
可以看到ork发布的话题信息/recognized_object_array
#rostopic echo /recognized_object_array
即可实时查看位置信息
#rostopic type /recognized_object_array
使用type来查看发布信息的类型，方便以后listener订阅talker,处理该信息发送给机械臂处理执行抓取任务。

这里我们知道物体位置在的数组，下一步就是把他抠出来发消息给机械臂就可以进行下一步的机械臂抓取部分了。