ORB-SLAM: A Versatile and Accurate Monocular SLAM System 笔记（二）
4. 自动地图初始化
地图初始化的目标是两个帧之间相对位姿来三角化一系列的点云（riangulate an initial set of map points），这个操作是独立与场景且不需要人为的干预的。文章提出平行的计算两个几何模型：对平面场景的 homography 和对非平面的 fundamental matrix 。然后根据实际情况选择一个模型做初始化，算法的步骤如下：
-
Find initial correspondences: 从当前帧 \(F_c\) 中提取 ORB 特征并且在相邻帧 \(F_r\) 寻找匹配 \(\bf x_c \leftrightarrow \bf x_r\)，如果没有找到足够的匹配，就重置参考帧。
-
Parallel computation of the two models: 同时计算 homography \(\bf H_{cr}\) 和 fundamental matrix \(\bf F_{cr}\) ：
\[{\bf x}_c = {\bf H}_{cr} \, {\bf x}_r, \quad {\bf x}_c^T \, {\bf F}_{cr} \, {\bf x}_r = 0 \]计算模型使用的特征点和迭代数是提前固定的，每次迭代都给模型打分：
\[\begin{eqnarray} S_M &=& \sum _i \big (\rho _M \big (d_{cr}^2({\bf x}_c^i,{\bf x}_r^i,M)\big) \nonumber +\; \rho _M (d_{rc}^2\big ({\bf x}_c^i,{\bf x}_r^i,M)\big) \big) \nonumber \\ \rho _{M} (d^2) &=& \left\lbrace \begin{array}{ll}\Gamma - d^2, & \textrm {if} \quad d^2 < T_M \\ 0, & \textrm {if} \quad d^2 \ge T_M \end{array}\right. \end{eqnarray} \]\(d_{cr}^2,d_{rc}^2\) 是一个帧到另一个帧的传递误差，\(T_M\) 是拒绝门限值，\(\Gamma\) 等于 \(T_H\) 这样所有的模型在得分均匀。 最后选取score最高的一组。如果没有模型被找到，那么就重新回到步骤一（we restart the process again from step 1）。
-
Model selection: 如果场景是近似于平面的，就可以用 homography 模型，否则选用 fundamental matrix 模型。在平面的情况下视差较低，使用 homography 重建的误差较低。文章设计了一个式子：
\[R_H=\frac{S_H}{S_H+S_F} \]当 \(R_H>0.45\) 的时候选择 homography 模型。
-
Motion and structure from motion recovery: 一旦选好了模型，我们就可以获得运动状态。在 homography 中，从方法 23 中提取8种运动假设，这个方法通过测试来选择有效的方案。如果测试失败就会返回 1 中重新做计算，
\[{\bf E}_{rc} = {\bf K}^{T} \, {\bf F}_{rc} \, {\bf K} \] -
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\) 中每个可见的点云将会如下操作：
- 把 x 映射到当前帧，如果超出边界就丢弃；
- 计算当前视角 \(\bf v\) 和点云中平均视角 \(\bf n\)，如果 \(\mathbf {v} \cdot \mathbf {n}<\textrm {cos}(60^{\circ})\) 就丢弃当前视角；
- 计算点云到相机中心的距离，超出the scale invariance region 就丢弃；
- 计算帧的 scale，比例 \(d/d_{\min}\) ；
- 对比点云中描述符 \(\bf D\) 和当前帧还没有匹配的ORB特征，在scale，和靠近x的云点作最优匹配。
相机位姿最后通过当前帧中获得所有的点云进行优化。
E. 新的关键帧选取
要尽可能快的插入关键帧，这样可以在相机移动时 tracking 过程更加稳健。插入新的关键帧遵循：
- 上一次全局定位已经过去 20 帧；
- 局部地图构建已经暂停，或者是距离上次插入关键帧超过20帧；
- 当前帧 track 出来 50个特征点；
- 当前帧跟踪少于参考关键帧特征点的90%。
6. Local Mapping
A. 关键帧插入
首先更新 covisibility graph ，插入节点 \(K_i\) ，更新关键帧间具有相同点云产生的edge。我们还要更新生成树上Ki和其他关键帧的链接。然后，计算表示关键帧的词袋，用于数据关联来三角化新的云点。
B. 点云的去冗余
点云必须通过三帧的严格测试，确保它们是可被 track 和没有错误的 triangulate。一个点必须满足两个条件：
- Track 必须在超过25%预测可见的帧中找到该点；
- 特征点必须被至少三个关键帧察觉到。
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 执行了一个优化，这样可以将回环的误差分散到图像中，优化程序通过相似变换校正尺度偏移。优化过后每一个特征点都根据关键帧的校正进行变换。
