Ubuntu16.04下用kinect v2实现RTABMAP-ROS建图
Ubuntu16.04下用kinect v2实现RTABMAP-ROS建图
本人最近刚接触到Visual SLAM的知识，作为初学者，看了很久高翔《视觉SLAM十四讲》，理论暂时没有完全搞明白。
近期再尝试使用kinect v2进行RTABMAP建图，网上查找了很多资料，发现对于SLAM大部分的内容都是在ROS下进行，于是我也在其中进行尝试，期间遇到很多问题，现在终于跑通，特来记录，以供我这样的小白日后方便查看。
一、前期准备
1.安装ROS环境：https://blog.csdn.net/jianzhuan2824/article/details/89098865
2.安装kinect v2驱动以及ROS下运行：https://blog.csdn.net/jianzhuan2824/article/details/89242345
二、安装rtabmap-ros包
1.参考文章
官网：http://introlab.github.io/rtabmap/
安装 教程：
github外文参考文章：https://github.com/introlab/rtabmap_ros#installation
网上查找到的自认为最好的一篇安装教程：https://www.ncnynl.com/archives/201709/1991.html
2.安装
安装方法总共分两种， 一种是直接deb安装，一种是源码安装。
源码安装详见上边的例程。本人亲自尝试了下，发现在编译RTABMAP时会出现 /usr/bin/ls 找不到lBoost::timer 的错误，为了解决这个错误，我花费了2个周时间，最后毫无收获。
所以这里不建议大家去折腾源码安装，费力不讨好。
我也尝试在~/catkin_ws/src下复制源码rtabmap_ros，然后到 ~/catkin_ws下进行catkin_make，但是还是编译出错，这种方法也放弃了。
其实，github上说的很明确， rtabmap已经集成到ROS下了，
所以只需要一步，简单粗暴：（注意ROS版本就行）
sudo apt-get install ros-kinetic-rtabmap-ros
三、运行测试
网上好多测试方法去启动深度相机，有直接rtabmap的，但是我总是显示连接不到设备，原因是深度相机那里选择不了kinect v2，所以识别不了。这个问题应该是可以解决的，毕竟选项里明明有kinect v2，只是无法选择，但暂未解决。
欢迎各路大神给予方案建议。
1.参考文章
参考外文：http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
CSDN博客：
https://blog.csdn.net/xmy306538517/article/details/78771018
https://blog.csdn.net/sean_xyz/article/details/65445038
2. kinect v2 准备
①运行指令：
roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
这时，kinect2的红外灯会亮一会然后熄灭， 白灯也会过一段时间熄灭。
出现打印 INFO] [1478769022.036459690]: [Kinect2Bridge::main] waiting for clients to connect，表示kinect2启动成功
如果遇到下面的问题：
请执行：
source ~/catkin_ws/devel/setup.bash
②查看相关节点，与测试无关，但有助于后边指令的理解
查看kinect2启动后的topic ：
rostopic list
出现结果：
/kinect2/bond
/kinect2/hd/camera_info
/kinect2/hd/image_color
/kinect2/hd/image_color/compressed
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_depth_rect
/kinect2/hd/image_depth_rect/compressed
/kinect2/hd/image_mono
/kinect2/hd/image_mono/compressed
/kinect2/hd/image_mono_rect
/kinect2/hd/image_mono_rect/compressed
/kinect2/hd/points
/kinect2/qhd/camera_info
/kinect2/qhd/image_color
/kinect2/qhd/image_color/compressed
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_depth_rect
/kinect2/qhd/image_depth_rect/compressed
/kinect2/qhd/image_mono
/kinect2/qhd/image_mono/compressed
/kinect2/qhd/image_mono_rect
/kinect2/qhd/image_mono_rect/compressed
/kinect2/qhd/points
/kinect2/sd/camera_info
/kinect2/sd/image_color_rect
/kinect2/sd/image_color_rect/compressed
/kinect2/sd/image_depth
/kinect2/sd/image_depth/compressed
/kinect2/sd/image_depth_rect
/kinect2/sd/image_depth_rect/compressed
/kinect2/sd/image_ir
/kinect2/sd/image_ir/compressed
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/points
/rosout
/rosout_agg
其中，hd 表示1080P(1920x1080)，qhd表示hd的四分之一大小 960x540，
sd 大小应该是640x480, 实际大小是 height: 424 width: 512
可以看到其中主要的是hd,qhd,sd三类，每一类都包含相机信息、彩色、深度、红外等数据，再加上最后的输出。
使用对应工具去 查看感兴趣的topic， 可以使用image_view查看，例如：
rosrun image_view image_view image:=/kinect2/qhd/image_color
也可以使用kinect2_view查看深度图（12位，image_view看不了）。
3.建图模式
在线可视化有两种选择：rtabmapviz或rviz。
使用RVIZ插件进行建图，点云可以逐步添加到RVIZ。
但是使用rtabmapviz，界面应该看起来像独立版本的教程。
使用下面的启动文件rtabmap.launch，设置rviz：= true以打开rviz和rtabmapviz：= true以打开rtabmapviz（默认为true）以进行可视化。
用默认的rtabmapviz进行建图
①执行官方给的指令tf
rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link kinect2_link 100
该指令主要是前半部分涉及静态转换（本人猜测是坐标转换或者标定，没有细查），后半部分涉及传感器选择的帧来源。
该指令默认的是qhd，即默认960*540.其对应的帧是kinect2_rgb_optical_frame，这一点可以不用管它。
如果使用的是hd, 那么对应的frame也是kinect2_rgb_optical_frame，hd和qhd的尺寸图像都是基于rgd镜头的，所以它们的frame是彩色镜头的frame。
如果使用的是sd, 那么对应的frame是kinect2_ir_optical_frame, 红外镜头的frame。
比如上面指令可以改成
rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link kinect2_rgb_optical_frame 100
②启动rtabmap_ros_
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rgb_topic:=/kinect2/qhd/image_color_rect depth_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info
指令解析：
roslaunch 启动文件 ，其操作对象是.launch文件，
所以第一句话就是启动rtabmap_ros文件的意思
rtabmap_args:="–delete_db_on_start" 是说启动前先删除数据库db下的数据。
rgb_topic:=/kinect2/qhd/image_color_rect depth_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info
topic是ROS系统里的架构，这一步就是指定rgb彩色数据、depth深度数据、相机参数信息分别对应好哪个topic。这也是为什么之前会列出kinect v2的所有topic节点。
这样，大家就可以根据自己喜好去选择hd,qhd or sd的数据源，然后写出相应的建图指令。
rtabmap.launch参数解析
来自：https://blog.csdn.net/xmy306538517/article/details/78771018
终端指令小结
一般使用默认情况下的qhd即可：
建立连接：
roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
静态tf：
rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link kinect2_link 100
启动文件：
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rgb_topic:=/kinect2/qhd/image_color_rect depth_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info
地图数据保存在 ~/.ros/rtabmap.db
可利databaseViewer工具查看数据库
rtabmap-databaseViewer ~/.ros/rtabmap.db
注：
①在三维点云地图中右键取消Z轴锁定，即可以上帝视角查看三维模型
②indigo版本和kinect版本之间的mapdata格式存在差异，不同版本之间发布和订阅会出现无tf输出的情况（没明白）
原地转2圈，手持深度相机进行测试，结果图如下：
用rviz建图
在启动.launch文件的指令后边添加一句设置即可
rviz：= true
进去后可以有很多参数进行设置，可自行玩耍