安装rtabmap_ros包

一、前言

RTAB-Map (Real-Time Appearance-Based Mapping)是一种基于全局贝叶斯闭环检测的RGB-D Graph SLAM方法。它可以用kinect的深度信息结合kinect变换得到的激光数据进行即时定位与建图（gmapping算法志只用到了kinect转换得到的激光数据，而把深度信息丢弃了）。
官网地址：http://introlab.github.io/rtabmap/
ROS下面安装请看：https://github.com/introlab/rtabmap_ros#rtabmap_ros
教程请看：https://github.com/introlab/rtabmap/wiki/Tutorials
在机器人上使用RTAB-Map：Setup RTAB-Map on Your Robot!

二、deb方式安装

嫌麻烦采用deb方式安装

Jade:   $ sudo apt-get install ros-jade-rtabmap-ros Indigo: $ sudo apt-get install ros-indigo-rtabmap-ros Hydro:  $ sudo apt-get install ros-hydro-rtabmap-ros 
1
2
3

三、源码安装

请看github上的安装说明：https://github.com/introlab/rtabmap_ros#rtabmap_ros
简单的安装方法是：
indigo:

$ sudo apt-get install libsqlite3-dev libpcl-1.7-all libfreenect-dev libopencv-dev 
1

hydro:

$ sudo apt-get install libsqlite3-dev libpcl-1.7-all ros-hydro-libfreenect ros-hydro-opencv2
1

然后下载RTAB-Map源码编译安装：

$ git clone https://github.com/introlab/rtabmap.git rtabmap$ cd rtabmap/build$ cmake ..$ make -j4$ make install
1
2
3
4
5
6

安装完之后插上Kinect在终端运行命令：

$ rtabmap
1

出现GUI，然后选择“File”——“New database”，再点击开始按键就可以出现图像了

在catkin工作空间中安装RTAB-Map ros-pkg

$ cd ~/catkin_ws$ git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros$ catkin_make
1
2
3

1、安装可选的依赖项

（1）如果你需要用到SURF/SIFT那就装一下opencv吧
请看：http://blog.csdn.net/u013453604/article/details/49781771
由于cv-bridge依赖的是opencv2，所以装opencv3的话rtabmap_ros里面有些库会冲突，这里建议装opencv 2.4.11。
（2）安装g2o，这是在闭环检测中实现图优化的包
先安装依赖项

$ sudo apt-get install cmake libeigen3-dev libsuitesparse-dev
1

下载源码编译安装

$ cd $ git clone https://github.com/RainerKuemmerle/g2o.git$ cd g2o/$ mkdir build$ cd build/$ cmake ../$ make
1
2
3
4
5
6
7

安装完之后库文件在/usr/local/lib，头文件在/usr/local/include/g2o
（3）安装其他依赖项Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge

$ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-22-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-rviz ros-indigo-cv-bridge
1

如果按照github上的命令安装依赖项不成功就运行上述命令吧，其中opencv和g2o我是源码安装的，所以相对于github上的命令我这里把ros-indigo-g2o去掉了，把libdc1394-dev改成了libdc1394-22-dev
（4）安装GTSAM

GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices.
1

把gtsam包下载到某处进行编译安装:

$ cd software/$ git clone https://bitbucket.org/gtborg/gtsam.git$ cd gtsam$ mkdir build$ cd build$ cmake ..$ make check$ make install
1
2
3
4
5
6
7
8

（5）安装cvsba

cvsba is an OpenCV wrapper for the well-known Sparse Bundle Adjustment library (sba) by M. Lourakis.
1

安装依赖项

$ sudo apt-get install liblapack-dev libf2c2-dev 
1

下载cvsba：http://sourceforge.net/projects/cvsba/files/latest/download?source=typ_redirect
解压得到cvsba-1.0.0文件夹，我把它放到～/software/文件夹下

$ cd ~/software/cvsba-1.0.0$ mkdir build$ cd build$ cmake ..$ make$ sudo make install
1
2
3
4
5
6

安装完之后还要执行以下命令才可以让rtabmap_ros找到它

$ sudo mkdir /usr/local/lib/cmake/cvsba $ sudo mv /usr/local/lib/cmake/Findcvsba.cmake /usr/local/lib/cmake/cvsba/cvsbaConfig.cmake
1
2

（6）安装Freenect2
请看这里：https://github.com/OpenKinect/libfreenect2#debianubuntu-1404-perhaps-earlier

Driver for Kinect for Windows v2 (K4W2) devices (release and developer preview).Note: libfreenect2 does not do anything for either Kinect for Windows v1 or Kinect for Xbox 360 sensors. Use libfreenect1 for those sensors.This driver supports:RGB image transferIR and depth image transferregistration of RGB and depth images
1
2
3
4
5
6
7
8
9
10

先下载Freenect2源代码

$ cd ~/software$ git clone https://github.com/OpenKinect/libfreenect2.git
1
2

安装依赖项

$ sudo apt-get install build-essential cmake pkg-config libturbojpeg libjpeg-turbo8-dev mesa-common-dev freeglut3-dev libxrandr-dev libxi-dev
1

安装libusb

$ sudo apt-get install libusb-1.0-0-dev
1

注意libusb版本要大于1.0.20，否则编译freenect2的时候会报错：

CMake Error: The following variables are used in this project, but they are set to NOTFOUND.Please set them or make sure they are set and tested correctly in the CMake files:LibUSB_LIBRARYlinked by target "freenect2" in directory /home/siat/libfreenect2
1
2
3
4

如果libusb版本太低可以先卸载libusb-1.0-0-dev更新软件源重装libusb-1.0-0-dev，运行如下命令

$ sudo apt-get remove libusb-1.0-0-dev$ sudo apt-add-repository ppa:floe/libusb $ sudo apt-get update $ sudo apt-get upgrade$ sudo apt-get install libusb-1.0-0-dev
1
2
3
4
5

安装 GLFW3

$ sudo apt-get install libglfw3-dev
1

如果上面命令无效请使用下面命令：

$ cd libfreenect2/depends$ sh install_ubuntu.sh$ sudo dpkg -i libglfw3*_3.0.4-1_*.deb
1
2
3

安装OPENCL依赖

对于AMD GPU：$ apt-get install opencl-headers对于Nvidia GPU：（前提是你已经安装Nvidia驱动）$ apt-get install opencl-headers对于Intel GPU：$ apt-get install beignet-dev不行的话运行：$ sudo apt-add-repository ppa:pmjdebruijn/beignet-testing
1
2
3
4
5
6
7
8
9
10

编译Freenect2

$ mkdir build && cd build$ cmake ..$ make$ sudo make install # without sudo if cmake -DCMAKE_INSTALL_PREFIX=$HOME/...
1
2
3
4

运行测试

$ ./bin/Protonect
1

2、下载安装RTAB-Map

$ cd ~$ git clone https://github.com/introlab/rtabmap.git rtabmap$ cd rtabmap/build$ cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel ..$ make -j4$ make install
1
2
3
4
5
6

3、在catkin工作空间中安装RTAB-Map ros-pkg

$ cd ~/catkin_ws$ git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros$ catkin_make
1
2
3

4、更新版本

$ cd rtabmap$ git pull origin master$ cd build$ make$ make install$ roscd rtabmap_ros$ git pull origin master$ cd ~/catkin_ws$ catkin_make
1
2
3
4
5
6
7
8
9
10

四、遇到的报错

编译时遇到如下报错，最后发现是关于openni的库有问题，因为我用源码装过openni驱动，有可能是版本冲突了，其实根本没必要从源码安装，可以从其他电脑上把usr/include下面的ni文件夹拷过来，我是重装了Ubuntu和ros indigo，然后按照前面简单的安装方法安装的，过程中自动安装了openni

siat@ThinkPad-siat:~/rtabmap/build$ make -j4[  7%] Built target rtabmap_utilite[  7%] Built target uresourcegenerator[  8%] Built target imagesJoiner[  9%] Built target extractObject[ 10%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/CameraThread.cpp.o[ 10%] [ 11%] [ 12%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/CameraRGBD.cpp.oBuilding CXX object corelib/src/CMakeFiles/rtabmap_core.dir/VisualWord.cpp.oBuilding CXX object corelib/src/CMakeFiles/rtabmap_core.dir/EpipolarGeometry.cpp.oIn file included from /usr/include/ni/XnOS.h:27:0,                 from /usr/include/pcl-1.7/pcl/io/openni_camera/openni.h:47,                 from /usr/include/pcl-1.7/pcl/io/openni_camera/openni_depth_image.h:44,                 from /home/siat/rtabmap/corelib/src/../include/rtabmap/core/CameraRGBD.h:37,                 from /home/siat/rtabmap/corelib/src/CameraRGBD.cpp:28:/usr/include/ni/XnPlatform.h:73:3: error: #error OpenNI Platform Abstraction Layer - Unsupported Platform!  #error OpenNI Platform Abstraction Layer - Unsupported Platform!   ^...In file included from /usr/include/pcl-1.7/pcl/io/openni_camera/openni.h:50:0,                 from /usr/include/pcl-1.7/pcl/io/openni_camera/openni_depth_image.h:44,                 from /home/siat/rtabmap/corelib/src/../include/rtabmap/core/CameraRGBD.h:37,                 from /home/siat/rtabmap/corelib/src/CameraThread.cpp:31:/usr/include/ni/XnCppWrapper.h:10045:76: error: macro "XN_VALIDATE_NEW" passed 4 arguments, but takes just 2    XN_VALIDATE_NEW(pTrans, StateChangedCallbackTranslator, handler, pCookie);                                                                            ^/usr/include/ni/XnCppWrapper.h:10104:75: error: macro "XN_VALIDATE_NEW" passed 4 arguments, but takes just 2   XN_VALIDATE_NEW(pTrans, StateChangedCallbackTranslator, handler, pCookie);                                                                           ^[ 13%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/VWDictionary.cpp.oIn file included from /usr/include/ni/XnStatusCodes.h:27:0,                 from /usr/include/ni/XnMacros.h:27,                 from /usr/include/ni/XnOS.h:28,                 from /usr/include/pcl-1.7/pcl/io/openni_camera/openni.h:47,                 from /usr/include/pcl-1.7/pcl/io/openni_camera/openni_depth_image.h:44,                 from /home/siat/rtabmap/corelib/src/../include/rtabmap/core/CameraRGBD.h:37,                 from /home/siat/rtabmap/corelib/src/CameraRGBD.cpp:28:/usr/include/ni/XnStatus.h:33:9: error: ‘XnUInt32’ does not name a type typedef XnUInt32 XnStatus;         ^
rtabmap + kinect v1
使用 kinect 跑 rtabmap 的方法十分简单，启动两个 launch 就可以了。
$ roslaunch freenect_launch freenect.launch depth_registration:=true$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start"
第一个是启动 kinect 的驱动程序，由 freenect_launch 包提供。
第二个就是启动 rtabmap_ros 中的 rgbd_mapping，后面的参数的含义是删除掉之前 储存的 db 文件，重新建图。
启动之后，如果没有错误的话就可以在 rtabmap 定制化的界面中看到生成的三维地图了。
rtabmap + stereo camera
双目相机比kinect稍微麻烦一点。因为摄像头采集图像时是按照opencv的坐标系来的，如果我们正对图像，图像的左上角为原点，图像矩形上边界为X轴，正右方为X轴正向，左边界为Y轴，正下方为Y轴正向，垂直与图像平面为Z轴，方向往外为Z轴正向。而 ROS 的世界坐标系是以机器人的正前方为X轴正向，正左方为Y轴正向，正上方为Z轴正向。所以在使用双目时需要经过一个坐标系变换，即广播这两个坐标系之间的 tf ，这个由 tf 包中的 static_transform_publisher 节点即可实现。
<arg name="pi/2" value="1.5707963267948966" /><arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" /><node pkg="tf" type="static_transform_publisher" name="camera_to_base_link"args="$(arg optical_rotate) laser_link camera_base_link 100" ><remap from="tf" to="tf_static"/></node>
启动这个节点的功能就是产生一个固定的由  camera_base_link 到 handsfree的 laser_link 的 tf 变换。static_transform_publisher 的用法如下：
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
在启动双目相机的 launch 文件中，记得加入 stereo_image_proc 来对图像进行矫正，当然做这些之前还需要标定哦，标定的方法就不在赘述了。
<group ns="/stereo_camera" > <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/></group>
启动相机之后，运行：
$ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rtabmap_args:="--delete_db_on_start"
运行之后就可以看到 rtabmap 的界面了。双目主要的问题在于上面提到的坐标系之间的转换和话题名称的对应，话题名称对应的问题可以使用 rqt_graph 工具来辅助解决。
运行界面以及参数调整
在打开 rtabmap 界面之后，会看到正中间有三维地图，左侧有显示闭环检测，视觉里程计等等的小窗口，这些都是来辅助查看算法运行效果的。建议大家在 windows 选项下试一试所有的窗口。
当你中止 rtabmap 程序时，在退出之前，rtabmap 会自动在 ~/.ros/ 隐藏文件夹下产生 rtabmap.db 文件，用以保存程序数据。这个文件包含的内容很多，不仅有三维地图，还有关键帧，甚至还有闭环检测的信息。而 rtabmap 提供的查看 db 文件的工具 rtabmap-databaseViewer 也是强大的不行。运行：
rtabmap-databaseViewer ~/.ros/rtabmap.db
可以看到以下界面：
这个工具来看算法的运行效果非常给力，拖动下方的进度条可以查看对应帧之间的特征匹配结果，在 view 选项中，还可以查看三维地图，如果有 scan 信息的话还可以查看二维地图，在 info 中还可以看到 Ground truth 、 Total odometry length 等等数据。在 edit 选项中，还可以生成 g2o graph，甚至还能再对该数据集进行更多的闭环检测。
rtabmap 自带工具的话就介绍到这里，功能太多不能一一去给大家讲解。下面来大致讲一下调整参数的方法。
我们可以运行以下命令获取 rtabmap 的参数：
rosrun rtabmap_ros rtabmap --paramsrosrun rtabmap_ros rgbd_odometry --params
参数的名称以及介绍都会打印在终端中，大家可以重定向到文件中以留备份查看。参数需要根据不同的使用环境来设置，这是一个相当痛苦的过程，建议大家先浏览一遍相关的论文，然后再根据wiki上的教程调试。
[论文地址](http://introlab.github.io/rtabmap/)，
[参数调整方法](http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning)，
使用rtabmap + handsfree 进行导航
使用 rtabmap + handsfree 进行导航其实是用 rtabmap 的投影地图或者是scan地图来作为 move_base 的地图输入，然后利用 Navigation Stack 进行路径规划等。
使用 scan 地图
rtabmap 可以接收 Laserscan 话题数据来生成二维地图，这种地图和现有的二维SLAM效果类似，生成的地图比较干净，但是需要生成激光的设备，或者需要将 kinect 水平放置，以产生模拟激光数据。以下是我新建的slam_kinect.launch文件：
<launch>  <!-- transform from /base_link to camera_rgb_optical_frame -->  <arg name="optical_rotate" value="0 0 0 0 0 0" />  <node pkg="tf" type="static_transform_publisher" name="camera_to_base_link"       args="$(arg optical_rotate) base_link camera_link 100" >                   <remap from="tf"   to="tf_static"/>  </node>  <!-- freenect for kinect -->  <include file="$(find freenect_launch)/launch/freenect.launch">    <arg name="depth_registration" value="true"/>  </include>  <!-- freenect for kinect END -->  <!-- rtabmap -->  <include file="$(find SLAM_ROS)/launch/rgbd_mapping.launch">    <arg name="rtabmap_args"      value="--delete_db_on_start"/>    <arg name="subscribe_scan"    value="true"/>    <arg name="frame_id"          value="/base_link"/>    <arg name="visual_odometry"   value="false"/>    <arg name="odom_topic"        value="/mobile_base/mobile_base_controller/odom"/>  </include>  <!-- rtabmap END -->  <!-- scan -->  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet camera/camera_nodelet_manager" output="screen">    <!-- Pixel rows to use to generate the laserscan. For each column, the scan will         return the minimum value for those pixels centered vertically in the image. -->    <param name="scan_height" value="10"/>    <param name="output_frame_id" value="/base_link"/>    <param name="range_min" value="0.45"/>    <remap from="image" to="/camera/depth_registered/image_raw"/>    <remap from="scan" to="/scan"/>    <remap from="camera/image" to="/camera/depth_registered/image_raw"/>    <remap from="camera/scan" to="/scan"/>  </node>  <!-- scan END --></launch>
在上面的文件中，先启动 freenect.launch，再启动 rtabmap_ros 的 rgbd_mapping.launch。注意，这里需要将 subscaribe_scan 设置为 true，表示接收 scan 数据。然后就是启动 depthimage_to_laserscan 这个 nodelet 了，这个 nodelet 是用于从深度数据中模拟激光数据，这个想必大家已经很熟悉了。
另外，我们将 visual_odometry 设置为 false，表示不用视觉里程计然后将 odom_topic 设置为 handsfree 的底盘里程计话题，即使用底盘的 odometry 数据，这样可以省下不少计算量，毕竟在室内二维环境中，编码器是最好的选择。
然后，我们需要将 handsfree 的 move_base.xml 文件中 move_base 节点中的 map 话题重映射为 /rtabmap/grid_map 以便使用 rtabmap 计算出的二维地图作为输入。
这里可能还会有两个相同的问题，一个是 tf 的问题，另外一个是话题名称对应的问题。tf 的问题一般是由于 frame_id 这种参数没有设置正确，一般的 tf tree 是类似于 map -> odom -> base_link -> laser_link(camera_link) 的结构。大家可以用 rviz 中的 tf 选项来查看，也可以使用命令：
rosrun tf view_frames
这条命令会监听 5s 的 tf 信息，并会自动在 /home 文件夹下保存 tf tree 为 pdf 文件。
做完了这些，打开你的 rviz，打开默认的也没关系，一个一个添加工具也不是很困难。这里需要添加的组件如下：
1、添加两个 Map ，一个用于显示全局地图，一个用于显示局部地图。
       全局地图的话题选择 /rtabmap/grid_map，局部地图的话题选择 /move_base_node/local_costmap/costmap 。
2、添加两个 Path ，一个用于显示全局路径，一个用于显示局部路径。
      全局路径的话题选择 /move_base_node/TrajectoryPlannerROS/global_plan ，局部路径的话题选择 /move_base_node/TrajectoryPlannerROS/local_plan。
3、添加 LaserScan，用于显示生成的模拟激光，话题选择你的 laserscan 话题名称。
4、可选，如果你想查看三维地图，可以添加 PointCloud2 ，然后话题名称选择 /rtabmap/cloud_map。
接下来的导航就是轻车熟路了，选择 2D Nav Goal 指定目标点，然后就可以进行导航了。
使用投影地图
rtabmap 除了可以用 scan 来生成二维地图以外，还可以使用 rtabmap 提供的一个 nodelet —— obstacles_detection 来计算二维投影地图。
它的原理是将三维点云直接投影到平面上，可以通过设定 ground_normal_angle 参数来区分地面和障碍物。使用这种方法的好处是不需要激光数据，但是需要将 kinect 向下倾斜一点角度，保证kinect能够照到地面。
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="standalone rtabmap_ros/obstacles_detection" output="screen">  <remap from="cloud" to="/camera/depth_registered/points"/>  <remap from="obstacles" to="/planner_cloud"/>  <param name="frame_id" type="string" value="base_link"/>  <param name="map_frame_id" type="string" value="map"/>  <param name="min_cluster_size" type="int" value="20"/>  <param name="max_obstacles_height" type="double" value="0.0"/></node>
在 obstacles_detection 这个 nodelet 中，接收名为 cloud  的点云数据，生成名为 ground  的地面点云和名为 obstacles  的障碍物点云。
在之前的 launch 文件中将 rtabmap 中的参数 subscaribe_scan 设置为 false，然后再加上以上一个节点。这里是单独启动了一个nodelet，并没有加载 nodelet manager。这里将 cloud  重映射为 kinect 产生的点云数据话题 /camera/depth_registered/points 。然后将 obstacles 重映射为 /planner_cloud 。
这里可以看到由于是投影地图，并不像激光只扫外面一圈，所以地图显得比较乱，我觉得这种比较适合室外的大场景的SLAM，因为这个并不需要地面是平面。
同样，这里需要将 handsfree 的 move_base.xml 中 move_base 节点中的 map 重映射为 /rtabmap/grid_map 。
使用投影地图来作为二维地图时，move_base 的参数配置文件也需要改一下了，需要在 local_costmap_param.yaml  中需要将之前的 observation\_sources 从 scan 改为 cloud：
observation_sources: point_cloud_sensor# assuming receiving a cloud from rtabmap_ros/obstacles_detection nodepoint_cloud_sensor: {  sensor_frame: base_link,  data_type: PointCloud2,  topic: planner_cloud,  expected_update_rate: 0.5,  marking: true,  clearing: true,  min_obstacle_height: -99999.0,  max_obstacle_height: 99999.0}
这样就可以使用 obstacles_detection 这个nodelet生成的障碍物点云来作为 costmap 的障碍物来源了。注意，这里的 topic 必须和 obstacles_detection 产生的障碍物点云的话题名一样。
这样就基本上配置好了两种不同方法的导航环境，没有问题的话即可像普通二维地图那样进行导航。
下面是采用 scan 地图方法进行的SLAM效果图：
后记
我个人觉得 rtabmap 的强大在于内存管理，但是整个系统比较复杂，所以仍然十分耗费内存，可能在 ARM 平台上流畅运行不太现实。我这里的教程只能说是从入门开始，给大家一个大致的思路，具体的一些细节可能还是要对照着wiki来，希望能对各位机友有一定帮助，另外感谢 @dajianli 和 小马哥 耐心地帮我解决了部分硬件上的问题。