ORB-SLAM2翻译
III. System Overview
B. Three Threads: Tracking, Local Mapping and Loop Closing
我们的系统分为三个线程：跟踪、局部建图和回环检测。
跟踪负责利用每一帧图像定位相机的位置，并决定何时插入一个新的关键帧。我们首先将当前帧与前一帧执行一个初始的特征匹配并使用motion-only BA来优化帧间位姿。如果跟踪丢失（例如，由于遮挡或者突然运动），则通过位置识别模块执行全局重定位。只要我们得到了一个初始的特征匹配和相机位姿估计，就可以使用系统维护的一个“关键帧共视图”（covisibility graph of keyframes）确定一个“局部可视地图”local visible map。然后，通过重投影搜索这个局部地图中（与当前图像）的匹配点，并基于所有的匹配进一步对相机的位姿进行优化。最后跟踪线程决定是否插入一个新关键帧。
局部建图负责处理新关键帧并执行local BA来实现相机位姿周围环境的最优重建。新关键帧中未匹配的ORB特征从covisibility graph中搜索新的匹配并三角化得到新的地图点。为了只保留高质量的地图点，通过一种苛刻的exigent地图点挑选策略并基于跟踪时积累的信息来对那些被创建一定时间后的地图点进行挑选。局部建图模块同时负责剔除冗余的关键帧。
闭环检测对每个新关键帧搜索闭环。一旦检测到闭环，我们计算一个相似变换，这个变换表达了这个闭环的误差积累。最后对相似变换约束执行位姿图优化来实现全局一致性。此处本文的主要创新在于我们对Essential Graph执行优化，它是一个比covisibiltiy更加稀疏的subgraph。
C. Map Points， KeyFrames and their Selection
每个地图点\(p_i\)保存了：
- 它在世界坐标系中的3D位置\(X_{\omega, i}\)
- 视角方向\(n_i\)，由该点所有视角方向的平均单位向量来表达。
- 一个有代表性的ORB描述子\(D_i\)，这个描述子是与其它所有看到这个点的关键帧中相关联的描述子hamming距离最小那个。
- 该点能被观测到的最大和最小距离，根据ORB特征的尺度不变性限制。
每个关键帧\(K_i\)保存了：
- 相机位姿 \(T_{iw}\), 即将地图点从世界系变换到相机坐标系。
- 相机内参，包括焦距和光心。
- 所有从该帧中提取的ORB特征，其中有些关联到地图点，有些没有，如果提供了畸变模型的话，这些点的坐标是矫正过的。
地图点和关键帧采用宽松的策略创建，但是之后会以一个非常严格的挑选culling机制来确保检测出冗余的关键帧和错误的匹配或者不可跟踪的地图点。这样在保证了探索期间可以灵活扩展地图的同时，又增强了在困难条件下（旋转、快速运动）的的鲁棒性，并且也能将反复看到的相同环境的地图点规模限制在一定范围内，即可以长期运行。此外我们的地图包含的外点比PTAM少很多，代价是包含更少的点。地图点和关键帧的挑选过程分别在VI-B和VI-E中解释。
D. Covisibility Graph and Essential Graph
关键帧之间的共视信息在我们系统的多个任务中是非常有用的，这些信息被表达为如文[7]一样的一个无向加权图。每个结点代表一个关键帧，如果两个关键帧之间观测到（至少15个）同样的地图点，则这两个关键帧对应的结点之间的边就存在，而且它们的连边通过共有地图点的数量来进行加权。
为了校正一个闭环我们执行一个位姿图优化以将闭环误差分散到graph中。共视图提供的所有边可能是非常稠密的，为了不包含所有的边，我们提出了一个Essential Graph。Essential Graph保留了所有的结点，边的数量更少，却依然可以保持一个可以产生精确结果的强网络。系统从初识关键帧开始增量式地构建一个spanning tree（最小生成树），这个最小生成树以最小数量的边提供了共视图的一个连接的子图。当一个新的关键帧被插入时，它被包含到与其共有最多地图点观测的那帧关键帧所关联的那颗树中。同样，当一个关键帧被“挑选机制”删除，系统会更新与该关键帧有关的树。Essential Graph包含了最小生成树、共视图中有高度共视性的边的子集以及闭环边，得到一个不同时刻相机之间的强网络。图2展示了共视图、最小生成树和对应的essential graph的一个例子。如VIII-E中的实验所展示的，当执行位姿图优化时，结果是非常精确的，以至于一个额外的完全BA优化几乎没有提升求解结果。essential graph的效率和共视性阈值的影响如VIII-E节所示。
E. Bags of Words Place Recognition
系统基于DBoW2集成了一个词袋位置识别模块，用于执行闭环检测和重定位。视觉单词（visual words）只是对描述子空间的一种离散化，所谓描述子空间也就是视觉词汇表visual vocabulary。视觉词汇表是使用从大量图像中提取的ORB描述子来离线创建的。如果图像足够普遍，相同的词汇表可以被用于不同的环境中并尝试不错的效果，正如我们之前的工作[11]中所展示的。系统增量式地构建一个包含invert index逆索引的数据库，词汇表中每个视觉单词的逆索引保存了它被看到的那些关键帧的索引，因此查询数据库可以很高效地完成。该数据库也会在有关键帧在挑选过程被删除时更新。
由于关键帧之间视觉重叠的存在，查询数据库时会得到不止一个高分的关键帧。原始的DBoW2考虑了这一情况，将时间上紧邻的那些关键帧的分数相加。这一做法的局限在于不会包含那些在同一位置观测但是在不同时间插入的关键帧。相反，我们把这些在共视图中相连的关键帧进行分组。此外我们的数据库返回所有分数比最好分数高75%的那些关键帧。
使用词袋表达的特征匹配的其他优势在[5]中报告。当我们想计算两个ORB特征集合的匹配时，我们可以将暴力搜索匹配约束在那些属于词汇表树的特定层（我们选择the second out of six 6层中的第二层？）的同一结点的特征。我们在搜索匹配以三角化新的地图点、闭环检测和重定位中都使用了这一技巧。我们也使用了一种方向一致性测试来改善匹配（详见[11]），这样可以丢弃外点从而确保所有匹配都是连贯一致的旋转。
IV. Automatic Map Initialization
地图初始化的目标是计算两帧之间的相对位姿来三角化一个初识地图点集。这里的方法应该独立于场景（平面的或一般的），也最好无需人工介入来选择一个良好的双视布局（也就是有显著视差的布局）。我们提出并行地计算两个几何模型：一个假设平面场景的homography和一个假设非平面场景的fundamental matrix。然后，我们采样一种启发式的（heuristic）方法来选择一种模型并试图基于该模型的特定方法来恢复相对位姿。我们的方法只会在确保双视布局安全的情况下才会初始化，通过检测低视差情况和已知的“对折平面歧义”twofold planar ambiguity，来避免初始化得到一个不好的地图。我们算法步骤如下：
1）搜索初始匹配：
在当前帧\(F_c\)提取ORB特征（只在最佳尺度）并在参考帧\(F_r\)中搜索匹配\(X_c \leftrightarrow X_r\)
2）并行计算两个模型：
在并行的两个线程分别采用[2]中解释的normalized DLT和八点法在ransac框架下计算一个homography\(H_{cr}\)和一个fundamental matrix \(F_{cr}\):
为了同质化（homogenous）两个模型的步骤，我们事先确定迭代次数并且对两种模型采用相同的次数。每次迭代中，fundamental matrix会用到8个点，而homography用到其中的4个。每次迭代中，我们分别计算两个模型的得分\(S_M\)：
其中\(d_{cr}^2\)和\(d_{rc}^2\)为从一帧到另一帧对称转移误差。\(T_M\)是基于卡方测试95%（\(T_H = 5.99, T_F = 3.84\)，假设测量误差中的一个标准偏差为一个像素）的外点拒绝阈值。\(\Gamma\)被定义为与\(T_H\)相等，从而使得两种模型对于它们内点域中相同的d有相同的得分，再一次保证了过程的同质。我们保留具有最高分的homography和fundamental matrix。如果没有找到任何模型（没有足够的内点），我们从步骤1重新开始。
3）模型选择：
如果场景是平面的、近似平面的或者低视差的，那它可以通过homography来解释。虽然一个fundamental matrix也可以被计算出来，但问题是它是没有被良好约束的，任何从该fundamental matrix中恢复运动的企图都可能产生错误的结果。我们应该选择homography作为从一个平面正确地初始化的重建方法，并检测是否为低视差的情况从而拒绝初始化。另一方面，有足够视差的非平面场景只能由fundamental matrix来解释，虽然也能找到一个homography来解释匹配的一个共面子集或者有低视差的子集（它们相隔很远）。在这种情况下我们应该选择fundamental matrix。我们得到一种稳健的启发式方法为计算：
并在\(R_H > 0.45\)时选择homography，此时等效于captures了平面和低视差情况。否则，我们选择fundamental matrix。
4）运动恢复与从运动中恢复结构
一旦选择好了模型，我们从相关的假设中恢复相机运动。在homography的情况中我们采用Faugeras的方法产生8个运动假设。该方法提出了cheriality tests来选择有效解。然而这些测试在低视差时会失败，因为点很容易在相机的前后游动，可能导致选择一个错误的解。我们提出直接三角化这8个解，并检查是否存在一个解有着最多的有足够视差、在两个相机前方、有低的重投影误差的点。如果没有找到一个明显胜出的解，我们不执行初始化并从步骤一重新开始。这种对抗歧义解的技术使得我们的初始化即使在低视差和twofold ambiguity结构下也是稳健的。而且这也可以认为是我们方法稳健性的关键所在。
在fundamental matrix的情况中，我们使用标定好的K矩阵把它转化为essential matrix：
然后使用[2]中提出的奇异值分解方法产生4个运动假设。我们三角化这4个解并用homography中同样的方法选出一个来重建。
5）Bundle adjustment：
最后我们执行一个full BA（详见附录），来优化初始重建。
如图3所示是户外NewCollege机器人图像序列的一个具有挑战的初始化的例子。可以看出PTAM和LSD-SLAM是初始化所有在某个平面上的点，而我们的方法等到有足够的视差，才正确地从fundamental matrix初始化。
V. TRACKING
本节我描述跟踪线程对相机图像每一帧执行的步骤。在多个步骤中提到的相机位姿优化主要为motion-only BA，在附录中描述。
A. ORB Extraction
我们在尺度因子为1.2的8个尺度级别上提取FAST角点。对于分辨率从512x384到752x480不等的图像，我们提取1000个角点。对于更高的分辨率，如果KITTI数据集中1241x376的分辨率，我们提取2000个角点。为了确保homogenous distribution，我们在一个网格上对每一层尺度划分，每个格子中尝试至少提取5个角点。然后我们检测每个格子中的角点，如果没有足够的角点，就调整检测子的阈值。每个格子保留角点的数量也会被调整，如果一些格子美原油包含角点（无纹理或者低对比度）。然后对保留下来的FAST角点计算方向和ORB描述子。ORB描述子用于所有的特征匹配中，相反PTAM中通过块关联（patch correlation）来搜索匹配。
B. Initial Pose Estimation from Previous Frame
如果能成功跟踪上一帧，我们使用匀速运动模型来预测相机位姿并执行一个对上一帧观测到的地图点的引导式搜索。如果没有找到足够的匹配（即明显违反了匀速运动模型），我们选择对地图点在上一帧中的位置附近执行一个更宽的搜索。然后使用搜索到的匹配优化位姿。
C. Initial Pose Estimation via Global Relocalization
如果跟踪丢失，我们将图像帧转换成词袋并查询“位置识别数据库”，找到一些关键帧候选用于全局重定位。我们使用如III-E中解释的方法计算每个关键帧中地图点对应的ORB之间的关联。然后我们对每一关键帧轮流地执行RANSAC迭代并使用PnP算法试图确定相机位姿。如果我们找到一个具有足够内点的相机位姿，我们优化该位姿并执行引导式搜索找到更多与候选关键帧地图点的匹配。最后再次优化相机位姿，如果有足够的内点支撑这一结果，则跟踪过程继续。
D. Track Local Map
一旦我们获得了相机位姿的估计和特征匹配的初始集合，我们就可以将地图投影到图像帧并搜索更多地图点匹配。为了限制在大地图情形下的复杂度，我们只投影一个局部地图。这个局部地图包含了关键帧集合\(K_1\)（它们与当前帧有共享的地图点）和关键帧集合\(K_2\)（它们在共视图中与\(K_1\)相邻）。局部地图也有一帧参考关键帧\(K_{ref} \in K_1\),它与当前帧共享了最多的地图点。接下来\(K_1\)和\(K_2\)中看到的地图点用于当前帧中的搜索如下：
1）计算地图点在当前帧中的投影点x，如果超出图像边界则丢弃该点。
2）计算当前视线v和地图点的平均视角方向n之间的角度，如果\(v \cdot n < cos(60^\circ)\)则丢弃。
3）计算地图点到相机中心的距离，如果超出了地图点的尺度不变性区域则丢弃\(d \notin [d_{min}, d_{max}]\)。
4）由比率\(d/d_{min}\)计算图像帧的尺度.
5）在预测的尺度上x周围比较地图点与图像中仍未匹配的ORB特征点之间的描述子D，将最好的匹配与该地图点进行关联。
相机位姿最后使用所有在该帧中找到的地图点进行优化。
E. New KeyFrame Decision
最后一步是确定是否当前帧应该被生成为一个新的关键帧。由于在local mapping中有一个剔除冗余关键帧的机制，这个阶段我们会尽可能快速地插入关键帧，因为这样可以使得跟踪对具有挑战的相机运动（如旋转）更加稳健。为了插入一个新的关键帧，必须满足下列所有条件：
1）自从上次全局重定位已经过去至少20帧
2）Local Mapping是空闲状态或者自从上一次插入关键帧已经过去20帧
3）当前帧跟踪了至少50个地图点
4）当前帧跟踪的点少于参考关键帧\(K_{ref}\)的90%
PTAM中使用了到其他关键帧的距离准则，相反我们引入了一个最小视觉改变（条件4）。条件1确保了一个良好的重定位，条件3确保了一个良好的跟踪。如果关键帧在local mapping繁忙的状态时插入（条件2的第二部分），会发送一个信号来阻止局部BA，以便它可以尽快处理该新关键帧。
VI. LOCAL MAPPING
本节我们描述local mapping线程对每一新关键帧执行的步骤。
A. KeyFrame Insertion
首先我们更新共视图，为\(K_i\)添加一个新结点并更新由与其他关键帧共享地图点所产生的边。然后我们更新与\(K_i\)关联的有最多共同地图点的关键帧形成的最小生成树。接着我们计算该关键帧的词袋表达，这有助于三角化新点时的数据关联。
B. Recent Map Points Culling
在前三帧关键帧创建后，地图点想要保留在地图中，需要经过严格的测试，这保证了它们是可跟踪的且不会被错误三角化（由于错误的数据关联）。一个点必须满足一下两个条件：
1）在预计能看到该点的所有图像帧中，跟踪模块能必须能在其中的25%以上的帧中找到该点。
2）如果从该地图点别创建起已经经过了不止一个关键帧，那么它必须至少被三个关键帧观测到。
一旦地图点通过了这个测试，任何时候只有当它不能被三帧以上关键帧观测到时该点才可以被移除。这可能发生在一些关键帧被剔除且local BA丢弃外点观测时。这个策略使得我们的地图包含很少的外点。
C. New Map Point Creation
新的地图点通过三角化共视图中相连的关键帧\(K_c\)的ORB特征来创建。对于\(K_i\)中每一个未匹配的ORB特征，我们在其他关键帧中搜索其他未匹配的点。这一匹配按照III-E中解释的方法来实现，并且丢弃那些不满足epipolar约束的匹配。ORB特征匹配对被三角化，并接受那些在两个相机中都为正深度、视差、重投影误差和尺度一尺度都满足要求的新点。起初一个地图点只被两个关键帧观测到，但是可以在其他关键帧中被匹配，因此它被投影到剩余相连的关键帧中，使用V-D中详细描述的方法搜索匹配。
D. Local Bundle Adjustment
local BA优化当前处理的关键帧\(K_i\)、所有在共视图中与之相连的关键帧\(K_c\)、以及所有被这些关键帧看到的地图点。所有其他能看到这些地图点但是与当前处理的关键帧没有连接的关键帧也被包含到优化中，不过它们被保持为固定的。优化过程中以及优化结束后被标记为外点的都会被丢弃。关于此处的优化更多细节见附录。
E. Local Keyframe Culling
为了保持紧凑的重建，local mapping模块试图检测出冗余的关键帧并删除它们。这是有好处的，不仅因为BA的复杂度随着关键帧的数量增长，而且这样可以使得系统在同一环境长期运行，因为关键帧的数量不会无限制增长，除非场景中的视觉内容发生改变。我们丢弃\(K_c\)中所有那些90%以上的地图点都被3帧以上其他关键帧在相同的或者更加精细的尺度上被看到的关键帧。尺度条件保证了地图点维持在它们被最精确地观测到的那些关键帧中。这个策略是受Tang[24]中的工作启发，他们提出在执行变化检测后考虑将关键帧丢弃。
VII. LOOP CLOSING
loop closing线程处理local mapping线程最后处理过的关键帧\(K_i\)，试图检测回环并进行闭环。步骤描述如下。
A. Loop Candidates Detection
首先我们计算\(K_i\)的词袋向量和所有它的在共视图中相邻的关键帧（\(\theta_{min} = 30\)）的词袋向量之间的相似性，并保留最低分数\(s_{min}\)。然后我们查询位置识别数据库并丢弃那些分数比\(s_{min}\)还要小的关键帧。这是一种为了获得稳健性类似DBoW2中归一化分数一样的操作，不过DBoW2中通过之前的图像计算，而这里我们使用了共视信息。这些关键帧中除了与\(K_i\)直接相连的全部会被丢弃。为了接受一个回环候选，我们必须检测到连续的三个一致的回环候选（共视图中相连的几个）。如果好几处地点与\(K_i\)有相同的外观，可能会有好几个回环候选。
B. Compute the Similarity Transformation
在单目SLAM中有七个自由度可能导致地图漂移：三个平移、三个旋转和一个尺度因子。因此为了闭合一个回环我们需要计算当前关键帧\(K_i\)与回环关键帧\(K_l\)之间的相似变换，这个相似变换告诉我们这个环中累计的误差。这个相似变换的计算也作为该回环的一个几何校验。
我们首先根据III-E中的步骤计算当前帧和回环帧的ORB特征关联的地图点之间的匹配。从而我们就有了每个回环候选中的3D-3D匹配。我们轮流对每一个回环候选执行RANSAC迭代，使用Horn[42]的方法试图找到一个相似变换。如果我们能找到一个具有足够内点的相似变换\(S_{il}\)，就对它进行优化（见附录）并执行一个引导式搜索找到更多匹配。之后我们再次优化，如果有足够的内点支持\(S_{il}\)，则这个与\(K_l\)形成的回环就会被接受。
C. Loop Fusion
回环校正的第一步是融合重复的地图点并在共视图中插入新的边以将闭环接入。首先当前关键帧的位姿\(T_{iw}\)通过相似变换\(S_{il}\)来校正，并且该校正被传递到所有与\(K_i\)相邻的关键帧，通过衔接变换（concatenating transformations）让回环两头对齐。所有回环帧及其相邻关键帧所看到的地图点全部投影到\(K_i\)及其相邻关键帧，然后在投影点周围的小范围内搜索匹配，如V-D中所实现的那样。所有这些匹配上的地图点以及在\(S_{il}\)计算中的内点被融合到一起。这个融合中所有涉及的关键帧都会通过高效地创建与回环连接的边来更新他们在共视图中的边。
D. Essential Graph Optimization
为了高效闭合回环，我们对Essential Graph（在III-D中描述）执行一个位姿图优化，这一操作将闭环误差分散到图的各处。该优化对相似变换执行以校正尺度漂移。误差项和代价函数的细节见附录。在优化之后，每一个地图点根据观测到它的关键帧中的其中一个的校正结果进行变换。
实验略
附录：非线性优化
- Bundle Adjustment(BA)：
地图点的3D位置\(X_{w,j} \in \Bbb R^3\)和关键帧位姿\(T_{iw} \in SE(3)\)（其中\(w\)代表世界参考系）通过最小化匹配的关键点\(X_{i,j} \in \Bbb R^2\)的重投影误差而被优化。关键帧\(i\)中观测到地图点\(j\)产生的误差项为：
其中\(\pi_i\)为投影函数：
其中 \(R_{iw} \in SO(3)\) 而 \({\bf t}_{iw} \in \Bbb R^3\) 分别为\(T_{iw}\)的旋转和平移部分。\((f_{i,u}, f_{i,v})\)和\((c_{i,u}, c_{i,v})\)为相机i的焦距和光心。需要最小化的代价函数为：
其中\(\rho_h\)为Huber鲁棒代价函数，\(\Omega_{i,j} = \sigma_{i,j}^2{\bf I}_{2\times2}\)为与关键点被检测到的那一层的尺度相关的协方差矩阵。在full BA的情况中（用于第IV节解释的地图初始化以及VIII-E节中的实验）除了第一个关键帧保持固定作为原点，我们优化所有的点和关键帧。在local BA的情况中（见VI-D节）所有包含在该局部区域中的点被优化，而关键帧子集是固定的。在位姿优化或者motion-only BA中（见第V节），所有的地图点都是固定的，只有相机位姿被优化。
- Sim(3)约束上的位姿图优化：
给定一个二元边的位姿图（见VII-D节）我们定义一条边的误差为：
其中\(S_{ij}\)为位姿图优化前且尺度设为1的两个关键帧的SE(3)位姿中计算得到的相对Sim(3)变换。在闭环边的情况中该相对变换使用Horn[42]的方法进行计算。\(log_{sim3}\)将误差变换到正切空间，从而误差为一个\(\Bbb R^7\)上的向量。目标是通过最小化下列代价函数来优化Sim(3)形式的关键帧位姿：
其中\(\Lambda_{i,j}\)为边的信息矩阵，跟[48]中一样，我们把它设为单位阵。我们固定闭环关键帧从而固定7个标准自由度（7 degrees of gauge freedom）。虽然这个方法是full BA的粗略近似，我们VIII-E节的实验表明该方法特别快且比BA收敛性更好。
- Relative Sim(3) Optimization
给定关键帧1和2之间n对匹配\(i \Rightarrow j\)（关键帧及其对应的3D地图点）的集合，我们想优化相对Sim(3)变换\(S_{12}\)（见VII-B节）以最小化两个图像的重投影误差：
需要最小化的代价函数为：
其中\(\Omega_{1,i}\)和\(\Omega_{2,j}\)为与该关键点在图像1和2中检测到的那个尺度有关的协方差矩阵。在这个优化中，地图点是固定的。
