视觉SLAM漫淡

视觉SLAM漫谈

1. 前言

开始做SLAM（机器人同时定位与建图）研究已经近一年了。从一年级开始对这个方向产生兴趣，到现在为止，也算是对这个领域有了大致的了解。然而越了解，越觉得这个方向难度很大。总体来讲有以下几个原因：

∙入门资料很少。虽然国内也有不少人在做，但这方面现在没有太好的入门教程。《SLAM for dummies》可以算是一篇。中文资料几乎没有。

∙SLAM研究已进行了三十多年，从上世纪的九十年代开始。其中又有若干历史分枝和争论，要把握它的走向就很费工夫。

∙难以实现。SLAM是一个完整的系统，由许多个分支模块组成。现在经典的方案是“图像前端，优化后端，闭环检测”的三部曲，很多文献看完了自己实现不出来。

∙自己动手编程需要学习大量的先决知识。首先你要会C和C++，网上很多代码还用了11标准的C++。第二要会用Linux。第三要会cmake，vim/emacs及一些编程工具。第四要会用openCV, PCL, Eigen等第三方库。只有学会了这些东西之后，你才能真正上手编一个SLAM 系统。如果你要跑实际机器人，还要会ROS。

当然，困难多意味着收获也多，坎坷的道路才能锻炼人（比如说走着走着才发现Linux和C++才是我的真爱之类的。）鉴于目前网上关于视觉SLAM的资料极少，我于是想把自己这一年多的经验与大家分享一下。说的不对的地方请大家批评指正。

这篇文章关注视觉SLAM，专指用摄像机，Kinect等深度像机来做导航和探索，且主要关心室内部分。到目前为止，室内的视觉SLAM仍处于研究阶段，远未到实际应用的程度。一方面，编写和使用视觉SLAM需要大量的专业知识，算法的实时性未达到实用要求；另一方面，视觉SLAM生成的地图（多数是点云）还不能用来做机器人的路径规划，需要科研人员进一步的探索和研究。以下，我会介绍SLAM的历史、理论以及实现的方式，且主要介绍视觉（Kinect）的实现方式。

2. SLAM问题

SLAM，全称叫做Simultaneous Localization and Mapping，中文叫做同时定位与建图。啊不行，这么讲下去，这篇文章肯定没有人读，所以我们换一个讲法。

3. 小萝卜的故事

从前，有一个机器人叫“小萝卜”。它长着一双乌黑发亮的大眼睛，叫做Kinect。有一天，它被邪恶的科学家关进了一间空屋子，里面放满了杂七杂八的东西。

小萝卜感到很害怕，因为这个地方他从来没来过，一点儿也不了解。让他感到害怕的主要是三个问题：

1. 自己在哪里？

2. 这是什么地方？

3. 怎么离开这个地方？

在SLAM理论中，第一个问题称为定位 (Localization)，第二个称为建图 (Mapping)，第三个则是随后的路径规划。我们希望借助Kinect工具，帮小萝卜解决这个难题。各位同学有什么思路呢？

4. Kinect数据

要打败敌人，首先要了解你的武器。不错，我们先介绍一下Kinect。众所周知这是一款深度相机，你或许还听说过别的牌子，但Kinect的价格便宜，测量范围在3m-12m之间，精度约3cm，较适合于小萝卜这样的室内机器人。它采到的图像是这个样子的（从左往右依次为rgb图，深度图与点云图）：

Kinect的一大优势在于能比较廉价地获得每个像素的深度值，不管是从时间上还是从经济上来说。OK，有了这些信息，小萝卜事实上可以知道它采集到的图片中，每一个点的3d位置。只要我们事先标定了Kinect，或者采用出厂的标定值。

我们把坐标系设成这个样子，这也是openCV中采用的默认坐标系。

o’-uv是图片坐标系，o-xyz是Kinect的坐标系。假设图片中的点为(u,v)，对应的三维点位置在(x,y,z)，那么它们之间的转换关系是这样的：

或者更简单的：

后一个公式给出了计算三维点的方法。先从深度图中读取深度数据（Kinect给的是16位无符号整数），除掉z方向的缩放因子，这样你就把一个整数变到了以米为单位的数据。然后，x,y 用上面的公式算出。一点都不难，就是一个中心点位置和一个焦距而已。f代表焦距，c代表中心。如果你没有自己标定你的Kinect，也可以采用默认的值：s=5000, cx = 320, cy=240,

fx=fy=525。实际值会有一点偏差，但不会太大。

5. 定位问题

知道了Kinect中每个点的位置后，接下来我们要做的，就是根据两帧图像间的差别计算小萝卜的位移。比如下面两张图，后一张是在前一张之后1秒采集到的：

你肯定可以看出，小萝卜往右转过了一定的角度。但究竟转过多少度呢？这就要靠计算机来求解了。这个问题称为相机相对姿态估计，经典的算法是ICP（Iterative Closest Point，迭代最近点）。这个算法要求知道这两个图像间的一组匹配点，说的通俗点，就是左边图像哪些点和右边是一样的。你当然看见那块黑白相间的板子同时出现在两张图像中。在小萝卜看来，这里牵涉到两个简单的问题：特征点的提取和匹配。

如果你熟悉计算机视觉，那你应该听说过SIFT, SURF之类的特征。不错，要解决定位问题，首先要得到两张图像的一个匹配。匹配的基础是图像的特征，下图就是SIFT提取的关键点与匹配结果：

对实现代码感兴趣的同学请Google“opencv匹配”即可，在openCV的教程上也有很明白的例子。上面的例子可以看出，我们找到了一些匹配，但其中有些是对的（基本平等的匹配线），有些是错的。这是由于图像中存在周期性出现的纹理（黑白块），所以容易搞错。但这并不是问题，在接下来的处理中我们会将这些影响消去。

得到了一组匹配点后，我们就可以计算两个图像间的转换关系，也叫PnP问题。它的模型是这样的：

R为相机的姿态，C为相机的标定矩阵。R是不断运动的，而C则是随着相机做死的。ICP 的模型稍有不同，但原理上也是计算相机的姿态矩阵。原则上，只要有四组匹配点，就可以算这个矩阵。你可以调用openCV的SolvePnPRANSAC函数或者PCL的ICP算法来求解。openCV 提供的算法是RANSAC（Random Sample Consensus，随机采样一致性）架构，可以剔除错误匹配。所以代码实际运行时，可以很好地找到匹配点。以下是一个结果的示例。