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

下面将从原理分析、源码解读和安装与运行这3个方面展开讲解ORB-SLAM2算法。

9.1.1 ORB-SLAM2原理分析

前面已经说过，ORB-SLAM2算法是特征点法的典型代表。因此在下面的分析中，首先介绍一下特征点法的基本原理。特征点法中除了最基本的特征提取和特征匹配外，还涉及到相机在三维空间运动时位姿的表示，以及帧与帧之间配对特征点、环境地图点、相机位姿等共同形成的多视图几何关系。在掌握了特征点法、三维空间运动和多视图几何这些基本知识后，就可以结合论文[4]对ORB-SLAM2系统框架展开具体分析了。

1.特征点法

通过第7章的学习，我们已经知道SLAM就是求解运动物体（比如机器人、无人机、相机等）的位姿和环境中路标点（也就是环境地图点）的问题。当相机从不同的角度拍摄同一个物体时可以得到不同的图像，而这些图像中具有很多相同的信息，这就构成了共视关系。一幅图像由很多像素点组成，那么如何利用每帧图像中像素点所包含的信息表示这种共视关系，并利用该共视关系计算出相机位姿和环境地图点呢？

可能大家会最先想到，直接将图像中的每个像素点放入某种计算模型中参与计算，通过这种由像素点直接参与计算的共视关系计算模型就能求出机位姿和环境地图点，这就是前面说过的直接法，关于直接法的详细分析将在9.2节中展开。

除了用像素点直接构建共视关系，还有一种比较间接的方法，就是先从图像像素点中提取一些比较独特的点（即特征点），然后在不同视角拍摄的图像中寻找特征点的匹配关系。接着，将这些匹配好的特征点用来构建共视关系计算模型求出机位姿和环境地图点，这就是前面说过的特征点法。下面对特征点法中的特征提取、特征匹配和模型构建展开讨论。

（1）特征提取

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（2）特征匹配

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（3）模型构建

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.三维空间运动

相机的运动过程可以看成三维空间的刚体运动，所谓刚体，就是运动物体的机械形状随运动不发生变化。假如以相机起始时刻的位姿（Pose[0]）处建立世界坐标系（World），经过运动之后相机到达位姿（Pose[1]），那么相机在世界坐标系下的位姿Pose[1]就可以看成位姿Pose[0]经过旋转和平移的合成，如图9-4所示。也就是说相机位姿由3自由度旋转量和3自由度平移量共同表示，一共为6个自由度。旋转量表示相机在空间中的朝向，具体表达形式包括欧拉角、旋转矩阵、四元数等；平移量表示相机在空间中的位置，也就是x、y、z坐标值。

图9-4 三维空间刚体运动

（1）欧拉角、旋转矩阵、四元数

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（2）转移矩阵

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（3）李群、李代数

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.多视图几何

现在有了用转移矩阵 $T$ 描述相机运动的基础知识后，就可以更详细地讨论图9-3中的多视图几何模型了。按照模型中给定已知条件的不同，可以分为2D-2D、3D-2D和3D-3D这3种情况讨论[22]p141~180。

（1）2D-2D模型

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（2）3D-2D模型

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（3）3D-3D模型

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.ORB-SLAM2系统框架

到这里就可以分析ORB-SLAM2的系统框架了，结合算法原作者清晰的论文思路[3,4]，很容易理解整个算法的组成架构。由于ORB-SLAM为纯单目系统，而ORB-SLAM2在原单目系统上添加了双目和RGB-D的支持，所以系统框架要分两部分来看。纯单目系统架构，如图9-17所示。系统架构非常清晰，由追踪（TRACKING）、局部建图（LOCAL MAPPING）和闭环（LOOP CLOSING）三个主要线程构成，除此之外系统还包括地图初始化（Map Initialization）、位置识别（PLACE RECOGNITION）、地图结构（MAP）等模块。

图9-17 纯单目系统框架

双目和RGB-D系统架构，如图9-18所示。相比于纯单目系统，只是增加了输入预处理（Pre-process Input）模块用于专门处理双目或RGB-D数据，闭环检测从计算 $Sim3$ 换成了计算 $SE3$ （因为双目或RGB-D系统的尺度不确定性消失了），还有就是增加了第四个线程全局BA优化（FULL BA），系统其他部分大体上与纯单目系统保持了一致。

图9-18 双目和RGB-D系统框架

由于双目和RGB-D系统架构是从纯单目系统架构发展而来，并且双目和RGB-D系统架构实现起来比纯单目更容易。也就是说掌握了纯单目系统的原理，理解双目和RGB-D系统非常容易，所以下面就以单目系统的原理展开进一步的分析。

（1）地图结构

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（2）地图初始化

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（3）位置识别

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（4）追踪线程

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（5）局部建图线程

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（6）闭环线程

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

9.1.2 ORB-SLAM2源码解读

上面讨论完ORB-SLAM2的原理，现在就来解读ORB-SLAM2的源码，其代码框架如图9-31所示。代码原作者提供了多个例程（Examples）来启动程序，每个例程都含有一个main()函数。算法的追踪、局部建图和闭环3个线程还有一些重要的类都被封装在ORB-SLAM2核心库，这是我们将要学习的重点。ORB-SLAM2核心库所依赖的第三方库分为两种，一种是C++11 or C++0x Compiler、Pangolin、OpenCV、Eigen、ROS这些直接装在操作系统之上的第三方库，另一种是DBoW2和g2o这样代码直接内嵌在ORB-SLAM2核心库里面的第三方库。

图9-31 ORB-SLAM2代码框架

ORB_SLAM2/Examples文件夹中提供了多个main()函数实现源文件，这些源文件可以从3个方面来分类。从是否支持ROS接口方面，可以分为非ROS例程和ROS例程；从传感器类型方面，可以分为单目（Monocular）、双目（Stereo）和RGB-D例程；从数据输入方式方面，可以分为从数据集获取数据（Dataset）、从传感器直接获取数据（Sensor Data）和从ROS话题获取数据（ROS Topic）例程。这3个方面通过排列组合，就可以组合出很多例程，本质上讲这些例程的区别仅在于数据输入方式不同，而最终都通过调用ORB-SLAM2核心库中的System类来启动。

ORB_SLAM2/include和ORB_SLAM2/src文件夹中存放ORB-SLAM2核心库的具体实现源码，如表9-3所示。代码层次非常清晰，代码自顶向下由各个封装类来实现。算法顶层接口封装在System类中，通过调用其他类完成整个系统配置并启动3个主线程让算法进入工作。地图数据结构封装在Map类、MapPoint类、Frame类和KeyFrame类，这4个类的内在关系如图9-19所示。整个系统的运行结果是直接输出到图形界面显示的，其实现封装在MapDrawer类、FrameDrawer类和Viewer类。前面讲过在单目情形下，追踪线程首先要执行地图初始化，其实现封装在Initializer类。前面讲过词袋模型由3部分组成，即离线字典、在线数据库和应用。离线字典就是程序启动后载入的一个固定文件ORBvoc.txt.tar.gz；在线数据库实现，则封装在KeyFrameDatabase类；词袋模型的应用则分散在重定位、闭环检测、帧间特征匹配等具体逻辑中。ORB特征处理包括特征提取和特征匹配，分别封装在ORBextractor类和ORBmatcher类。算法中求解多视图几何3D-2D模型的方法（即PnP求解器），封装在PnPsolver类。闭环中求解相似变换的方法（即Sim3求解器），封装在Sim3Solver类。整个系统涉及到的局部BA、全局BA、位姿图、本征图、Sim3等优化问题，封装在Optimizer类。提供中涉及到Mat、g2o、Eigen等数据格式转换，封装在Converter类。最后，追踪、局部建图和闭环3个主线程分别封装在Tracking类、LocalMapping类和LoopClosing类。

表9-3 ORB-SLAM2核心库文件

用途		源文件
算法顶层接口	System类	System.cc	System.h
地图数据结构	Map类 MapPoint类 Frame类 KeyFrame类	Map.cc MapPoint.cc Frame.cc KeyFrame.cc	Map.h MapPoint.h Frame.h KeyFrame.h
图形界面显示	MapDrawer类 FrameDrawer类 Viewer类	MapDrawer.cc FrameDrawer.cc Viewer.cc	MapDrawer.h FrameDrawer.h Viewer.h
地图初始化	Initializer类	Initializer.cc	Initializer.h
词袋模型接口	KeyFrameDatabase类	KeyFrameDatabase.cc	KeyFrameDatabase.h ORBVocabulary.h
ORB特征处理	ORBextractor类 ORBmatcher类	ORBextractor.cc ORBmatcher.cc	ORBextractor.h ORBmatcher.h
PnP求解器	PnPsolver类	PnPsolver.cc	PnPsolver.h
Sim3求解器	Sim3Solver类	Sim3Solver.cc	Sim3Solver.h
优化	Optimizer类	Optimizer.cc	Optimizer.h
数据格式转换	Converter类	Converter.cc	Converter.h
3个主线程	Tracking类 LocalMapping类 LoopClosing类	Tracking.cc LocalMapping.cc LoopClosing.cc	Tracking.h LocalMapping.h LoopClosing.h

由于代码层次非常清晰，下面就按照代码自顶向下调用的流程，对涉及到的各个类展开具体分析。

1.System类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.Map、MapPoint、Frame和KeyFrame类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.Initializer类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.KeyFrameDatabase类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

5.ORBextractor和ORBmatcher类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

6.PnPsolver类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

7.Sim3Solver类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

8.Optimizer类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

9.Tracking、LocalMapping和LoopClosing类

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

9.1.3 ORB-SLAM2安装与运行

学习完ORB-SLAM2算法的原理及源码之后，大家肯定迫不及待想亲自安装运行一下ORB-SLAM2体验一下真实效果。在第1章中已经声明过，本书在Ubuntu18.04和ROS melodic环境下进行讨论。不管是使用X86主机、X86主机虚拟机还是ARM主机，一旦装好Ubuntu18.04系统后，就可以在该系统上安装ROS melodic发行版了。如果你只是想利用数据集离线跑算法，可以选择在X86主机或X86主机虚拟机上运行Ubuntu18.04和ROS melodic，关于这一部分的环境搭建请参考1.2.1节的内容。如果需要在实际机器人上在线跑算法，可以选择在ARM主机上运行Ubuntu18.04和ROS melodic，关于这一部分的环境搭建请参考第5章的内容。所以，下面的讨论假设Ubuntu18.04和ROS melodic环境已经准备妥当了。下面所讨论的ORB-SLAM2安装、配置和运行的内容都参考自ORB-SLAM2官方文档。由于ORB-SLAM2所提供的测试例程既包括ROS例程也包括非ROS例程，就是说没有安装ROS的读者也可以体验ORB-SLAM2。

1.ORB-SLAM2安装

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.ORB_SLAM2离线运行

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.ORB_SLAM2在线运行

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

9.1.4 拓展

本书写作至此时，ORB-SLAM3已经正式发布，鉴于诸多新特性具有很高的学习价值，这里就简要介绍一下。ORB-SLAM3是从ORB-SLAM2和ORB-SLAM-VI发展而来，创新点主要体现在多地图机制（multi-map）和视觉惯导融合（visual-inertial），其系统架构如图9-45所示。

图9-45 ORB-SLAM3系统架构

在ORB-SLAM2中始终就维护一个全局的大地图结构（MAP），这个大地图中包含所有的关键帧、地图点及相应的约束关系。如果不小心某些错误帧被引入到地图之中或者闭环优化的时候出现较大偏差之类的，整个地图的效果会很糟糕，而且这种错误会长远地影响整个地图。其实google-cartogapher中就用了更为鲁棒的子图（submap）机制来组织地图，而ORB-SLAM3中采用了类似的机制，只不过这里叫ATLAS机制。也就是在线用关键帧和地图点构建成子地图，这里叫Active Map；然后将构建成熟的Active Map离线保存起来，这里叫Non-active Map；随时间推移离线保存起来的Non-active Map会有很多个。

在ORB-SLAM中支持单目传感器，ORB-SLAM2中增加了对双目和RGB-D传感器的支持，而ORB-SLAM3又增加了对IMU的支持，并且同时对针孔和鱼眼相机的支持。如果有IMU数据输入系统的话，追踪（TRACKING）线程会同时读取图像帧和IMU数据，并且初始位姿估计中的匀速模型的速度值会改为IMU来提供。当追踪丢失进行重定位时，会在Active Map和所有Non-active Map中搜索，如果在Active Map搜索范围内重定位成功则追踪继续，如果在Non-active Map搜索范围内重定位成功则将该Non-active Map变成Active Map并让追踪继续，而如果重定位失败了则初始化地图并开始构建新的Active Map。在ORB-SLAM2中重定位失败后系统就死掉了，而ORB-SLAM3在重定位失败后选择构建新的Active Map，一旦新构建出的Active Map在闭环检测中与以前离线保存的Non-active Map匹配成功则重定位成功，追踪又可以继续了。虽然定位丢失会持续一段时间，但系统终究有机会找回定位，这个机制是实现SLAM系统持续鲁棒建图的关键，在需要持续工作的商业机器人中具有重要的应用价值。回顾一下8.2节中的cartographer算法正是凭借这种类似的重定位持续鲁棒建图性能，被广泛应用于商业机器人之中。如果有IMU数据输入系统的话，在局部建图（LOCAL MAPPING）线程中，会采用最大后验（MAP）估计来融合视觉和IMU之间的数据进行局部BA优化。其实就是先用最大后验（MAP）估计对纯视觉约束进行一轮优化，然后再用最大后验（MAP）估计对纯惯导约束进行一轮优化，最后将纯视觉优化的结果和纯惯导优化的结果再用最大后验（MAP）估计进行一次融合的优化。如果有IMU数据输入系统的话，在闭环与地图合并（LOOP & MAP MERGING）线程中，将建图线程输入的每个当前关键帧与ATLAS中的Active Map和所有Non-active Map进行回环检测匹配。如果当前关键帧与Active Map中的候选帧匹配成功，则按照ORB-SLAM2中类似的方法对Active Map进行闭环融合、位姿图优化和全局BA优化；如果当前关键帧与Non-active Map中的候选帧匹配成功，则将Active Map与该Non-active Map进行合并。最后，方便大家进一步学习，给出ORB-SLAM3与其他SLAM框架的性能对比，如图9-46所示。