视觉SLAM漫谈

本人觉得博主写的文章非常好&＃xff0c;想分享给更多学习SLAM的同学&＃xff0c;故此转载原文地址http://www.cnblogs.com/gaoxiang12/p/3695962.html

1.    前言

　　开始做SLAM&＃xff08;机器人同时定位与建图&＃xff09;研究已经近一年了。从一年级开始对这个方向产生兴趣&＃xff0c;到现在为止&＃xff0c;也算是对这个

领域有了大致的了解。然而越了解&＃xff0c;越觉得这个方向难度很大。总体来讲有以下几个原因&＃xff1a;

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

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

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

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

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

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

2.    SLAM问题

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

3.    小萝卜的故事

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

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

　　1.          自己在哪里&＃xff1f;

　　2.          这是什么地方&＃xff1f;

　　3.          怎么离开这个地方&＃xff1f;

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

4.    Kinect数据

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

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

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

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

　　或者更简单的&＃xff1a;

　　后一个公式给出了计算三维点的方法。先从深度图中读取深度数据&＃xff08;Kinect给的是16位无符号整数&＃xff09;&＃xff0c;除掉z方向的缩放因子&＃xff0c;这样你就把一个整数变到了以米为单位的数据。然后&＃xff0c;x,y用上面的公式算出。一点都不难&＃xff0c;就是一个中心点位置和一个焦距而已。f代表焦距&＃xff0c;c代表中心。如果你没有自己标定你的Kinect&＃xff0c;也可以采用默认的值&＃xff1a;s&＃61;5000, cx &＃61; 320, cy&＃61;240, fx&＃61;fy&＃61;525。实际值会有一点偏差&＃xff0c;但不会太大。

5.    定位问题

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

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

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

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

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

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

　　上面两张图转过了16.63度&＃xff0c;位移几乎没有。

　　有同学会说&＃xff0c;那只要不断匹配下去&＃xff0c;定位问题不就解决了吗&＃xff1f;表面上看来&＃xff0c;的确是这样的&＃xff0c;只要我们引入一个关键帧的结构&＃xff08;发现位移超过一个固定值时&＃xff0c;定义成一个关键帧&＃xff09;。然后&＃xff0c;把新的图像与关键帧比较就行了。至于建图&＃xff0c;就是把这些关键帧的点云拼起来&＃xff0c;看着还有模有样&＃xff0c;煞有介事的&＃xff1a;

1&＃xff0d;200帧的匹配结果

　　然而&＃xff0c;如果事情真这么简单&＃xff0c;SLAM理论就不用那么多人研究三十多年了&＃xff08;它是从上世纪90年代开始研究的&＃xff09;&＃xff08;上面讲的那些东西简直随便哪里找个小硕士就能做出来……&＃xff09;。那么&＃xff0c;问题难在什么地方呢&＃xff1f;

6.    SLAM端优化理论

　　最麻烦的问题&＃xff0c;就是“噪声”。这种渐近式的匹配方式&＃xff0c;和那些惯性测量设备一样&＃xff0c;存在着累积噪声。因为我们在不断地更新关键帧&＃xff0c;把新图像与最近的关键帧比较&＃xff0c;从而获得机器人的位移信息。但是你要想到&＃xff0c;如果有一个关键帧出现了偏移&＃xff0c;那么剩下的位移估计都会多出一个误差。这个误差还会累积&＃xff0c;因为后面的估计都基于前面的机器人位置……哇&＃xff01;这后果简直不堪设想啊&＃xff08;例如&＃xff0c;你的机器人往右转了30度&＃xff0c;再往左转了30度回到原来的位置。然而由于误差&＃xff0c;你算成了向右转29度&＃xff0c;再向左转31度&＃xff0c;这样你构建的地图中&＃xff0c;会出现初始位置的两个“重影”&＃xff09;。我们能不能想办法消除这个该死的误差呢&＃xff1f;

　　朋友们&＃xff0c;这才是SLAM的研究&＃xff0c;前面的可以说是“图像前端”的处理方法。我们的解决思路是&＃xff1a;如果你和最近的关键帧相比&＃xff0c;会导致累计误差。那么&＃xff0c;我们最好是和更前面的关键帧相比&＃xff0c;而且多比较几个帧&＃xff0c;不要只比较一次。

　　我们用数学来描述这个问题。设&＃xff1a;

　　不要怕&＃xff0c;只有借助数学才能把这个问题讲清楚。上面的公式中&＃xff0c;xp是机器人小萝卜的位置&＃xff0c;我们假定由n个帧组成。xL则是路标&＃xff0c;在我们的图像处理过程中就是指SIFT提出来的关键点。如果你做2D SLAM&＃xff0c;那么机器人位置就是x, y加一个转角theta。如果是3D SLAM&＃xff0c;就是x,y,z加一个四元数姿态&＃xff08;或者rpy姿态&＃xff09;。这个过程叫做参数化&＃xff08;Parameterization&＃xff09;。

　　不管你用哪种参数&＃xff0c;后面两个方程你都需要知道。前一个叫运动方程&＃xff0c;描述机器人怎样运动。u是机器人的输入&＃xff0c;w是噪声。这个方程最简单的形式&＃xff0c;就是你能通过什么方式&＃xff08;码盘等&＃xff09;获得两帧间的位移差&＃xff0c;那么这个方程就直接是上一帧与u相加即得。另外&＃xff0c;你也可以完全不用惯性测量设备&＃xff0c;这样我们就只依靠图像设备来估计&＃xff0c;这也是可以的。

　　后一个方程叫观测方程&＃xff0c;描述那些路标是怎么来的。你在第i帧看到了第j个路标&＃xff0c;产生了一个测量值&＃xff0c;就是图像中的横纵坐标。最后一项是噪声。偷偷告诉你&＃xff0c;这个方程形式上和上一页的那个方程是一模一样的。

　　在求解SLAM问题前&＃xff0c;我们要看到&＃xff0c;我们拥有的数据是什么&＃xff1f;在上面的模型里&＃xff0c;我们知道的是运动信息u以及观测z。用示意图表示出来是这样的&＃xff1a;

　　我们要求解的&＃xff0c;就是根据这些u和z&＃xff0c;确定所有的xp和xL。这就是SLAM问题的理论。从SLAM诞生开始科学家们就一直在解决这个问题。最初&＃xff0c;我们用Kalman滤波器&＃xff0c;所以上面的模型&＃xff08;运动方程和观测方程&＃xff09;被建成这个样子。直到21世纪初&＃xff0c;卡尔曼滤波器仍在SLAM系统占据最主要的地位&＃xff0c;Davison经典的单目SLAM就是用EKF做的。但是后来&＃xff0c;出现了基于图优化的SLAM方法&＃xff0c;渐渐有取而代之的地位[1]。我们在这里不介绍卡尔曼滤波器&＃xff0c;有兴趣的同学可以在wiki上找卡尔曼滤波器&＃xff0c;另有一篇中文的《卡尔曼滤波器介绍》也很棒。由于滤波器方法存储n个路标要消耗n平方的空间&＃xff0c;在计算量上有点对不住大家。尽管08年有人提出分治法的滤波器能把复杂度弄到O(n) [2]&＃xff0c;但实现手段比较复杂。我们要介绍那种新兴的方法: Graph-based SLAM。

　　图优化方法把SLAM问题做成了一个优化问题。学过运筹学的同学应该明白&＃xff0c;优化问题对我们有多么重要。我们不是要求解机器人的位置和路标位置吗&＃xff1f;我们可以先做一个猜测&＃xff0c;猜想它们大概在什么地方。这其实是不难的。然后呢&＃xff0c;将猜测值与运动模型&＃xff0f;观测模型给出的值相比较&＃xff0c;可以算出误差&＃xff1a;

　　通俗一点地讲&＃xff0c;例如&＃xff0c;我猜机器人第一帧在(0,0,0)&＃xff0c;第二帧在(0,0,1)。但是u1告诉我机器人往z方向&＃xff08;前方&＃xff09;走了0.9米&＃xff0c;那么运动方程就出现了0.1m的误差。同时&＃xff0c;第一帧中机器人发现了路标1&＃xff0c;它在该机器人图像的正中间&＃xff1b;第二帧却发现它在中间偏右的位置。这时我们猜测机器人只是往前走&＃xff0c;也是存在误差的。至于这个误差是多少&＃xff0c;可以根据观测方程算出来。

　　我们得到了一堆误差&＃xff0c;把这些误差平方后加起来&＃xff08;因为单纯的误差有正有负&＃xff0c;然而平方误差可以改成其他的范数&＃xff0c;只是平方更常用&＃xff09;&＃xff0c;就得到了平方误差和。我们把这个和记作phi&＃xff0c;就是我们优化问题的目标函数。而优化变量就是那些个xp, xL。

　　改变优化变量&＃xff0c;误差平方和&＃xff08;目标函数&＃xff09;就会相应地变大或变小&＃xff0c;我们可以用数值方法求它们的梯度和二阶梯度矩阵&＃xff0c;然后用梯度下降法求最优值。这些东西学过优化的同学都懂的。

　　注意到&＃xff0c;一次机器人SLAM过程中&＃xff0c;往往会有成千上万帧。而每一帧我们都有几百个关键点&＃xff0c;一乘就是几百万个优化变量。这个规模的优化问题放到小萝卜的机载小破本上可解吗&＃xff1f;是的&＃xff0c;过去的同学都以为&＃xff0c;Graph-based SLAM是无法计算的。但就在21世纪06&＃xff0c;07年后&＃xff0c;有些同学发现了&＃xff0c;这个问题规模没有想象的那么大。上面的J和H两个矩阵是“稀疏矩阵”&＃xff0c;于是呢&＃xff0c;我们可以用稀疏代数的方法来解这个问题。“稀疏”的原因&＃xff0c;在于每一个路标&＃xff0c;往往不可能出现在所有运动过程中&＃xff0c;通常只出现在一小部分图像里。正是这个稀疏性&＃xff0c;使得优化思路成为了现实。

　　优化方法利用了所有可以用到的信息&＃xff08;称为full-SLAM, global SLAM&＃xff09;&＃xff0c;其精确度要比我们一开始讲的帧间匹配高很多。当然计算量也要高一些。

　　由于优化的稀疏性&＃xff0c;人们喜欢用“图”来表达这个问题。所谓图&＃xff0c;就是由节点和边组成的东西。我写成G&＃61;{V,E}&＃xff0c;大家就明白了。V是优化变量节点&＃xff0c;E表示运动/观测方程的约束。什么&＃xff0c;更糊涂了吗&＃xff1f;那我就上一张图&＃xff0c;来自[3]。

　　图有点模糊&＃xff0c;而且数学符号和我用的不太一样&＃xff0c;我用它来给大家一个图优化的直观形象。上图中&＃xff0c;p是机器人位置&＃xff0c;l是路标&＃xff0c;z是观测&＃xff0c;t是位移。其中呢&＃xff0c;p, l是优化变量&＃xff0c;而z,t是优化的约束。看起来是不是像一些弹簧连接了一些质点呢&＃xff1f;因为每个路标不可能出现在每一帧中&＃xff0c;所以这个图是蛮稀疏的。不过&＃xff0c;“图”优化只是优化问题的一个表达形式&＃xff0c;并不影响优化的含义。实际解起来时还是要用数值法找梯度的。这种思路在计算机视觉里&＃xff0c;也叫做Bundle Adjustment。它的具体方法请参见一篇经典文章[4]。

　　不过&＃xff0c;BA的实现方法太复杂&＃xff0c;不太建议同学们拿C来写。好在2010年的ICRA上&＃xff0c;其他的同学们提供了一个通用的开发包&＃xff1a;g2o [5]。它是有图优化通用求解器&＃xff0c;很好用&＃xff0c;我改天再详细介绍这个软件包。总之&＃xff0c;我们只要把观测和运动信息丢到求解器里就行。这个优化器会为我们求出机器人的轨迹和路标位置。如下图&＃xff0c;红点是路标&＃xff0c;蓝色箭头是机器人的位置和转角&＃xff08;2D SLAM&＃xff09;。细心的同学会发现它往右偏转了一些。&＃xff1a;

7.    闭环检测

　　上面提到&＃xff0c;仅用帧间匹配最大的问题在于误差累积&＃xff0c;图优化的方法可以有效地减少累计误差。然而&＃xff0c;如果把所有测量都丢进g2o&＃xff0c;计算量还是有点儿大的。根据我自己测试&＃xff0c;约10000多条边&＃xff0c;g2o跑起来就有些吃力了。这样&＃xff0c;就有同学说&＃xff0c;能把这个图构造地简洁一些吗&＃xff1f;我们用不着所有的信息&＃xff0c;只需要把有用的拿出来就行了。

　　事实上&＃xff0c;小萝卜在探索房间时&＃xff0c;经常会左转一下&＃xff0c;右转一下。如果在某个时刻他回到了以前去过的地方&＃xff0c;我们就直接与那时候采集的关键帧做比较&＃xff0c;可以吗&＃xff1f;我们说&＃xff0c;可以&＃xff0c;而且那是最好的方法。这个问题叫做闭环检测。

　　闭环检测是说&＃xff0c;新来一张图像时&＃xff0c;如何判断它以前是否在图像序列中出现过&＃xff1f;有两种思路&＃xff1a;一是根据我们估计的机器人位置&＃xff0c;看是否与以前某个位置邻近&＃xff1b;二是根据图像的外观&＃xff0c;看它是否和以前关键帧相似。目前主流方法是后一种&＃xff0c;因为很多科学家认为前一种依靠有噪声的位置来减少位置的噪声&＃xff0c;有点循环论证的意思。后一种方法呢&＃xff0c;本质上是个模式识别问题&＃xff08;非监督聚类&＃xff0c;分类&＃xff09;&＃xff0c;常用的是Bag-of-Words (BOW)。但是BOW需要事先对字典进行训练&＃xff0c;因此SLAM研究者仍在探讨有没有更合适的方法。

　　在Kinect SLAM经典大作中[6]&＃xff0c;作者采用了比较简单的闭环方法&＃xff1a;在前面n个关键帧中随机采k个&＃xff0c;与当前帧两两匹配。匹配上后认为出现闭环。这个真是相当的简单实用&＃xff0c;效率也过得去。

　　高效的闭环检测是SLAM精确求解的基础。这方面还有很多工作可以做。

8.    小结

　　本文我们介绍了SLAM的基本概念&＃xff0c;重点介绍了图优化解决SLAM问题的思路。我最近正在编写SLAM程序&＃xff0c;它是一个Linux下基于cmake的工程。目前仍在开发当中。欢迎感兴趣的同学来交流研究心得&＃xff0c;我的邮件是&＃xff1a;gaoxiang12&＃64;mails.tsinghua.edu.cn。

参考文献

[1] Visual SLAM: Why filter? Strasdat et. al., Image and Vision Computing, 2012.

[2] Divide and Conquer: EKF SLAM in O(n), Paz Lina M et al., IEEE Transaction on Robotics, 2008

[3] Relative bundle adjustment, Sibley, Gabe, 2009

[4] Bundle adjustment - a Modern Synthesis. Triggs B et. el., Springer, 2000

[5] g2o: A General Framework for Graph Optimization, Kummerle Rainer, et. al., ICRA, 2011

[6] 3-D Mapping with an RGB-D Camera, IEEE Transaction on Robotics, Endres et al., 2014