ORB-SLAM: A Versatile and Accurate Monocular SLAM System 笔记（二）

4. 自动地图初始化

地图初始化的目标是两个帧之间相对位姿来三角化一系列的点云（riangulate an initial set of map points），这个操作是独立与场景且不需要人为的干预的。文章提出平行的计算两个几何模型：对平面场景的 homography 和对非平面的 fundamental matrix 。然后根据实际情况选择一个模型做初始化，算法的步骤如下：

1. Find initial correspondences: 从当前帧 (F_c) 中提取 ORB 特征并且在相邻帧 (F_r) 寻找匹配 (f x_c leftrightarrow f x_r)，如果没有找到足够的匹配，就重置参考帧。

2. Parallel computation of the two models: 同时计算 homography (f H_{cr}) 和 fundamental matrix (f F_{cr}) ：

   [{f x}_c = {f H}_{cr} \, {f x}_r, quad {f x}_c^T \, {f F}_{cr} \, {f x}_r = 0 ]

   计算模型使用的特征点和迭代数是提前固定的，每次迭代都给模型打分：

   [egin{eqnarray} S_M &=& sum _i ig ( ho _M ig (d_{cr}^2({f x}_c^i,{f x}_r^i,M)ig) onumber +; ho _M (d_{rc}^2ig ({f x}_c^i,{f x}_r^i,M)ig) ig) onumber \ ho _{M} (d^2) &=& leftlbrace egin{array}{ll}Gamma - d^2, & extrm {if} quad d^2 < T_M \ 0, & extrm {if} quad d^2 ge T_M end{array} ight. end{eqnarray} ]

   (d_{cr}^2,d_{rc}^2) 是一个帧到另一个帧的传递误差，(T_M) 是拒绝门限值，(Gamma) 等于 (T_H) 这样所有的模型在得分均匀。 最后选取score最高的一组。如果没有模型被找到，那么就重新回到步骤一（we restart the process again from step 1）。

3. Model selection: 如果场景是近似于平面的，就可以用 homography 模型，否则选用 fundamental matrix 模型。在平面的情况下视差较低，使用 homography 重建的误差较低。文章设计了一个式子：

   [R_H=frac{S_H}{S_H+S_F} ]

   当 (R_H>0.45) 的时候选择 homography 模型。

4. Motion and structure from motion recovery: 一旦选好了模型，我们就可以获得运动状态。在 homography 中，从方法 23 中提取8种运动假设，这个方法通过测试来选择有效的方案。如果测试失败就会返回 1 中重新做计算，

   [{f E}_{rc} = {f K}^{T} \, {f F}_{rc} \, {f K} ]

5. Bundle adjustment: 使用 full BA 来初始化地图。

5. Tracking

这一节详细介绍线程通过相机执行每一帧的步骤。

A. ORB 提取

为了确保角点分布均匀，将一张图片分成多个 cell ，每个 cell 提取至少五个角点。如果找不到足够的角点，通过调整检测器阈值来重新提取。如果某些 cell 不包含角点，每个 cell 的角点也要进行调整。方向和ORB描述符会做进一步的计算。

B. 从前面的帧初始化位姿

如果最后一帧的 tracking 成功，就是用匀速运动模型来预测位姿并找出上一帧中匹配的点。如果匹配不够，那么使用更加宽泛的距离去寻找对应的点进行相应的优化位姿。

C. 全局重定位的初始姿态估计

如果 tracking 失败，就把当前帧转为 bag of words 然后到数据库中做匹配。我们计算与每个关键帧中与地图点相关联的ORB 特征，使用 PnP 算法来确定相机位姿，如果有足够的 inlier ，就可以对丢失的帧做出位姿估计。

D. Track Local Map

一旦对相机位姿和特征匹配有了估计，就可以将地图投影到帧中来寻找更多的关联点。定义与当前帧有交集的集合(mathcal {K}_1) 和 (mathcal {K}_2) ，形成一个 covisibility graph 。在 (mathcal {K}_1) 中与当前帧有最多的匹配点的帧叫做 (mathcal {K}_{ref}) 。(mathcal {K}_1) 和 (mathcal {K}_2) 中每个可见的点云将会如下操作：

1. 把 x 映射到当前帧，如果超出边界就丢弃；
2. 计算当前视角 (f v) 和点云中平均视角 (f n)，如果 (mathbf {v} cdot mathbf {n}< extrm {cos}(60^{circ})) 就丢弃当前视角；
3. 计算点云到相机中心的距离，超出the scale invariance region 就丢弃；
4. 计算帧的 scale，比例 (d/d_{min}) ；
5. 对比点云中描述符 (f D) 和当前帧还没有匹配的ORB特征，在scale，和靠近x的云点作最优匹配。

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

E. 新的关键帧选取

要尽可能快的插入关键帧，这样可以在相机移动时 tracking 过程更加稳健。插入新的关键帧遵循：

1. 上一次全局定位已经过去 20 帧；
2. 局部地图构建已经暂停，或者是距离上次插入关键帧超过20帧；
3. 当前帧 track 出来 50个特征点；
4. 当前帧跟踪少于参考关键帧特征点的90%。

6. Local Mapping

A. 关键帧插入

首先更新 covisibility graph ，插入节点 (K_i) ，更新关键帧间具有相同点云产生的edge。我们还要更新生成树上Ki和其他关键帧的链接。然后，计算表示关键帧的词袋，用于数据关联来三角化新的云点。

B. 点云的去冗余

点云必须通过三帧的严格测试，确保它们是可被 track 和没有错误的 triangulate。一个点必须满足两个条件：

1. Track 必须在超过25%预测可见的帧中找到该点；
2. 特征点必须被至少三个关键帧察觉到。

C. 新点云的创建

在 covisibility graph 中的关联关键帧 $mathcal {K}_c $ 三角化 ORB 可以创建点云，ORB特征对三角化后，将要获得新的云点，这时要检查两个相机视图的景深，视差，重映射误差，和尺度一致性等。

D. 局部BA

局部优化当前处理的关键帧 (K_i) 、所有在 covisibility graph $mathcal {K}_c $ 上关联的帧和之前所有帧中的特征点。离群点会在优化的中间和最后阶段被丢弃。

E. 局部关键帧剔除

为了使得重构更加简洁，局部映射会尝试探测冗余关键帧然后删除它们。这会大有帮助，因为BA 的复杂度随着关键帧的增加而增长。一个关键帧如果90%的特征点已经在至少三帧出现过，也会被剔除。

7. 闭环检测

闭环检测处理的是最近处理的关键帧，判断它是否与之前的关键帧存在闭环。

A. 闭环候选检测

首先，计算 (K_i) 和它在 covisibility graph 邻近的关键帧的 bag of words vector 的相似度，保留最小得分 (s_{min}) 。然后查询数据库，丢弃所有得分小于 (s_{min}) 的关键帧，然后丢弃所有和 (K_i) 直接关联的关键帧。接着从剩下的帧中选取候选项，候选项必须是连续的三帧以上的才能作为候选项。

B. 计算相似转换

在单目 SLAM 中，可能有 7 个自由度发生漂移：三个平移分量，三个旋转分量和 scale 因子。因此要计算 (K_i) 和回环帧 (K_l) 的相似度来计算环中的累计误差。

首先计算当前关键帧和环候选关键帧中特征点的 ORB 对应关系，对应每个候选环，有 3D to 3D 的对应关系。对每个候选回环执行RANSAC迭代，，balabala

C. 闭环融合

第一步融合重复的点，在covisibility graph插入与 loop closure 有关的新的 edges。当前关键帧的位姿 (mathbf {T}_{iw}) 通过相似变化 (mathbf {S}_{il}) 修正，这个修正也会应用于所有的相邻帧 (K_i) ，来执行 concatenating 变换，这样环中所有的边都可以对齐。

回环关键帧所有的特征点和它的邻居映射到 (K_i) 中，通过映射在较小的区域内搜索它的近邻和匹配。所有匹配和未离群的点都被融合起来，融合过程中所有的关键帧将会更新它们的边缘，这些视图内容相关的图像创建的边缘用于回环控制。

D. Essential Graph 优化

为了进行有效的闭环，我们在 Essential Graph 执行了一个优化，这样可以将回环的误差分散到图像中，优化程序通过相似变换校正尺度偏移。优化过后每一个特征点都根据关键帧的校正进行变换。