ORB-SLAM源码解读

第一部分：总体框架

本部分，我们从总体上了解整个 ORB-SLAM2 的设计以及相关的代码结构，为后续深入分析每个类和函数找些依据。我们从编译安装试运行开始， 根据编译脚本和例程找到整个系统的入口，找出其中涉及到系统运行的几个关键的数据结构进行详细分析。

ORB-SLAM2的总体框架与安装试用	本文我们简单分析它的总体框架，介绍如何源码编译这个工具库，并在TUM数据集上运行官方的单目和rgb-d的demo。 以后我们将从这两个demo出发深入研究ORB-SLAM算法的机理和实现。
初看单目和RGBD例程	本文我们将简单介绍一下 TUM 数据集。然后从 CMakeLists.txt 开始找到单目和 RGB-D 例程的入口文件， 发现 ORB_SLAM2 通过一个 System 的对象完成SLAM任务。另外为了方便下载 TUM 数据集， 我上传了一个百度网盘(提取码:btku)。
系统对象——System	本文我们将总体上查看一下 System 类型的成员变量和成员函数。可以看到针对TRACKING, LOCAL MAPPING, LOOP CLOSING三项子任务 ORB-SLAM2 专门设计了轨迹跟踪器、局部地图管理器和闭环探测器。
轨迹跟踪器——Tracking	本文中，我们从总体上解一下 Tracking 类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的跟踪过程我们将在第二部分中进行分析。
局部地图管理器——LocalMapping	本文中，我们从总体上解一下 ORB_SLAM2::LocalMapping 这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的地图管理过程我们将在第三部分中进行分析。
闭环探测器——LoopClosing	本文中，我们从总体上解一下 ORB_SLAM2::LoopClosing 这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的闭环探测和全局优化过程我们将在第四部分中进行分析。

第二部分：TRACKING——轨迹跟踪过程

轨迹跟踪的主要目的是估计相机的位姿，很多vSLAM系统都将完成这一任务的部分称为视觉里程计。ORB-SLAM2 中在一个独立的线程 TRACKING 中完成这一任务， 右图是从 ORB-SLAM论文 中抠下的系统框图。

该线程主要涉及到五个环节: ORB特征点提取、地图初始化、相机位姿估计、根据局部地图优化位姿、生成关键帧。 其中 ORB特征点提取我们放到附录中介绍。 由于在 ORB-SLAM2 中单目相机、深度相机、双目相机的处理过程大同小异，所以本部分以单目例程为主介绍剩余的四个环节。 最后专门用一篇文章来介绍深度相机和双目相机的相关改动。 这里，我们只研究定位建图的过程，关于纯定位的工作模式，本部分一律省略。

轨迹跟踪的总体过程

本文中，我们将结合2015年的论文和代码从总体上了解轨迹跟踪的具体过程。虽然针对三种不同类型的相机，在类型 Tracking 中分别有各自的接口，但最终都归结到成员函数 Track() 中完成跟踪任务。 拆解函数 Track() 的过程中，本文简要讨论了五个环节的主要工作内容。

对于单目相机而言，需要根据具有一定视差的前后两帧图像，估计出相机的单应矩阵或者基础矩阵，进而推测出这两帧之间的相对变换关系，建立一个相对的初始尺度关系。 以这两帧中较早的那一帧为参考，建立世界坐标系，完成相机和地图的初始化。

帧间位姿估计和地图初始化总体过程	本文中，我们重点分析单目初始化的基本原理及其实现方式，并简单讨论一下深度相机的初始化过程。实际上单目相机的初始化问题， 可以归结为计算前后两帧图像的帧间位姿估计。
单应矩阵的求解过程	本文中，我们结合代码和论文分析单目相机初始化过程中，单应矩阵的计算及其奇异值分解方法，从而获得帧间位姿的详细过程。
基础矩阵的求解过程	本文中，我们结合代码和论文分析单目相机初始化过程中，基础矩阵的计算方法及其奇异值分解过程获得帧间位姿的详细过程。
三角测量	我们已经了解到单应矩阵恢复出的相机位姿有8组解，基础矩阵将给出4组解。Initializer 还会通过函数 CheckRT 对匹配点进行三角化， 来逐一检查这些解，选取其中最优的那个。本文中，我们将详细分析该函数，介绍三角测量方法，计算匹配点所对应的空间点坐标，以及筛选过程。

ORB-SLAM2 提供了三种估计相机位姿的方式，在正常情况下以匀速运动模型估计相机位姿；如果跟丢了，先通过参考关键帧估计相机位姿； 如果这样无法恢复，则尝试进行重定位。重定位的过程可以看做是从历史的关键帧中搜索出一个最有希望的作为新的参考关键帧估计相机位姿。

匀速运动模型估计相机位姿	本文中，我们将介绍匀速运动模型，并详细分析其位姿估计过程。先根据匀速运动模型估计一个相机位姿，再通过投影关系搜索匹配特征点。 最后使用 g2o 以图优化的方式进一步优化相机位姿，剔除误匹配的特征点。
词袋模型与重定位	本文中先了解一下词袋模型的基本思想，再结合代码分析函数 Relocalization 的实现过程。 然后，分析 ORB-SLAM2 所用的 DBoW2 的词袋模型基本结构，并研究候选关键帧的计算过程。
求解PnP问题	在重定位过程中，ORB-SLAM2 通过当前帧与候选帧的匹配建立了 3D 地图点到 2D 图像特征点的映射关系，并通过 PnPsolver 求解了相机的位姿。 本文中，我们详细分析一下 ORB-SLAM2 所用的 PnP 求解方法。

前端里程计的一些其它细节。

根据局部地图优化位姿	完成相机位姿的初始估计之后，Tracking线程又通过共视关系构建了局部地图，并将其中的地图点投影到当前帧中，通过一系列的条件进行筛选， 获得更多的匹配地图点，并根据这些地图点再次优化相机的位姿估计。
生成关键帧	根据局部地图进一步优化了相机位姿之后，如果Tracking仍然没有跟丢，那么在满足一些限制条件的情况下就可以生成新的关键帧了。
针对双目和深度相机的调整(TRACKING)	针对双目相机 ORB-SLAM2 以左目为基准提取特征点，并将之分为 Stereo 和 Monocular 两类。针对深度相机，ORB-SLAM2 选择将之模拟成双目相机。 本文中我们将详细查看这两类传感器数据的预处理过程、系统的初始化过程、以及 Stereo 类型的特征点构建的边约束。

第三部分：LOCAL MAPPING——局部建图

局部建图的任务是处理由轨迹跟踪生成的关键帧，并进行局部的BA优化(local BA)，获得一个对当前相机附近的环境比较好的稀疏重建结果。 针对TRACKING线程生成的新关键帧中没有匹配的特征点，LOCAL MAPPING线程还会在共视图中相关的关键帧中进行匹配搜索，进而实现这些特征点的三角化得到它们的空间坐标。 此外为了保证系统的运行效率不因时间的推移和场景的扩大而严重降低，该线程还会对地图点和关键帧进行比较严格的筛选，抛弃一些质量较差或者冗余的地图点和关键帧。

右图是从ORB-SLAM论文中抠下的关于该线程的系统框图。 它的输入KeyFrame就是TRACKING线程的输出，整个线程有5个子任务，分别完成了新关键帧的插入、地图点的筛选、新地图点的生成、局部BA优化以及关键帧的筛选。

局部建图的总体过程	本文中，我们结合2015年的论文和代码从总体上分析局部建图的具体过程。整个线程有5个子任务，分别完成了新关键帧的插入、地图点的筛选、新地图点的生成、局部BA优化以及关键帧的筛选。
插入新关键帧和筛选地图点	LOCAL MAPPING 线程以 TRACKING 线程生成的关键帧为输入，先将其插入到共视图中合适的位置上，并根据共视关系建立连边。 再对最近生成的地图点进行筛选，达到降低测量噪声的影响，增加系统可靠性和效率的目的。
新地图点生成	对轨迹跟踪环节没有成功定位的 ORB 特征点，LocalMapping 会在共视图中再次进行匹配，然后通过 DLT 算法计算匹配的特征点坐标，最后经过一系列严苛的筛选，生成质量过关的地图点。
局部BA优化	我们终于要面对 BA 算法了，其本质是把地图的优化问题描述成非线性最小二乘问题，再通过迭代求解的方法完成。 据说这个知识点劝退了很多初学者，其实求解器 g2o 已经隐藏了大量的数学细节，只要了解了其基本思想，照着葫芦画瓢我们也能写出不错的工程代码来。 为了缓解初学者的焦虑，本文特意没有列写任何数学公式。
局部地图中关键帧筛选 & LocalMapping 针对双目/深度相机的调整	局部建图的最后一个环节是关键帧筛选，剔除掉那些冗余的关键帧。其实并没有什么特别的内容，就是其字面意思。 此外，由于 LocalMapping 主要是针对关键帧和地图点进行的操作，不再直接处理图像，所以并没有过多的针对双目/深度相机的调整。 原本是要写两篇文章的，但是内容实在太少了，分开写太水了，所以就放一块儿写了。

第四部分：LOOP CLOSING——闭环探测

闭环探测的主要作用是，降低前端里程计的累计误差。通过判定机器人是否回到了曾经到过的地方，在共视图中建立闭环约束。再对整个地图进行 BA 优化， 将较大的累计误差均摊到闭环的各个关键帧上。

ORB-SLAM2 在一个独立的线程 LOOP CLOSING 中对每个新生成的关键帧进行闭环检测，并根据 Essential Graph 进行闭环修正。 该线程还会开启第四个线程 FULL BA，对整个地图进行优化，获得数据意义上最优的地图点和相机运动轨迹的估计。

闭环探测的总体过程	2015年发布的ORB-SLAM在检测到闭环之后只根据基图完成了位姿图优化。2017年ORB-SLAM2新增了一个 Full BA 的线程，对所有的关键帧和地图点进行全局优化， 然后根据生成树将优化结果传播给新增的关键帧和地图点。
计算候选闭环关键帧	通过词袋模型评价两个关键帧之间的相似性，取当前帧与其一级邻接关键帧的最低相似度，作为初步筛选候选闭环关键帧的阈值。为了降低闭环的误检率， 还根据一致共视组(ConsistentGroup)的概念对这些候选帧进行筛选。
计算相似变换SE3	接下来我们需要估计这些候选帧与当前帧之间的位姿关系，建立几何约束进一步筛选候选关键帧。 由于单目相机存在尺度不确定的问题，除了需要估计三轴平移和三轴旋转之外，还需要估计尺度，也就是所谓的相似变换(SE3)。
闭环修正	实际上，闭环修正的核心工作内容只有三个：① 计算当前帧及其一级邻接关键帧的修正相似变换；② 对当前帧及其一级邻接中观测到的地图点进行融合，建立闭环关系；③ 完成基图的图优化，更新地图。
全局优化	本文中我们研究 ORB-SLAM2 中最后一个优化。它并没有什么具体的知识点，其核心的 BA 图优化， 我们已经在前文[1, 2, 3]中多次见到，套路应该很熟了。

附录

ORB特征点

ORB 特征点的全称是 Oriented FAST and Rotated BRIEF，是一种局部的特征点。 它具有较好的旋转不变性和抗噪声能力，而且它很快，实验表明它要比SIFT算法快上两个数量级，在很多情形下都能取得近似SIFT算法的效果。