ORB-SLAM: a Versatile and Accurate Monocular SLAM System 翻译&学习

Abstarct

ORB-SLAM是一个基于特征的单目SLAM系统，可以实时的运行在小尺度或大尺度、室内或室外环境。该系统对于剧烈运动有较好的鲁棒性，可以有比较大的余地自由处理闭环控制、重定位、甚至全自动位置初始化。 基于最近几年的优秀算法，作者从基础开始设计了一种对所有SLAM任务使用相同功能的新系统：跟踪、地图构建、重定位和回环检测。一个有关于点云和重建关键帧的最优选择策略带来了较好的鲁棒性，并且生成了一个紧凑的和可追踪的地图，该地图只有在场景内容改变时更新，允许地图的可持续性构建。作者从最流行的数据集中对27个序列进行了详尽的评价。与其他最先进的单目SLAM方法相比，ORB-SLAM实现了前所未有的性能。作者公布了其源码: https://github.com/raulmur/ORB_SLAM

INTRODUCTION

BA可以用于精准的相机位姿估计和稀疏的几何重建，但是由于强大的匹配网络并且 需要提供良好的初始猜测，所以很长时间以来人们认为BA优化是不可以用于像视觉SLAM这种实时应用的。作者提出了在SLAM中可用的BA：

• 在候选图像帧子集中（关键帧）匹配观测的场景特征（地图云点）
• 复杂度随着关键帧数量的增加而增加，需要进行筛选以避免不必要的冗余
• 一个强大的关于关键帧和点云的网络配置就可以产生准确的结果，即关键帧和点的强大网络配置可产生准确的结果，即，分布良好的关键帧集可观察到具有明显视差并具有大量闭环匹配的点
• 关键帧和云点位置的初始估计，采用非线性优化的方法
• 在构建局部地图的过程中，优化的关键是获得良好的可量测性
• 具有实时进行快速全局优化回环的能力

在首先在实时应用中使用BA的是Mouragon等人的visual odometry，首先在SLAM中使用BA的是 Klein and Murray 的PTAM（ParallelTracking and Mapping）。但是PTAM中用的算法仅限于小尺度的操作中，提供简单但是高效的的方法用于关键帧的选取、特征匹配、云点三角化，每帧相机位置估计，追踪失败后的重定位非常有效。但是不幸的是几个因素限制了它的应用：缺少回环控制和针对闭塞的充分处理， 较差的视图不变特性和在形成地图过程中需要人工干预。

在本文中，作者基于PTAM的主要观点以及 place recognization、scale-aware loop closing和大场景的视图关联信息 设计了ORB-SLAM。

ORB-SLAM的主要贡献有：

• 在所有的任务中使用相同的特征：追踪、地图构建、重定位和回环检测。使用相同的特征使得ORB-SLAM更加高效简单和可靠。作者使用了ORB 特征，该特征可以在无需GPU的情况下实现实时计算，具有很好的旋转和光照不变性。

• 在大尺度的环境中也可以实现实时。这得益于共视图的使用，追踪、地图构建都可以只关注于局部的共视区域，可以独立于全局尺度的地图。

  共视图是具有内容关联性的视图

• 实时的回环检测。这个是基于一个作者称为*“Essential Graph”*的位姿图实现的，它是从由系统维护的生成树、回路闭合链接和共视图的强边缘构建的。

• 实时的相机重定位，并且具有显著的旋转和光照不变性。这就允许从跟踪失败中恢复，并且还增强了地图重用。

• 一个自动的、鲁棒的基于模型选择的初始化程序。可以允许在平面或者非平面的初始化地图 。

• 一种适者生存的映射点和关键帧选择方法，在选取时非常的宽松，但是会有严格的策略进行剔除。该策略提高了追踪的鲁棒性，并增强了长时间操作，因为冗余关键帧被丢弃。

作者在流行的公共数据集上做了测试，取得了很好成绩。闭环和重定位的方法是基于作者之前的论文：Fast relocalisation and loop closing in keyframe-based SLAM ，最初的版本是发表在作者的ORB-SLAM: Tracking and mapping recognizable features这篇论文上的，本论文主要是新增了初始化方法、用于回环检测的Essential Graph和其他的关键方法。

RELATED WORK

位置识别

目前基于图形处理技术的位置识别方法有图像到图像的匹配、地图到地图的匹配和图像到地图的匹配。其中图像到图像的匹配在大环境下相比其他两者拥有更好地尺度特性。在图像方法中词袋模型（Bow算法）的效率更高，比如说基于概率论的FAB-MAP。 由BRIEF特征描述子和FAST特征检测产生的二进制词袋可以用DBoW2获得。与SURF和SIFT相比，它的特征提取运算时间减小一个数量级。尽管系统运行效率高、鲁棒性好，采用BRIEF不具有旋转不变性和尺度不变性，系统只能运行在平面轨迹中，闭环检测也只能从相似的视角中获得。 作者用DBoW2生成了基于ORB的词袋模型位置识别器。ORB是具有旋转不变和尺度不变特性的二进制特征，它是一种高效的具有良好针对视图不变的识别器。作者提出了一种改进版本的位置识别方法，采用内容相关的视图，检索数据库时返回几个前提而不是最好的匹配。

BoW算法，就是Bags of Words，是CNN大火之前的王者，现在的过气网红了 = =。 用一个方法（BRIEF、FAST、SURF、SIFT etc.）获取图像中的特征点（单词）,把最具代表性的单词（visual words/vocabulary）罗列出来，构造成一个字典（codebook）。 之后，我们可以把单张图像上的所有特征点和这些vocabulary做对比， 如果这个特征点和某个vocabulary 相似， 就把这个vocabulary的计数+1 ，遍历所有特征点就能得到这个图像的所有频率分布，遍历特征点获得频率分布，我们叫做quantization. 先这样处理training set， 得到已知类别（class）的频率分布，然后扔到任何一个multicalss classifier 里。再处理test set， 得到频率分布以后，把频率分布扔到上一个模型里做预测。在选取具有代表性的单词时，可以使用K-means.

随着图像数据的增加，为了改进大规模图像检索场景下的检索效果，有人提出了 Vocabulary Tree （VT）算法。ORB-SLAM使用的就是VT.

产生词典树形结构的步骤：
1、从训练图像中离线抽取特征
2、将抽取的特征用 k-means++ 算法聚类，将描述子空间划分成 k 类
3、将划分的每个子空间，继续利用 k-means++ 算法做聚类
4、按照上述循环，将描述子建立树形结构，如下图所示：

字典树在建立过程中，每个叶子也就是每个 word 记录了该 word 在所有的训练图像中出现的频率，出现的频率越高，表示这个 word 的区分度越小，频率的计算公式如下
$$idf(i)=\log \frac{N}{n_i}$$
当在字典树中需要插入一幅新图像$I_t$ ，在图像中提取的特征描述子按照 Hamming 距离从字典树的根部节点开始逐级向下到达叶子节点，可以计算每个叶子节点也就是每个 word 在图像$I_t$ 中的出现频率：

$$tf(i,I_t)=\frac{n_{i{I}_t}}{n_{I_t}}$$
其中$n_{i{I}_t}$ 代表word在图像中出现的次数，$n_{I_t}$代表图像中的特征子的总数。

在树构建的过程中每个叶子节点存储了 inverse index，存储了到达叶子节点的图像$I_t$ 的 ID 和 图像 It描述 vector 中第 i维(第i个words）的值：
$$v_t^i=tf(i,I_t)\times idf(i)$$
对于一幅图像的所有描述子，作上述操作，可以得到每个Word的值，然后这些值就组成了图像的描述向量$v_t$

对两幅图像比较计算其相似度时，两幅图像相似度计算公式如下：
$$s(v_1,v_2)=1-\frac{1}{2}|\frac{v_1}{|v_1|}-\frac{v_2}{|v_2|}|$$
两幅图像越相似得分越高。

字典树除了存储了 inverse index，还存储了 direct index 如下图所示(这是两种不同的存储direct index的数据结构），direct index 方便两幅图像特征搜索，建立特征之间的对应，计算两帧之间的位姿转换。

More details：

• https://blog.csdn.net/fuxingyin/article/details/51489160

• https://blog.csdn.net/qq_24893115/article/details/52629248

地图初始化

单目SLAM需要一个过程来创建初始地图，因为深度不能从单个图像中恢复。解决这个问题的一种方法是首先跟踪一个已知的结构。在滤波方法的背景下，可以使用反向深度参数化来初始化深度高度不确定的点，该参数化有望稍后收敛到它们的真实位置。例如 Engel的工作，将像素深度初始化为具有高方差的随机值。

通过两个局部平面场景视图进行初始化的方法，从两个相关相机（视图）位姿进行3D重构，相机的位姿关系用单映射表示，或者计算一个基本矩阵，通过5点算法构建平面模型或者一般场景模型。两种方法都不会受到低视差的约束，平面上的所有的点也不需要靠近相机中心。另外，非平面场景可以通过线性8点算法来计算基本矩阵，相机的位姿也可以重构。 如果平面场景的所有点都靠近相机中心之一，则这两种重建方法在低视差下都不会受到很好的约束，并且会遇到双重歧义解。另一方面，如果看到具有视差的非平面场景，可以用八点算法计算唯一的基本矩阵，并且可以毫无歧义地恢复相对相机姿态。

作者提出了一种新的基于平面的单应矩阵或者非平面的基本矩阵的自动化方法用于地图初始化。 基于相似变换理论，作者开发了初始化算法，选择退化二次曲线例子中基本矩阵，或单应矩阵。在平面例子中，为了程序稳定性，如果选择情况模糊不清，应该尽量避免做初始化，否则方案可能崩溃。作者选择在这种情况下延迟初始化过程，直到所选的方案产生明显的视差。

单目SLAM

最初的单目SLAM是用滤波来解决问题的。在这种方法中，每一帧都会被滤波处理以估计地图特征点的位置以及相机的位姿。这种方法有一个严重的缺点就是会浪费大量的计算用于处理没有多少新信息的连续帧，并且会产生线性的累积误差。而基于关键帧的方法，则是使用选取的关键帧用于地图估计，这种方法的执行成本更低并且使用BA约束带来了更好地精度，而且不依赖于帧率。Strasdat等人的论文证明了基于关键帧的技术优于在连续帧上使用滤波的方法。

最具有代表性的基于关键帧的SLAM恐怕就是 Klein 和 Murray的PTAM了，在PTAM中首次提出了相机追踪和地图构建分开，并行计算，在小型场合，如增强现实领域非常成功。PTAM中的地图云点通过图像区块与FAST角点匹配。云点适合追踪但不适合位置识别。实际上，PTAM并不适合检测大的闭合回路，重定位基于低分辨率的关键帧小图像块，对视图不变性较差。

Strasdat发表的大尺度的单目SLAM系统。前端是基于在GPU上实现的光流追踪，使用FAST特征进行匹配和运动BA 。在后端上使用基于滑动窗口的BA。 闭环检测通过7自由度约束的相似变换位姿图优化，能够校正单目系统中的尺度偏移。ORB- SLAM也使用了具有7自由度的位姿图优化，将它应用到了Essential Graph中。

接下来的一些单目SLAM的Related Work，批判一番：

Strasdat的PTAM前端，通过内容相关的视图提取局部地图执行追踪。他们使用两个窗口优化后端，在内部窗口中采用BA，在外部一个限制大小的窗口做位姿图。只有在外部窗口尺寸足够大可以包含整个闭环回路的情况下，闭环控制才能起作用。我们采用了基于内容相关的视图构建局部地图的方法，并且通过内容相关的视图构建位姿图，同时用它们设计前端和后端。另一个区别是，我们并没有用特别的特征提取方法做闭合回路检测（比如SURF方法），而是在相同的追踪和建图的特征上进行位置识别，获得具有鲁棒性的重定位和闭环检测。

Pirker等人搞得 D-SLAM方法，一个非常复杂的系统，包括闭环控制，重定位，动态环境、大场景下运行。但它的地图初始化并没有讲。所以没法做精确性、鲁棒性和大场景下的测试对比。

Song等人的 视觉里程计方法使用了ORB特征做追踪，处理BA后端滑动窗口。相比之下，我们的方法更具一般性，他们没有全局重定位，闭环回路控制，地图也不能重用。他们使用了相机到地面的真实距离限制单目尺度漂移。

Lim这个人的工作和我们之前版本的工作一样， 也采用的相同的特征做追踪，地图构建和闭环检测。由于选择BRIEF，系统受限于平面轨迹。从上一帧追踪云点，访问过的地图不能重用，与视觉里程计很像，因此系统不能扩展。

Engel 最近搞得这个LSD-SLAM， 可以构建大场景的半稠密地图，特征提取并没有采用BA方法，而是直接方法（优化也是直接通过图像像素）。没有用GPU加速，构建了半稠密地图，可以运行在实时应用中，非常适合机器人应用，比其他基于特征的稀疏地图SLAM效果好。但它们仍然需要特征做闭环检测，相机定位的精度也明显比PTAM和我们的系统慢。

Forster 提出了介于直接方式和基于特征的方法之间的半直接视觉里程SVO方法。不需要每帧都提取特征点，可以运行在较高的帧率下，在四轴飞行器上效果很好。然而，没有闭环检测，而且只使用了视角向下的摄像头。

最后，作者想讨论一下关于关键帧选取的问题。用所有的云点和图像帧搞BA肯定是不行的。Strasdat等人的PTAM虽然在他们的方法中保留了尽可能多云点和非冗余的关键帧。但是Strasdat的PTAM在插入关键帧非常谨慎以避免运算量增大的方法可能使在未知地图上的追踪失败。作者提出了一种比较好的策略: 在插入关键帧的时候使用比较宽松的限制以实现快速插入，然后再移除那些冗余的图像帧，避免额外的运算成本 。

系统概述

特征选择

ORB-SLAM的一个主要设计思想就是在不同的任务中使用相同的特征，包括构建地图、追踪、位置识别、基于图像帧率的重定位和回环检测。使用同样的特征让系统更加高效并且避免了极化特征识别的深度图。作者要求这个特征能够满足每帧的提取速度小于33ms，因此SIFT(约300ms)、SURF（约300ms）和A-KAZE（约100ms）这些都不行。而且作者为了获得一般的位置识别能力，因此还需要旋转不变性，所以BRIEF和LDB也不行。

最终作者选择了ORB， 是一个旋转多尺度FAST角点检测具有256位特征描述子。 计算速度贼拉快，同时还有旋转不变的特性 ，这就可以在更宽的基准线上匹配他们，增强了BA的精度。

三线程：追踪，局部地图构建和回环检测

ORB-SLAM的构架，如上图所示，包含三个并行运行的线程:跟踪、局部地图构建和回环检测。追踪负责使用每一帧图片对相机进行定位，并且决定哪一张图片应该被插入关键帧中。先通过前一帧图像帧初始化特征匹配，再采用运动B A优化位姿 。如果追踪丢失的话（由于遮挡或者突然移动），位置识别模块就执行全局重定位。 一旦获得最初的相机位姿估计和特征匹配 ，就可以使用系统中存储的关键帧的共视图得到一个局部的可视化地图 ，如图2a和2b所示。然后使用重投影搜索来和局部地图点云匹配，相机位姿再根据所有的匹配进行优化。最终追踪线程将会决定是否插入个新的关键帧。 所有的追踪步骤将在第五章详细解释。创建初始化地图将在第四章进行说明。

局部地图构建负责处理新的关键帧并且在相机位姿的环境下实施局部BA以实现更好地重建。 根据共视图中已经连接的关键帧，搜索新关键帧中未匹配的ORB特征的对应关系，来三角化新的云点。 有时在创建了新的云点后，基于在追踪中得到的信息实施一个紧急云点剔除策略（ exigent point culling policy）来保证只留下高质量的云点。局部地图构建同时也负责剔除冗余关键帧，具体的细节会在第六章中详细解释。

回环检测是用于搜索每一个新的关键帧中是否出现了回环。如果检测到回环的话，就计算相似变换来得到回环中累积的漂移。然后将回环两侧对齐，重复的点云进行融合。最后利用一个基于相似性约束的位姿图优化以取得更好地全局一致性。ORB-SLAM的创新之处在于使用Essential Graph进行优化，这个Essential Graph 是共视图的稀疏子图，具体会在第三章D节进行介绍。回环检测和矫正的步骤会在第七章详细解释。

作者使用在g2o中实现的Levenberg-Marquardt算法来执行所有优化。在附录中，描述了每个优化中涉及的误差项、成本函数和变量。

地图云点，关键帧和它们的选取

每一个地图云点$p_i$存储了:

• 世界坐标系下的三维位置信息$X_{w,i}$
• 观察方向（ viewing direction）$n_i$, 它是所有观察方向的平均单位向量 （这个方向是以观察关键帧的光学中心到云点的方向）
• 代表ORB特征描述子（ representative ORB descriptor）$D_i$,它是关键帧中被观察云点的所有关联ORB描述子（ associated ORB descriptor）中hamming距离最小的
• 根据ORB特征尺度不变性约束，所有能够被观察到的云点中的最大距离$d_{max}$和最小距离$d_{min}$

在每个关键帧$K_i$中存储了：

• 相机位姿$T_{iw}$,它可以用在刚体变换中把世界坐标系转换为相机坐标系。
• 相机内参，包括焦距和主点。
• 所有从该帧中提取的ORB特征，如果提供了失真模型，则其坐标不会失真，无论是否和云点关联。

云点和关键帧的选取是一个慷慨策略( generous policy ) ,随后会使用紧急剔除策略来侦测和剔除冗余关键帧以及错误匹配或无法追踪的地图点。这种方式允许在探索过程中灵活地拓展地图，从而提高了恶劣条件下的跟踪稳定性（例如旋转，快速移动），同时它的大小在重复访问相同场景时也是有限的。此外，与PTAM相比，我们的地图包含的异常值很少，但这是以包含更少的点为代价的。地图云点和关键帧的剔除过程分别在第六章B节和E节中进行了说明。

共视图和基本图

关键帧之间的共视信息在我们系统中的几个任务中有非常重要用处，并表示为无向加权图，如论文 7。无向加权图的每个节点是关键帧，如果两关键帧之间存在共视的地图云点（至少15个）的话则有边相连，每条边的权重θ代表两关键帧之间相同云点的个数。

为了纠正回环，我们使用了位姿图优化，他可以回环误差平均到位姿图上。为了不包括共视图提供的所有边缘（可能非常密集），我们建议构建一个基本图，该图保留所有节点（关键帧），但保留较少的边缘，这依然是一个能产生精确结果的强大网络。系统从初始关键帧开始逐步构建生成树，该树提供具有最小边数的共视图的连接子图。当一个新的关键帧被插入后，关键帧会被添加到树中，并且连接到和该关键帧拥有最多共视点的关键帧上，当一个关键帧被剔除时，系统会更新受影响的链接。基本图当中包含了生成树、具有高可视性(${\theta }_{min}=100$)的共视图的边缘子图以及闭环回路边缘产生一个相机的强网络。图2 是共视图、生成树和相关的基础图的一个例子。 第8章E节的实验里，运行位姿图优化时，方案效果精确BA优化几乎没有增强系统效果。关键图像的效果和${\theta }_{min}$的效果如第8章E节所示。

基于词袋模型的位置识别

系统内置了基于DBoW2的词袋位置识别模块,执行回环识别和重定位。 视觉单词离散分布于特征描述子空间，视觉单词(Visual words)组成视觉字典。视觉字典是离线创建的，用ORB特征描述子从大量图像中提取。如果图像都比较多，相同的视觉字典在不同的环境下也能获得很好的性能，如论文11那样。系统增量式地构建一个数据库，包括一个逆序指针，存储每个视觉字典里的视觉单词，关键帧可以通过视觉字典查看，所以检索数据库效率比较高。当关键帧通过筛选程序删除时，数据库也会更新。

由于关键帧之间存在视觉重叠，因此在查询数据库时，不会存在得分较高的唯一关键。原始的DBoW2考虑到了这种重叠，将时间上接近的图像得分相加。这有一个局限性，即不包括查看同一位置但在不同时间插入的关键帧。于是我们将共视图中连接的那些关键帧分组。此外，我们的数据库还返回所有得分高于最佳得分75％的关键帧匹配项。

词袋模型表示的特征匹配的另外一个优势在论文5里详细介绍。如果我们想计算两个ORB特征的对应关系，我们可以通过暴力匹配视觉字典树上某一层（6层里面选第2层）的相同节点（关键帧）里的特征，这可以加快搜索。在闭环回路检测和重定位中，我们通过这个方法搜索匹配用作三角化新的云点。我们还通过方向一致性测试改进对应关系，具体如论文11，这可以去掉无效数据，保证所有对应关系的内在方向性。

自动地图初始化

地图初始化的目标是计算两个帧之间的相对姿态，以对一组初始的地图点进行三角测量。这种方法应该独立于场景(平面的或一般的)，并且不需要人为干预来选择好的两视图配置, 比如具有明显视差 。我们建议并行计算两个几何模型，假设一个平面场景的单映射和非平面场景的基本矩阵。然后，我们使用启发式方法选择一个模型，并尝试用特定的方法恢复所选模型的相对姿态。 当两个视图之间的关系比较确定时，我们的方法才发挥作用，检测低视差的情况或已知两部分平面模糊的情况（如论文27所示），避免生成一个有缺陷的地图。这个算法的步骤是：

1、寻找初始对应：

提取当前帧 $F_c$中的ORB特征(仅以最精细的比例)，并在参考帧$F_r$中搜索匹配的 $x_c\leftrightarrow x_r$ 。如果没有找到足够的匹配，重置参考帧。

2、并行计算两种模型：

在并行进程中计算单应矩阵$H_{cr}$和基本矩阵$F_{cr}$:

$x_c=H_{cr}{x}_r$ $x_c^T{F}_{cr}{x}_r=0$

在《多视图几何》里详细解释了分别使用归一化直接线性变换DLT和8点算法计算原理，通过RANSAC计算。为了使两个模型的计算流程尽量一样，将两个模型的迭代循环次数预先设置成一样，每次迭代的云点也一样，8个基本矩阵，4个单映射。每次迭代我们都给每一个模型M（H表示单映射，F表示基本矩阵）计算一个分值$S_M$：
$$S_M=\sum _i\left({\rho }_M\left(d_{cr}^2\left({\mathbf{x}}_c^i,{\mathbf{x}}_r^i,M\right)\right)+{\rho }_M\left(d_{rc}^2\left({\mathbf{x}}_c^i,{\mathbf{x}}_r^i,M\right)\right)\right)$$

$${\rho }_M\left(d^2\right)=\{\begin{array}{ll}\Gamma -d^2& \text{ if }{d}^2<T_M\\ 0& \text{ if }{d}^2\ge T_M\end{array}$$

其中，$d_{cr}^2$和$d_{rc}^2$是帧和帧之间对称的传递误差。$T_M$是无效数据的排除阈值，它的依据是$X^2$的95%（$T_H$=5.99, $T_F$=3.84，假设在测量误差上有1个像素的标准偏差）。τ等于$T_H$，两个模型在有效数据上对于同一误差d的分值相同，同样使得运算流程保持一致。

我们保留得分最高的单应矩阵和基本矩阵。如果找不到模型（未找到足够的对应特征点），我们将从步骤1重新开始。

3、模型选择：

如果场景是平面的，接近平面的或视差较低，可以通过单应矩阵进行描述。 同样地，我们也可以找到一个基本矩阵，但问题不能很好地约束表示，从基本矩阵重构运动场景可能会导致错误的结果。我们应该选择单应矩阵作为重构的方法，它可以从二维图像正确初始化或者检测到的低视差的情况而不进行初始化工作。另外一方面，非平面场景有足够的视差可以通过基本矩阵来表示，但单应矩阵可以用来表示二维平面或者低视差的匹配子集。在这种情况下我们应该选择基本矩阵。我们用如下公式进行计算：
$$R_H=\frac{S_H}{S_H+S_F}$$
如果$R_H>0.45$的话学则单应矩阵，否则的话选择基础矩阵。

4、运动和从运动中恢复结构

选择模型后，我们将检索关联的运动假设。在单应矩阵的情况下，我们使用Faugeras等人的方法检索了8个运动假设。 这个方法通过测试选择有效的方案。 如果在低视差的情况下，云点跑到相机的前面或后面，测试就会出现错误从而选择一个错的方案。我们建议直接三角化8种方案，检查两个相机前面具有较少的重投影误差情况下，在视图低视差情况下是否大部分云点都可以看到。如果没有一个明确的方案胜出，我们就不执行初始化，重新从第一步开始。这种方法就提供了一个清晰的方案，在低视差和两个交叉的视图情况下，初始化程序更具鲁棒性，这是我们方案鲁棒性的关键所在。在基本矩阵的情况下，我们用校准矩阵和本证矩阵进行转换：
$${\mathbf{E}}_{rc}={\mathbf{K}}^T{\mathbf{F}}_{rc}\mathbf{K}$$
然后用论文2中解释的奇异值分解方法检索4个运动假设。我们对四个解进行三角剖分，然后使用处理单应矩阵时的方法进行处理。

5、捆绑调整（BA）

最后，我们执行完整的BA，以完善初始重构。

图3所示的论文39，室外环境下初始化工作具有很大挑战。PTAM和LSD-SLAM初始化二维平面所有云点，我们的方法是有足够视差才进行初始化，从基本矩阵正确地初始化。

追踪

这一章我们会描述使用从相机得到的每一帧图像进行追踪的步骤。相机位姿优化如前几步提到的，由局部BA构成。

A、ORB特征提取

我们以1.2的比例因子在8个尺度（金字塔结构）上提取FAST角点。对于512×384到752×480像素的图像分辨率，我们发现提取1000个角点比较合；对于更高的分辨率，例如KITTI数据集中的1241X376的分辨率，我们提取2000个角点。为了保证单应性分布我们把每个尺度划分成网格，视图在每个单元格至少提取五个角点。我们在每个单元格侦测角点，如果没有找到足够的角点则调整阈值。如果某些单元格没有找到角点（没有足够的纹理或者对比度不够）我们依然保留每个单元格的角点数量。在找到的FAST角点的基础上，我们计算方向和ORB特征算子。ORB特征算子将被用于所有的特征匹配中，而不是像PTAM那样通过斑块相关性进行搜索（ search by patch correlation ）。

B、从前一帧初始化位姿估计

如果上一帧的追踪成功，我们就用同样的速率运动模型计算相机位置，搜索上一帧观测到的地图云点。如果没有找到足够的匹配（比如，运动模型失效），我们就加大搜索范围搜索上一帧地图云点附近的点。通过寻找到的对应关系优化相机位姿 。

C、通过全局重定位初始化位姿估计

如果追踪丢失，我们把该帧转化为词袋模型并且检索数据库， 为全局重定位查找关键帧。我们计算ORB特征和每个关键帧的地图云点的对应关系，如第三章E节描述。接着，我们对每个关键帧执行RANSAC迭代计算，用PnP算法找出相机位置。如果通过足够的有效数据找到相机位姿，我们优化它的位姿，搜索候选关键帧的地图云点的更多的匹配。最后，相机位置被进一步优化，如果有足够的有效数据，跟踪程序将持续执行。

D、局部追踪地图

一旦我们有了相机文字的估计和一系列初始的特征匹配，我们就可以把地图投影到每一帧上并且搜索更多的地图云点对应关系了。为了减少大地图的复杂性，我们仅投影一个本地地图。此本地地图包含关键帧集${\mathcal{K}}_1$ ，他们和当前帧共享云点，以及存储和${\mathcal{K}}_1$有共视关系的集合${\mathcal{K}}_2$。本地地图同时有一个参考关键帧$K_{\mathrm{r}\mathrm{e}\mathrm{f}}\in {\mathcal{K}}_1$他是和当前帧共享最多云点的一帧。 针对${\mathcal{K}}_1$ 和${\mathcal{K}}_2$可见的每个地图云点，我们通过如下步骤，在当前帧中进行搜索：

1)在当前帧中计算云点投影$\mathbf{X}$。如果它不在图像范围内，则丢弃。

2）计算当前视角v和云点平均视角n之间的夹角。如果$\mathbf{v}\cdot \mathbf{n}<\cos \left(60^{\circ }\right)$的话则丢弃。

3）计算云点到相机中心的距离d。如果其超过尺度不变区域则丢弃，即$d\notin \left[d_{\min },d_{\max }\right]$。

4）计算当前帧的尺度$d/d_{\mathrm{m}\mathrm{i}\mathrm{n}}$

5）将地图点的特征描述子D与帧中仍不匹配的ORB特征（在预测比例和接近x处）进行比较，然后将地图点与最佳匹配相关联。

相机位姿最后通过当前帧中获得所有的地图云点进行优化。

E、新关键帧决定

最后一步是决定当前帧是否可以作为关键帧。局部地图构建的过程中有一个机制去筛选冗余的关键帧，我们尽可能快地插入关键帧，这可以使跟踪线程对相机的运动更具鲁棒性，尤其是旋转。我们根据以下要求插入新的关键帧：

1. 每次全局重定位过程需要超过20个图像帧。
2. 局部地图构建处于空闲状态，或者上一个关键帧插入时，已经有超过20个关键帧。
3. 当前帧跟踪至少50个地图云点。
4. 当前帧跟踪少于参考关键帧云点的90%。

与PTAM中用关键帧的距离作为判断标准不同，我们加入一个最小的视图变换，如条件4要求。条件1 确保一个好的重定位，条件3保证好的跟踪。当局部地图构建处于忙状态（条件2的后半部分）的同时插入关键帧的时候，就会发信号去暂停局部BA，这样就可以尽可能快地去处理新的关键帧。

局部地图构建

在本节中我们将介绍我们对每一个关键帧$K_i$实行局部地图构建的步骤。

A、关键帧插入

首先我们更新共视图，添加一个新的节点$K_i$并更新关键帧间具有相同地图云点产生的边缘。我们还要更新生成树上$K_i$和其他关键帧的链接。然后计算表示关键帧的词袋，用于数据关联来三角化新的云点。

B、地图云点筛选

能在地图中保留下来的云点必须通过一个严格的测试，最初创建的3个关键帧上的，这是为了确保其能被跟踪并且不被由于虚假数据关联导致的错误的三角化。一个云点必须满足一下两个条件：

1. 跟踪线程必须要找到超过25%的关键帧。
2. 如果超过一个关键帧完成地图云点创建过程，它必须至少是能够被其他3个关键帧可被观测到。

一旦一个地图云点通过测试，它只能在被少于3个关键帧观测到的情况下移除。在局部BA删除无效观测数据的情况下，关键帧才能被筛除掉。这个策略使得我们的地图包含很少的无效数据。

C、新地图云点创建

新的地图云点通过三角化在共视图中连接的关键帧集${\mathcal{K}}_C$ 的ORB特征实现。对于每个在${\mathcal{K}}_i$中 没有匹配的ORB 算子我们搜索在另一个帧中的未匹配云点来实现匹配。这个匹配过程就像我们在第三节E中所说的， 丢掉那些不满足极对约束的匹配。ORB特征对三角化后，将要获得新的云点，这时要检查两个相机视图的景深，视差，重映射误差，和尺度一致性。起初，一个地图云点通过2个关键帧进行观测，但它却是和其他关键帧匹配，所以它可以映射到其他相连的关键帧，按照第5章D节的方法搜索对应关系 。

D、局部BA

局部BA优化当前处理的关键帧${\mathcal{K}}_i$, 在交互视图集${\mathcal{K}}_C$ 中所有连接到它的关键帧，和所有被这些关键帧观测到的地图云点。所有其他能够观测到这些云点的关键帧但没有连接到当前处理的关键帧的这些关键帧会被保留在优化线程中，但会被修复。被标记为无效数据的观测将在优化中间阶段和最后阶段被丢弃。附录有详细的优化细节。

E、局部关键帧筛选

为了使重构保持简洁，局部地图构建尽量检测冗余的关键帧，删除它们。这会大有帮助，随着关键帧数量的增加，捆集调整的复杂度增加，但关键帧的数量也不会无限制地增加，因为它要在同一环境下在整个运行执行操作，除非场景内容发生变化。我们会删除${\mathcal{K}}_C$中所有90%的地图云点可以至少被其他3个关键帧在同一或更好的尺度上观测到的关键帧。

回环控制

回环控制使用${\mathcal{K}}_i$，被用于局部地图构建的最后一个关键帧，回环控制尝试检测和关闭回环。步骤会在下面介绍。

A、回环候选侦测

我们使用${\mathcal{K}}_i$的词袋向量计算它和在共视图中的其他临近帧（${\theta }_{\min }=30$）的相似性，并且保留最低分$S_{min}$。然后我们查询识别数据库，剔除掉所有分数小于$S_{min}$的关键帧。这是与DBoW2中的归一化分数相似的操作，以获得鲁棒性，该归一化分数是根据前一张图像计算得出的，但是这里我们使用共视信息。另外，直接与${\mathcal{K}}_i$连接的所有关键帧将从结果中删除。 为了获得候选回环，我们必须联系检测3个一致的候选回环（内容相关图像中的关键帧）。如果对${\mathcal{K}}_i$来说环境样子都差不多，就可能有几个候选回环。

B、计算相似变换

单目SLAM系统有7个自由度，3个平移，3个旋转，1个尺度因子，如论文6。因此，闭合回环，我们需要计算从当前关键帧${\mathcal{K}}_i$到回环关键帧${\mathcal{K}}_l$的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。

我们先计算ORB特征关联的当前关键帧的地图云点和回环候选关键帧的对应关系，具体步骤如第3章E节所示。此时，对每个候选回环，我们有了一个3D到3D的对应关系。我们对每个候选回环执行RANSAC迭代，通过Horn方法（如论文42）找到相似变换。如果我们用足够的有效数据找到相似变换$S_{il}$，我们就可以优化它，并搜索更多的对应关系。如果$S_{il}$有足够的有效数据，我们再优化它，直到${\mathcal{K}}_l$回环被接受。

C、回环融合

回环矫正的第一步就是 融合重复的地图云点，插入与回环相关的共视图的新边缘。先通过相似变换$S_{il}$矫正当前关键帧位姿$Tiw$，这种矫正方法应用于所有${\mathcal{K}}_i$相邻的关键帧，执行相似变换，这样回环两端就可以对齐。 回环关键帧所有的地图云点和它的近邻映射到${\mathcal{K}}_i$，通过映射在较小的区域内搜索它的近邻和匹配，如第5章D节所述。所有匹配的地图云点和计算$S_{il}$l过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们的边缘，这些视共视图创建的边缘用于回环控制。

D、基本图（Essential Graph）优化

为了更高效的关闭回环，我们使用了一种基于基本图的位姿图优化,在第三章D节中介绍过，他可以把回环误差分散到每个图中。该优化使用相似变换来纠正尺度漂移。 误差和成本计算如附录所示。优化过后，每一个地图云点都根据关键帧的校正进行变换。