视觉SLAM实战（一）：RGB-D SLAM V2

写在前面

首先打个广告。SLAM研究者交流QQ群：254787961。欢迎各路大神和小白前来交流。

看了前面三篇博文之后，是不是有同学要问：博主你扯了那么多有用没用的东西，能不能再给力一点，拿出一个我们能实际上手玩玩的东西啊？没错，接下来我们就带着大家，实际地跑一下视觉SLAM里的那些经典程序，给大家一个直观的印象——因此博文就叫"视觉SLAM实战"啦。这些程序包括：

RGBD SLAM V2
SVO
KinectFusion
Orb-SLAM

　　如果你有什么建议，可以发我的邮件或来群里交流。当然，我是边跑边写博客，而不是一口气调通再稀里哗啦写的。所以呢，最后跑的程序可能会和现在的计划有些出入啦。好了，废话不多说，先来介绍下实验器材。

实验器材

1.　　硬件

　　说好的SLAM，没有机器人怎么行？老板，先给我来三份！

这种机器人是Turtlebot的一个改装版本：Viewbot。Turtlebot的详情请参见：http://wiki.ros.org/Robots/TurtleBot/。Viewbot是上海物景公司做的改装版本，包括可以装些额外的传感器，以及把原来的黑色底板变成透明底板什么的，用起来和turtlebot大同小异，价格1W+。为了避免做广告的嫌疑我就不放链接了。用这个机器人的好处，是ROS有直接对应的包，不用你自己写。一句话即可打开传感器读数据，再一句话就可遥控它，方便省事。

机器人的主要部件就是它的底座和上头的Kinect啦。座子自带惯导，可以估计自己的位置；Kinect么，大家都知道了，不多讲。事实上今天要跑的rgbd-slam-v2不需要座子，只要一台kinect就能动起来。我们用不着那么多机器人，只用一个就行啦（就是贴了标签那台）。

2.　软件

　　软件方面只需一台笔记，放在机器上跑程序。我用的是一台华硕的ubuntu，装了mac主题包真是山寨气十足啊：

具体的软件配置后面会细说。

3.　　环境

　　环境嘛就是我的实验室，这个也不细说了。

SLAM程序

RGB-D-SLAM-V2程序是由F. Endres大大写的。论文见[1]。为什么首先选这个程序呢？因为它的原理在我们前面的博客中介绍过了。它是一个基于深度相机、Pose graph （图优化）的程序，后端用的是g2o。另一方面，它的代码直接兼容 ROS hydro版，基本不用配置就可以运行，非常方便。下面我们一步步地教大家运行这个程序：

从作者主页下载源代码。链接：http://felixendres.github.io/rgbdslam_v2/ 点击右侧的tar.gz或zip下载至本地。
下载完成后解压，得到一个包，里面好多文件呀：

不着急，先从README看起："RGBDSLAMv2 is based on the ROS project, OpenCV, PCL, OctoMap, SiftGPU and more – thanks!" 你还在等什么？装装装！好在这些东西在Ubuntu下安装，就是几句话搞定的事。

ROS hydro安装指南： http://wiki.ros.org/cn/hydro/Installation/Ubuntu （加ppa源后直接安装）

Linux OpenCV安装指南：http://blog.sciencenet.cn/blog-571755-694742.html （从源代码编译）

PCL：http://www.pointclouds.org/downloads/linux.html （加ppa后安装）

后面几个嘛，装不装就看心情了，即使没有装，rgbd-slam-v2也能运行起来。

　　3. 装完之后，看"Installation from scratch"一栏，基本照着做一遍即可。作者把命令都给出来了，我就不贴了。装完之后，rgbdslam就在你的ros包里啦。

　　4. 把机器人的Kinect usb口插进电脑，运行 roslaunch rgbdslam openni+rgbdslam.launch，就能看到一个漂亮的界面了。

可以看出作者真的挺用心，居然做了UI。像我这样的懒人绝对不可能做UI的……不谈这些，底下的两个图就是Kinect当前采到的彩色图与深度图，而上面则是3D在线点云图（可以用鼠标转视角）。现在，程序还在待机状态，敲下Enter会采集单帧数据，而敲下空白键则会连续采集。

此外，程序的参数可以在openni+rgbdslam.launch文件中调整。例如特征点类型呀（支持SIFT，SURF，ORB，SIFTGPU），最大特征数量等等。

运行程序

　　现在，我们连上Turtlebot的遥控端：

　　roslaunch turtlebot_bringup minimal.launch (启动底座)

　　roslaunch turtlebot_teleop keyboard_teleop.launch (启动遥控)

　　按下rgbd-slam-v2的空白键，让机器人四处走起来。UI的状态栏会显示程序的运行状态，我看到过的有正在提取特征、加入新Frame，等等。如果它成功匹配上，上图的点云就会更新，并且会跟着机器人转动。

我让小车在实验室的一个角上转了几圈。它把中间放置的一堆箱子（实际上是垃圾）都扫出来了。当你觉得满意后，再按下Space键，停止程序。然后从菜单中选出需要保存的东西：机器人的轨迹，最终点云图，等等。轨迹是一个txt文件，而点云图则一个pcd，在安装PCL之后调用pcl_viewer即可查看。

别看上面像是打了码一样的，最后出的点云图可是高清无码的：

轨迹方面，用Matlab写个脚本plot一下即可：

可以看到轨迹上出现了断层，实际上是机器人转的比较快，算法跟丢了，然而之后又通过回环检测给找了回来。

评价

最后，总结一下这个实验吧。

Rgbd-slam-v2是14年论文里提到的算法。它整合了SLAM领域里的各种技术：图像特征、回环检测、点云、图优化等等，是一个非常全面且优秀的程序。它的UI做的也很漂亮，你可以在它的源代码上继续开发。作者也提供了数据集供研究者们测试。

缺点：在美观之外，由于要提特征（SIFT很费时）、渲染点云，这些事情是很吃资源的，导致算法实时性不太好。有时你会发现它卡在那儿不动了，不得不等它一小会。机器人如果走的太快，很容易跟丢。于是我的机器人就真的像只乌龟一样慢慢地在地板上爬了……一旦转头快了，轨迹基本就断掉了。此外，程序采集关键帧的频率很高，稍微一会就采出几十个帧，不太适合做长时间的SLAM。最后合出来的点云有300W+个点，我用网格滤波之后才能勉强显示出来。

参考文献

[1]. Endres et al, 3D Mapping with an RGB-D camera, TRO, 2014.