写在前面

最近准备开始做本科毕业设计，准备对 SLAM 系统中的回环检测模块下手。因为新冠疫情不知道什么时候才能返校，这次放假回家就带了个 matebook 14 回来，搬砖全靠这小电脑，它苦我也苦（流泪）。

作为开源 SLAM 系统中的经典之一 —— ORB-SLAM2，自然是要拿来好好研究一番。之前阅读了 ORB-SLAM 和 ORB-SLAM2 论文，之后配置安装了 ORB-SLAM2 跑了个 example 看看样子（我的博文 ORB-SLAM2 初体验 —— 配置安装），这回准备开始磕代码。但越磕越发现，这玩意也太复杂了吧。为了实现较好的鲁棒性，ORB-SLAM2 中加入了很多很多小 trick 来从细节上提升系统的性能。这些细节在论文里往往就是一句话带过，但在代码里就是一大堆环环相扣绕来绕去的东西。虽然大家都说 ORB-SLAM2 的代码是结构清晰，注释完整，易于理解，但本渣渣还是看的把头发挠成了鸟窝状。

所以，我决定通过写博文的方式，来更好地梳理 ORB-SLAM2 代码的框架和步骤，以加深自己的理解，也希望我的梳理能够对大家有所帮助。

ORB-SLAM2 简介

ORB-SLAM是15年Raul等人提出的一个单目SLAM系统，其在单目SLAM领域影响广泛。详情可见论文：[Monocular] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics, vol. 31, no. 5, pp. 1147-1163, 2015. (2015 IEEE Transactions on Robotics Best Paper Award). PDF.

在单目ORB-SLAM的基础上，17年Raul等人又提出了ORB-SLAM2，增加了对于双目相机和RGB相机的支持。详情可见论文：[Stereo and RGB-D] Raúl Mur-Artal and Juan D. Tardós. ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras. IEEE Transactions on Robotics, vol. 33, no. 5, pp. 1255-1262, 2017. PDF

ORB-SLAM2 的论文中，对于单目部分并没有做很大修改，也没有笔墨去重写单目实现的细节。而其虽然增加了双目和 RGB-D 相机的支持，但本质上还是在单目系统的基础上加的，并不是从根本上以双目或 RGB-D 输入为设计出发点。所以个人认为，学习 ORB-SLAM2 还是要以单目为主，不能绕开单目去看双目或 RGB-D 的实现。另外，因为 ORB-SLAM2 论文中并没有重写单目实现的细节，所以对于 ORB-SLAM2 的学习还是要从第一篇 ORB-SLAM 的论文入手。以下内容均以 ORB-SLAM 单目部分为基础。

一些铺垫

ORB-SLAM2 中的实体对象

ORB-SLAM2 中的实体对象包括：

Frame
KeyFrame
MapPoint
Map
KeyFrame Database

它们之间的关系是这样的：

每一个送入系统的视频帧都会构造一个 Frame
Frame 中比较重要的会设为 KeyFrame
每个 Frame 会提取很多 ORB 特征点，每一个 ORB 特征点可能会对应一个 MapPoint
同一个 MapPoint 会对应多个不同 Frame 中 ORB 特征
KeyFrames 和 MapPoints 构成了 Map
重要的 KeyFrame 会存入 KeyFrame Database，用于回环检测和重定位

博文中可能出现的简称

KF：表示 KeyFrame
Current KF：表示当前 KeyFrame
Covisible KF：表示在 Covisibility Graph 中与 Current KF 相连的 KFs
Loop KF：表示回环 KeyFrame，即和 Current KF 相匹配的 KeyFrame Database 中的 KF
Candidate KF：表示可能是 Current KF 的 Loop KF 的候选 KF
Reference KF：表示距离当前帧最近的上一 KF
KF Database：表示用来存储 KFs 的 Database，这些 KFs 会用于回环检测和重定位

ORB-SLAM2 系统概览

下面我会从论文和代码（程序导图）两个角度出发，对 ORB-SLAM2 系统进行一个概览。

以论文为参考

ORB-SLAM 论文中，有一张图非常经典且重要：

ORB-SLAM 系统同时运行三个线程：

Tracking 线程：
- 对于新读取的帧，提取 ORB 特征
- （系统初始化）
- 相机位姿初值估计（根据上一帧 + motion-only BA，或进行重定位）
- 局部地图跟踪
  - 对上一步得到的位姿初值进行进一步 BA 优化
  - 局部地图：指 Covisibility Graph 中附近的 KFs 及其 MapPoints 所组成的局部的地图
- 决定是否将该帧作为关键帧插入 LocalMapping 线程
LocalMapping 线程：
- 接收从 Tracking 线程插入的 KF，并进行预处理
- 剔除质量较差的 MapPoints
- 通过三角化生成新的 MapPoints
  - Current KF 未与现有 MapPoints 匹配的 ORB 特征点与其 Covisible KFs 的特征点进行匹配，并三角化
- Local BA
- 剔除冗余的局部关键帧
LoopClosing 线程
- 接收 LoopClosing 送来的筛选处理后的 KF
- 检测出一批 Candidate KFs
- 计算 Sim3，确定最终的 Loop KF
- 进行回环融合
- 优化 Essential Graph

其中 Covisibility Graph 指的是：其节点为所有 KF。一个 KF，如果它与另一个 KF 观测到的相同的 MapPoints 的数量大于15个，则这两个 KFs 之间有边相连。从而组成了 Covisibility Graph。同时，Covisibility Graph 中的每条边有权重，权重即为两个 KFs 共同观测到的 MapPoints 数目。

其中 Essential Graph 指的是：系统会构造一个生成树。当一个新的 KF 插入时，将它与(与它观测到相同的 MapPoints 的数量最多的 KF)相连，从而得到一个生成树。Essential Graph = 该生成树 + Covisibility Graph 中权重大于100的边。

从上图可以看出，Covisibility Graph 中的边很多，生成树就是一条线，而 Essential Graph 介于两者之间。

以代码（程序导图）为参考

上面论文里那张框架图概括的非常好，但是这张图还是太简略了，光看它也搞不懂每个步骤之间的逻辑关系是怎样。而它们之间复杂的逻辑关系用大段文字更是很难描述清楚。在通读代码的时候，我经常读着读着就忘了前面读了什么，忘了现在读的部分是嵌套在哪里循环里，是属于哪个子函数。于是我想了个办法 —— 用思维导图来梳理代码的结构和逻辑。试验过后，我觉得这个办法很好用。此处献上我梳理的非常非常大的 ORB-SLAM2 程序导图：

（就是下方白花花的这一片，大家用鼠标多上下左右划划就能看到导图的主体部分了）

这么看实在是不方便，没事，这里放下程序导图的链接：ORB-SLAM2 程序框图，这个导图梳理的挺详细的了，希望它能对大家有帮助。

（我用的 MindMaster 这款画思维导图的软件，Windows Linux ios Andriod 都支持，同时可以将导图存储在个人云上多设备共享，另外也支持生成并分享导图链接）

我绘制的程序导图是以 mono_tum.cc 程序为入口，该程序是官方提供的 example，其对 TUM 数据集中的视频序列进行 SLAM。根据该程序，我们可以清晰地看出该怎么调用整个 ORB-SLAM2 系统。

ORB-SLAM2 系统以 System.cc 为系统的入口，其负责创建各种对象，同时创建 Tracking，LocalMapping, LoopCLosing 三个线程并运行。其中，System::TrackMonocular()是启动 Tracking 线程的入口。Tracking 线程为主线程，而 LocalMapping 和 LoopClosing 线程是通过 new thread 创建的。

关于 Tracking，LocalMapping, LoopCLosing 三个线程的具体内容，我会在以后的博文中进行更为详细的介绍和梳理。

其它相关博文

ORB-SLAM2 初体验 —— 配置安装

ORB

ORB-SLAM2 论文&代码学习 —— 概览