ORB-SLAM：一个多功能的、精确的单目SLAM系统

备注

• 场景特征=地图点

  0. 摘要

  ORB-SLAM，一个基于特征点法的单目SLAM系统，可以实时运行。该系统包括前端跟踪、后端建图、回环检测、重定位，支持全自动初始化。

  1. 介绍

  1.1 motivation：

• BA和实时性的矛盾：
  BA可以准确从从视觉图像中提炼出最优的相机位姿和特征点的位置，但是由于运算开销比较大，对于VSLAM这种需求实时性的应用而言，负担不起。

BA算法如果想在实时SLAM里用，必须确保：

1. 选取部分帧作为关键帧，仅对关键帧做BA来估计特征点位置
2. 考虑到复杂性，关键帧的数量需要合适
3. 一组良好的关键帧中，特征点需要具有显著视差(parallax)，和大量回环匹配【？】
4. 对关键帧对应的相机位姿和特征点位置进行初始化
5. 具有可扩展性
6. 针对回环检测的需求，需要能够快速实现全局优化（位姿图）【？】

• 前辈的工作

  • PTAM[4]
    • 仅限于小规模场景
  • 重定位[5]
  • 尺度感知的回环检测[6]
  • 共视信息[7,8]

• 我们的工作：

  • 将PTAM的多功能性扩展到对该系统而言困难的环境中。为了实现这一目标，从头开始设计了一个新颖的单目SLAM系统。

1.2 Contribution:

1. 实现ORB-SLAM系统，
   1. 该系统包含跟踪、建图、重定位和回环检测；
   2. 使用ORB特征[9]；
   3. 允许在无GPU情况下实时；
   4. 对视点(viewpoint)和关照具有良好的不变性
2. 在大规模场景下可以实现实时。这点得益于共视图（covisibility graph）的使用，可以在局部范围内跟踪和建图。
3. 实时回环检测。这点得益于使用essential graph来初始化。
4. 实时相机重定位。
5. 基于模型选择的自动初始化程序，建立初始地图。
6. 地图点和关键帧的选择：适者生存策略，宽进严出。

2. 相关工作

2.1 Place Recognition

2.2 地图初始化

2.3 单目SLAM

3. 系统总览

3.1 特征选择

1. 用于跟踪和建图的特征 = 用于重定位和回环检测的特征
2. 采用ORB特征 [9,11]

3.2 三线程：跟踪、局部建图、回环检测

• 跟踪：对每一帧的相机进行定位，并决定何时插入新的关键帧。（跟踪详情参考section 5，初始化详情参考section 4）

  1. 首先进行位姿初始化。

     1. 方法1：和前一帧做特征匹配，再使用motion-only BA来优化位姿。
     2. 方法2：如果由于遮挡、突然移动等因素导致跟丢，就通过place recognition模块来做全局重定位。

  2. track local map。

     初始化对相机位姿和特征点【不确定，文章用的是‘特征匹配’】后，使用系统维护的关键帧的共视图来恢复局部可视化地图。

     

     然后通过重投影来搜索局部特征点的匹配，再用所以的匹配来再次优化相机位姿。【？】

  3. 最后决定是否插入新的关键帧。

• 局部建图：处理新的关键帧并使用local BA来实现相机位姿环境的最优化重建。（详见section 6）

  在共视图关联的关键帧中搜索新关键帧中未匹配的新ORB特征点，通过三角化来创建新点。

  在创建新点后，根据跟踪过程中获取的信息，使用一个point culling policy来剔除一些点，仅保留高质量的点。

  局部建图还需要剔除一些冗余的关键帧。

• 回环检测：对每个新的关键帧检测一次回环，再做回环修正。（详情见section 7）

  如果检测到回环，计算一个相似性变换，来获知回环中的漂移累积。

  然后做修正：对齐回环的两边，融合重复的点。

  最后，对similarity constraint[6]做一次位姿图优化，来实现全局一致性。【？】

  主要创意是优化Essential Graph。

  • Essential Graph：共视图的一个稀疏子图。详情见section 3.3

• 优化：使用列文伯格-马夸尔特算法（麦夸特法）+ g2o

3.3 地图点，关键帧，及其选择

• 地图点p_i包含以下信息：

  • 世界坐标系下3d坐标X_w,i
  • 观测方向 n_i：所有观测方向（连接p_i和观测它的关键帧的光学中心的射线）的平均单位向量。
  • ORB描述子D_i：其汉明距离在所有观测该点的关键帧所关联的描述子中最小。【？】
  • d_max, d_min：在ORB特征尺度不变性的限制下，可以观测到该点的最大和最小距离。

• 关键帧K_i包含以下信息：

  • 相机位姿T_iw：世界坐标系中的点到相机坐标的刚体变化。
  • 相机内参：包括focal length和principal point。
  • 在这帧中提取到的所有ORB特征：这些提取到的特征无所谓是否与地图点相关联。（如果给出了畸变模型，这里的ORB特征坐标是undistorted）

地图点和关键帧的创建比较随意，但是剔除策略比较严格，剔除策略需要检测冗余的关键帧以及错误匹配或不可追踪的地图点。正因为有宽进严出的策略，地图可以在探索过程中灵活的扩展，并且在旋转、快速移动等困难条件下，还可以保持一定的跟踪稳定性。地图的大小在同一个环境不断重访的情况下受限，不会无限扩张，可以终生运行。

与PTAM相比，地图包含的点和异常值更更少。

详情请参考6.2和6.5。

3.4 共视图和Essential Graph

关键帧之间的共视信息很重要，这些信息以一无向加全图的形式呈现——共视图。

图中每个节点是一个关键帧，如果两个关键帧共享一定数量(>=15)的地图点，则两帧之间存在一条边，边的权重θ为共享地图点的数量。

在回环校正的过程中，需要进行一次位姿图优化，来沿着图distribute the loop clossing error。由于共视图的所有边画在一起太过密集，我们建一个Essential Graph来保留所有关键帧节点和部分的边。Essential Graph是共视图的一个子图，但是仍是一个强大的网络，且结果依旧准确。

该系统从初始关键帧开始递增的建一棵spanning tree，这棵树表示共视图的连接子图，该子图拥有最少的边。当插入一个新关键帧，spanning tree中会加上这个新关键帧节点，并连接与该节点具有高共视度的关键帧节点。当剔除一个关键帧，系统更新所有受影响的连接。

Essential Graph中包括这个生成树、具有高共视度(θ>=100)的共视图中边的子集、回环检测边【？】，因此形成了一个强大的相机网络。

例子（共视图、生成树、essential graph）：

实验见section 8.5。

3.5 Bags of Words Place Recognition

系统嵌入了词袋位置重识别模块，该模块基于词袋模型DBoW2[5]设计。此模块用于回环检测和重定位。

Visual words = visual vocabulary。

该词典是用从一堆图像中提取出的ORB描述子离线生成的。[11]以递增的方式构建了一个数据库，储存了词汇表中每个word在哪一帧中出现的索引。当关键帧被剔除时，数据库也会同步更新【不是离线生成的？】。

由于关键帧存在视觉上的重叠，查询数据库时，不会出现高分only一帧的情况。原始的DBoW2考虑到这个重叠，将时间相近的图像的分数叠加。但是这种做法具有局限性：不能涵盖时间不同但是地点相同的关键帧。鉴于此，我们的方法是：group 共视图中相连的关键帧。此外，我们的数据库返回的是所有分数高于best score【？】75%的关键帧matches。

词袋的另一个优点[5]：当我们需要计算两组ORB特征的关系时，可以将暴力匹配的过程限制在词典树的某一层上的相同节点的特征上【？】。在为新点的三角化测量做特征匹配、回环检测、重定位时，都可以使用这个trick。我们还使用orientation consistency test来细化这个关系[11]，这种方法去除了一些异常值，以确保所有对应关系具有旋转一致性。

4. 自动地图初始化

目标：计算两帧的相对位姿，通过三角化测量初始化一组初始地图点。

并行计算两个几何模型：平面场景的单应矩阵(homography) + 非平面场景的基础矩阵

用heuristic方法选择模型，再用算法X来计算相对位姿。

我们算法的前提是确定这两个帧是安全的，即提前检测低视差和二重平面模糊的情况，以避免初始化一个被破坏的地图。

算法步骤：

1. 找到初始对应关系：

   在当前帧F_c的最优尺度下提取ORB特征，和参考帧F_r进行特征匹配，得到匹配的特征X_c——X_r。

   如果没有足够的匹配特征，重置参考帧。

2. 并行计算两个模型：

   并行计算一个单应矩阵H_cr和一个基础矩阵F_cr。

   

   分别采用RANSAC方案中的DLT算法和八点法来计算H和F。[2]

   为了统一两个模型的计算步骤，预设好相同的迭代次数、以及每次迭代使用的点（F用8对点，H用4对点）。

   在每次迭代中，为每个模型M（F/H）计算一个分数S_M：

   

   1. d^2是两帧间的symmetric transfer error[2]
   2. 假设标准差为1像素的情况下，阈值T_H=5.99，T_F=3.84。
   3. 大gamma = T_H

   H和F是得分最高的模型。

   如果找不到模型了，就回步骤1。

   【为啥要迭代？】

3. 模型选择：

   • 选单应H：平面场景、近似平面场景、低视差场景。

     【检测到低视差（far away【只有far away这种情况吗？】）–>拒绝初始化 or 选H？】

   • 选基础F：具有足够视差的非平面场景。

   • 使用启发式算法来选择模型，计算：

   

   R_H >0.45选H，否则选F。

4. 恢复 Motion and Structure from Motion：

• 单应矩阵H模型：采用[23]方法恢复8个(4对)运动假设(R,t)。

  [23]中使用cheriality test来选择有效方案。但是该测试在低视点情况下会失效（点容易跑到相机前方或者后方）。

  我们之间对8个方案进行三角化测量，检查是否存在一个解决方案，使得大多数点能看到视差，且都在相机前，且具有低重投影误差。如果没有一个适合的解决方案，就不进行初始化了，直接返回步骤1重新做特征匹配。

• 基础矩阵F模型：

  将基础矩阵转换成本质矩阵E，其中K为内参(标定矩阵)，

  

  再用奇异值分解SVD解出4个运动假设(R,t)。再对这4个方案进行三角化测量，选择其中一个来重建。

相当于从H/E中恢复运动R,t，再用三角测量获得特征点的空间位置。

5. BA：full BA，来优化这个初始化重建。

例子：

PTAM，LSD-SLAM，ORB-SALM

5. 线程1-跟踪

对相机的每一帧执行该线程。

在线程1中的相机位姿优化仅包含motion-only BA。

5.1 ORB提取

在8个尺度上(scale factor = 1.2)提取FAST角点。

对于分辨率在512x284——752x480像素的图片，提取1000个角点。

对于分辨率更高（如KITTI数据集上1241x376）的图片，提取2000个角点。

为了确保角点均匀分布，我们在每个尺度上按网格划分，试图在每个单元格内至少提取5个角点。然后我们在每个单元格内检测角点，如果没有检测到足够的角点就调整detector threshold。如果某些单元格内没有角点（无纹理/低对比度），也需要调整该阈值。

在保留的FAST角点上计算方向和ORB描述子。不同于PTAM中对patch correlation的检索，ORB描述子被用于所有特征匹配。

5.2 从先前帧中初始化位姿估计

如果成果跟踪到最后一帧，就使用一个恒速运动模型来预测相机位姿，并对最后一帧中观察到的地图点进行引导性搜索。如果找不到足够的匹配点（即运动模型明显不对），就在最后一帧中对地图点周围进行更广泛的搜索。根据找到的对应关系优化位姿。

5.3 从全局重定位中初始化位姿估计

如果跟丢了，就将当前帧转换成词包，然后查找识别数据库中的候选关键帧用于全局重定位。计算与每个关键帧中的地图点相关的ORB的对应关系（3.5）。然后对于每个关键帧进行RANSAC迭代，使用PnP算法估计相机位姿[41]。

如果找到一个有足够inliers的相机位姿，就优化该位姿并对该候选关键帧的地图点进行引导性搜索，找到更多的匹配。最后，再次优化相机位姿，如果有足够的inliers，就继续跟踪。

5.4 跟踪局部地图

【没看太懂？】

一旦估计了相机位姿，且有了一组初始的匹配特征，就可以将地图投影到当前帧中，并搜索更多的地图点对应关系。为了约束大地图的复杂度，我们只投影局部地图。

局部地图包含一组关键帧K1，它们与当前帧共享地图点；以及一组关键帧K2（关键帧组K1在共视图中的邻居关键帧）。局部地图还包括参考关键帧K_ref（属于K1），参考关键帧与当前帧共享的地图点数量最多。现在，在当前帧中检索每一个K1、K2中可见的地图点：

1. 计算当前帧中的地图点投影x。如果超出图像边界，抛弃。
2. 计算当前viewing ray v和地图点mean viewing direction n的夹角。如果v·n<cos60°就抛弃。
3. 计算相机中心到地图点的距离d。如果超过地图点的尺度不变范围[dmin,dmax]就抛弃。
4. 在预测的尺度下，在x附近，比较地图点的描述子D和当前帧中仍未匹配的ORB特征，将地图点与最佳匹配点相连接。

使用在帧中找到的所有地图点来优化相机位姿。

5.5 决定新关键帧

这一步是判断是否要将当前帧作为一个新的关键帧。

局部建图环节有一个机制可以剔除多余的关键帧，为了能更稳健的跟踪复杂的相机运动（如旋转），在这个步骤中我们要尽可能快速的插入关键帧。插入新的关键帧需要满足以下条件：

1. 距离上一次全局重定位，至少已过了20帧。
2. 线程2局部建图空闲，或者距离上一次插入关键帧已过了20帧。
3. 当前帧至少跟踪到50个点。
4. 当前帧跟踪到的点至少比参考帧K_ref少90%。

不同于PTAM中使用与其他关键帧的距离作为评判标准，我们规定了一个最小的视觉变化（条件4）。条件1可以确保一个好的重定位，而条件3可以确保一个好的跟踪。如果在线程2忙碌时插入关键帧，需要传递一个暂停局部BA的信号【给谁？】，使得新的关键帧可以尽快被处理。

6. 线程2-局部建图

对每一个新的关键帧K_i执行该线程。地图由一个个地图点组成，地图的扩展来自于新地图点的创建。

6.1 插入关键帧

首先更新共视图：新节点K_i；更新边（根据与其他关键帧共享的地图点数量）。

然后更新生成树，链接K_i与具有most共享点的关键帧节点。【这里的most是最多还是大多？】

然后计算关键帧的词袋representation（有助于三角化新点时提供数据关联）。

6.2 剔除最近的地图点

地图点如果想保留在地图上，必须在创建后的前三个关键帧中通过一个严格的测试，以确保地图点是可以跟踪的，且没有错误的三角化（由于数据关联错误）。这个地图点必须满足以下两个条件：

1. 跟踪必须在25%以上的帧（这些帧中该点是预测可见的）中找到该点。
2. 如果从地图点创建开始已经过了一个以上的关键帧，则必须在至少三个关键帧中观察到该点。【？】

通过测试的地图点的移除：任何时候，观测到该点的关键帧少于3帧。这种情况会在关键帧剔除、BA抛弃离群观测(outlier observations)的情况下发生。

该策略减少了地图离群点。

6.3 创建新地图点

三角化共视图中相连关键帧K_c的ORB特征来创建新地图点。

对于每个K_i中未匹配的ORB特征，搜索其他关键帧中的未匹配点来做特征匹配。匹配过程如3.5所述，如果匹配不满足对级约束(epipolar constraint)则将其抛弃。

我们三角化了一对ORB特征，为了接受新点，还要检查两个相机的正向深度、视差、重投影误差以及尺度一致性。一开始，这个地图点只在这两个关键帧中被观测到，但是这个点也会与其他关键帧匹配，所以这个地图点被投影到其余的连接关键帧中，并且按照5.4来检索对应关系。

6.4 局部BA

局部BA优化

1. 当前处理的关键帧K_i
2. 共视图中与该帧相连的关键帧K_c
3. 这些关键帧可以看到全部的地图点

其他可以看到这些地图点但不与该关键帧相连的关键帧也包括在优化范围内，但是固定不变。

被标记为outliers的观测点在优化的中间和最后被抛弃。（具体细节见附录）

6.5 剔除局部关键帧

为了保持重建地图的紧凑性，局部建图步骤试图检测并删除冗余的关键帧。

• 优点：

1. 对BA有利，因为BA的复杂度随着关键帧数量增多而增大
2. 使得在同一个环境下的lifelong操作具有可行性，因为关键帧的数量增长在相同环境下受限，只有改变场景才会继续增加。

• 抛弃K_c中的关键帧：

  这些关键帧中，至少有90%的地图点在相同尺度或更精细尺度下的其他三个（至少）关键帧中可见。

  这里设置尺度条件是为了确保地图点保留了最精准的关键帧。

启发：[24]

7. 回环检测

7.1 Loop Candidates Detection

首先，计算K_i关键帧的词袋向量与共视图中K_i的邻居关键帧(θmin=30)的相似度得分，保留最低的得分S_min。

然后在recognition database中检索并抛弃所以得分小于S_min的关键帧。这是一个类似DBoW2中归一化分数的操作（增加鲁棒性），但是DBoW2是用先前图像来计算，而我们用的是共视信息。

另外，还有从结果中去掉所以与K_i直接相连的关键帧。

要接受一个回环候选帧，必须连续检测到三个一致的回环候选帧（这些关键帧在共视图中相连）。如果有多个地方看起来像K_i，就有多个回环候选帧。

7.2 计算Similarity Transformation

单目SLAM中，有7个自由度（3旋转、3平移、1比例因子）的地图会发生漂移[6]。为了闭环，我们需要计算从当前关键帧K_i到回环关键帧K_l的相似转换，该值表示回环中的误差累积。这个相似度的计算可以作为回环的几何验证。

首先，计算当前帧与回环候选帧的地图点相关联的ORB特征的对应关系，参见3.5。每个回环候选都有一个3D-3D的对应关系【？】。对于每个回环候选帧交替进行RANSAC迭代，使用Horn方法[42]来寻找相似转换。

如果找到了具有足够多inliers的相似度S_il，就用附录的方法优化它，并对更多的对应关系进行引导性搜索。如果S_il有足够多的inliers，再次优化它，并接受回环K_l。

7.3 回环融合

回环矫正的第一步就是融合重复的地图点并在共视图中插入新边来将回环闭合。

首先，用相似转换S_il来修正当前关键帧的位姿T_iw，这个修正会传播到K_i的所有邻居节点，串联转换，使得回环的两个边对齐。【？】

所有回环关键帧和邻居关键帧可见的地图点都投影到K_i和它的邻居上，再在投影周围的一个狭窄区域内搜索匹配(如5.4所述)。所有匹配的地图点和那些计算S_il时的inliers都被融合。【？】

所有参与融合的关键帧都将更新它们在共视图中的边，以此来有效的创建闭合回环的新边。

7.4 Essential Graph优化

为了更高效的闭合回环，我们在Essential Graph上执行了一次位姿图优化（3.4），该操作将回环检测误差分布在了图上。该优化是通过相似性变换来纠正尺度漂移的[6]，详见附录。

优化完成后，每个地图点都根据观察它的一个关键帧的修正来进行转换。