连载文章，长期更新，欢迎关注：

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

        9.1 ORB-SLAM2算法

        9.2 LSD-SLAM算法

        9.3 SVO算法

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

上一章介绍了以激光雷达做为数据输入的激光SLAM系统，激光雷达的优点在于数据稳定性好、测距精度高、扫描范围广，但缺点是价格昂贵、数据信息量低、安装部署位置不能有遮挡、雨天烟雾等环境容易失效。相比于激光雷达，视觉传感器价格便宜许多、所采集到的图像数据信息量更高、室内室外场景都能适用并且雨天烟雾场景影响较小，不过视觉传感器数据稳定性和精度较差、巨大的数据量也消耗更大的计算资源。主流的视觉传感器包括单目、双目和RGB-D这3类相机，其原理见4.3节相关内容。依据对图像数据的不同处理方式，视觉SLAM可以分为特征点法、直接法和半直接法。下面按照特征点法、直接法和半直接法分类，列出了几种常见的视觉SLAM算法，如表9-1所示。

表9-1  几种常见的视觉SLAM算法

在特征点法中，首先对输入图像进行特征提取（比如SIFT、SURF、ORB特征，详细过程见3.4节相关内容），然后进行特征匹配，这里特征提取和匹配的过程就是所谓的数据关联；接着就可以利用数据关联信息计算相机运动，也就是前端视觉里程计（Visual Odometry,VO）；最后进行后端优化和全局建图。在直接法中，不需要对图像进行特征提取和匹配，而是直接利用图像灰度信息进行数据关联同时计算相机运动。也就是说直接法中的前端VO是直接在图像灰度信息上进行的，这样就省去了很多花费在特征提取上的时间。接下来的后端处理过程，其实和特征点法都类似。在半直接法中，结合了特征点法和直接法这两种思路，优点是处理速度大大提升。

典型的特征点法SLAM算法主要有MonoSLAM、PTAM和ORB-SLAM2。其中MonoSLAM[1]算得上是首个基于纯单目相机的视觉SLAM系统，系统前端从输入的灰度图像中提取Shi-Tomasi角点特征，并采用匀速运动模型进行运动预测，最后将相机状态和路标特征点当成一个整体状态估计量放入后端EKF滤波器利用观测做更新。很遗憾，MonoSLAM由于采用滤波方法进行求解，因此只能构建很小规模的地图；并且其中的Shi-Tomasi角点特征不具备旋转不变特性，故特征稳定性受运动影响较大。

而PTAM[2]则是首个采用优化方法求解的视觉SLAM系统，PTAM也是以纯单目相机为输入传感器，系统前端从图像中提取FAST特征并用相邻两帧间的匹配特征点对构建重投影误差函数来求解VO，系统后端进行优化得到地图。PTAM中有两大创新，一个创新是将前端和后端在代码中用双线程来实现，另一个创新是引入了关键帧机制。所谓双线程，就是将前端特征提取、匹配、VO这些跟定位相关的逻辑放在一个单独线程中实现，而将后端全局优化和局部优化这些跟建图相关的逻辑放在另一个单独线程中实现，这样运行较慢的后端线程就不会拖累运行较快的前端线程以保证前端定位的实时性。关键帧顾名思义就是某些具有代表性的图像输入帧，所谓关键帧机制，就是整个算法在运行途中维护一个由一系列关键帧组成的关键帧序列，前端在定位丢失时能借助关键帧信息快速重定位，后端在关键帧序列上进行全局优化和局部优化从而避免将大量冗余输入帧纳入优化过程带来的计算资源浪费。

毫不夸张来说，ORB-SLAM是目前为止效果最好的视觉SLAM系统。该算法有两个版本，第一代算法ORB-SLAM[3]只支持单目相机，由于单目SLAM普遍存在尺度不确定性问题，因此第二代算法ORB-SLAM2[4]支持了单目、双目和RGB-D这3种相机。该算法借鉴了PTAM双线程的思想，并增加了一个线程用于闭环检测，也就是说算法用前端、后端、闭环这3个线程实现。系统前端从图像中提取ORB特征并用相邻两帧间的匹配特征点对构建重投影误差函数来求解VO，其所提取的特征ORB正是该算法名称之由来，ORB特征具有极好的稳定性和极快的提取速度。后端进行局部优化建图，并且当闭环检测成功后触发全局优化。全局优化过程在相机位姿图上进行而不考虑地图特征点，这样能大大加快优化速度。除了在前端提取选择ORB特征和引入闭环检测外，该算法还在很多程序细节上做了大量创新，因此这个算法非常具有学习和研究的价值。

典型的直接法SLAM算法主要有DTAM、LSD-SLAM和DSO。其中DTAM[5]的思路是采用单目直接法构建稠密地图，系统前端直接利用图像像素构建代价函数求解VO，并利用逆深度（也就是深度值的倒数）描述每个像素的深度，在系统后端全局优化中为图像中每个像素都恢复其深度图，这样就构建出了3D稠密地图。可想而知，为每个像素都恢复稠密深度将非常消耗计算资源，因此这个算法也只能在GPU加速下才能跑得动。

LSD-SLAM[6,7,8]构建出来的是半稠密地图，这与DTAM直接构建稠密地图有所区别。系统前端直接利用当前图像和当前关键帧之间的像素构建代价函数求解VO，系统后端会在每个关键帧中按梯度大小抽取部分像素用深度滤波器恢复其深度值，这样就构建出了3D半稠密地图，系统闭环检测成功后触发在相机位姿图上进行的全局优化。刚开始LSD-SLAM仅支持单目相机，后来作者将其扩展到单目、双目和全景这3种相机。

DSO[9]与LSD-SLAM相比地图更为稀疏，系统前端采用直接法求解VO，系统后端将关键帧中梯度比较突出的像素抽取出来用于深度恢复，算法整个优化是在光度标定模型、相机内参数、相机外参和逆深度上进行的，所以求解精度会更高。

其实特征点法与直接法的区别就在于数据关联，特征点法的数据关联是由特征提取和特征匹配这两个过程来完成，而直接法则是一步到位直接在图像像素上完成数据关联。SVO[10]算法中结合了特征点法特征提取与匹配的鲁棒性优势和直接法计算快速性优势，因此具有更快速更稳定的性能，这种方法也称之为半直接法。不过由于开源出来的SVO是经过阉割后的版本，并没有后端优化和闭环检测环节，严格意义上并不能称之为SLAM系统，确切点说只是一个VO而已。

由于篇幅限制，就分别选取特征点法、直接法和半直接法中的代表性算法ORB-SLAM2、LSD-SLAM和SVO，在下文中展开具体分析。

9.1 ORB-SLAM2算法

        9.1.1 ORB-SLAM2原理分析

        9.1.2 ORB-SLAM2源码解读

        9.1.3 ORB-SLAM2安装与运行

        9.1.4 拓展

9.2 LSD-SLAM算法

        9.2.1 LSD-SLAM原理分析

        9.2.2 LSD-SLAM源码解读

        9.2.3 LSD-SLAM安装与运行

9.3 SVO算法

        9.3.1 SVO原理分析

        9.3.2 SVO源码解读

本章选取了ORB-SLAM2、LSD-SLAM和SVO，分别作为视觉SLAM算法中特征点法、直接法和半直接法的一种代表性算法进行分析。其中ORB-SLAM2具有较高的研究价值和商业价值，并且代码书写得非常规范易读，需要作为重点内容进行学习并掌握。而LSD-SLAM和SVO的代码存在较多瑕疵，仅需了解原理即可，不要求掌握。特征点法需要借助特征提取和特征匹配建立数据关联，然后最小化重投影误；直接法则省去了特征提取和特征匹配步骤，直接在两帧图像的像素上建立数据关联，然后最小光度误差；半直接法则是结合了特征点法和直接法的优势，提取特征点，然后利用直接法建立数据关联。这里的关键概念是重投影误差和光度误差，一定要搞清楚两者的区别。

虽然激光SLAM和视觉SLAM已经能够覆盖到目前绝大部分的机器人应用了，但是还有一些其他类型的SLAM也备受关注，比如激光+视觉融合SLAM、IMU+视觉融合SLAM、基于深度学习的端到端SLAM、基于模式识别的语义SLAM等，将在下一章中讨论。

源码仓库

• Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation

• Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation