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 执行了一个优化，这样可以将回环的误差分散到图像中，优化程序通过相似变换校正尺度偏移。优化过后每一个特征点都根据关键帧的校正进行变换。