ORB-SLAM2 论文&代码学习 —— 单目初始化

转载请注明出处，谢谢
原创做者：Mingrui
原创连接：http://www.javashuo.com/article/p-gmtyqhmy-hz.html

---

本文要点：

• ORB-SLAM2 单目初始化部分 论文内容介绍
• ORB-SLAM2 单目初始化部分 代码结构介绍

写在前面

以前的 ORB-SLAM2 系列文章中，咱们已经对 Tracking 线程作了介绍，可是当时咱们跳过了 Tracking 线程中一个很重要的部分 —— 单目初始化。咱们将在本文中，对 ORB-SLAM2 系统的单目初始化部分进行介绍。

依旧祭出该图，方便查看：

也再次献上我绘制的程序导图全图：ORB-SLAM2 程序导图

老规矩，仍是分两部分：以 ORB-SLAM 论文为参考 和 以 ORB-SLAM2 代码（程序导图）为参考。

以 ORB-SLAM 论文为参考

对于任何一个单目 SLAM 系统来讲，在系统运行之初都要进行初始化，其目的在于，要计算出某两帧的相对位姿，以此来经过三角化获得一些初始 MapPoints，从而获得一个初始的 Map，这样以后的跟踪也好优化也好都在这个基础上进行。在 ORB-SLAM 以前的单目 SLAM 系统的初始化，每每须要依赖真实场景中某样肯定的物体（eg. MonoSLAM）或者须要人工介入（eg. PTAM），而 ORB-SLAM 的单目初始化是彻底自动的。

对 SLAM 基础知识有过了解的同窗会知道，恢复两帧之间的相对运动，有两种模型：基础矩阵（Fundamental）（等同于本质矩阵）模型和单应矩阵（Homography）模型。正常状况下基础矩阵模型应该能够应付，但若是特征点共面（初始化场景中主要是一个平面），或者两帧之间的相对位姿未纯旋转时，基础矩阵的自由度会降低，也就是所谓的退化，相似于方程数少于变量数。此时为了保证运动恢复的精度，就不能再用基础矩阵模型。由此提出了单应矩阵，其假设特征点落在同一平面上，从而适用于这种场景下的运动恢复。

ORB-SLAM 在初始化时，它也不知道场景中的特征点在不在同一平面，因此它选择两种模型各自算一遍（开两个线程同时算），以后计算两种模型各自进行运动恢复的得分，取得分高的模型，再根据该模型，计算两帧之间的相对位姿并进行初始化。

单目初始化具体步骤

1. 提取 ORB 特征点

在当前帧和参考帧中提取 FeaturePoints（只在最优的尺度），同时将当前帧和参考帧的 FeaturePoints 作匹配。若是匹配点不够多，从新初始化。

2. 同时计算两种模型

• 单应矩阵 \(H_{cr}\)：DLT 算法 + RANSAC 迭代
• 基础矩阵 \(F_{cr}\)：8点法 + RANSAC 迭代

同时计算两种模型各自的得分，计算公式以下（其中 M 能够表示 H (Homography)，也能够表示 F (Fundamental)）：

\(S_{M}=\sum_{i}\left(\rho_{M}\left(d_{c r}^{2}\left(\mathbf{x}_{c}^{i}, \mathbf{x}_{r}^{i}, M\right)\right)+\rho_{M}\left(d_{r c}^{2}\left(\mathbf{x}_{c}^{i}, \mathbf{x}_{r}^{i}, M\right)\right)\right)\)

\(\rho_{M}\left(d^{2}\right)=\left\{\begin{array}{ll} {\Gamma-d^{2}} & {\text { if } \quad d^{2}<T_{M}} \\ {0} & {\text { if } \quad d^{2} \geq T_{M}} \end{array}\right.\)

其中 \(d_{c r}^{2}\) \(d_{r c}^{2}\) 是对称转换偏差。\(\rho_{M}()\) 的做用是令大的偏差对低的得分，其中 \(T_M\) 阈值是算出来的：假设测量值偏差的标准差为1像素，经过95%的 \(\chi^{2}\) 检验获得的。对于每种模型，在 RANSAC 迭代过程当中保留得分最高的模型。若是最终没能求出解（对于 RANSAC，inliers不够多），则从新初始化。

3. 选择合适的模型

根据两种模型各自的得分：

\(R_{H}=\frac{S_{H}}{S_{H}+S_{F}}\)

若是 \(R_H\) > 0.45（代码中是 0.4），则选用单应矩阵；反之则选基础矩阵模型。大概就是选得分高的，此处的 0.4 应该是经验值。

4. 进行运动恢复，求解两帧相对位姿（\(R, t\)）并经过三角化获得初始 MapPoints

在求出 \(H_{cr}\) 或 \(F_{cr}\) 后，就要根据该矩阵求出相对位姿（\(R, t\)）。但这个过程的求出的解不是惟一的。ORB-SLAM 采起的筛选办法是：对这些解所有进行三角化恢复 MapPoints，哪一个解恢复出来的 MapPoints 大部分都在相机前方且重投影偏差小，就选哪一个解。若是不能明确选出一个最合适的，则从新初始化。

5. BA 优化

最后，进行一次全局 BA，优化如下，获得最终的初始化结果。

从以上步骤能够看出，ORB-SLAM 在单目初始化花了不少心思。有一个很明显的特色：只要出现一点不妥，ORB-SLAM 就会选择从新初始化。论文中说，这种高标准严要求的初始化准则，是 ORB-SLAM 系统鲁棒性很是好的重要缘由之一。由于若是初始化就不合适或出错，后面的跟踪只会一错再错，错上加错。

以 ORB-SLAM2 代码（程序导图为参考）

在上图中 MonocularInitialization() 就是初始化的程序，咱们能够看到它在 Tracking 线程中的位置。

上图是 MonocularInitialization() 部分的程序框图，其大致和论文中介绍的步骤是彻底一致的，这样图已经很清晰了，这里就很少描述了。

若是嫌这张图不够清晰的话，能够点击 ORB-SLAM2 程序导图连接（文首）查看清晰全图

PS： 从中上图咱们能够看到，在恢复出两帧之间的相对运动后，程序中还要使用 Tracking::CreateInitialMapMonocular() 来创建初始化的地图。这部分在论文里几乎没有笔墨提到，但在程序里须要很大篇幅来实现。这个细节就反映了我在本系列博文开篇就提到过的读通 ORB-SLAM2 代码的困难之处。以我如今小菜鸡的水平，我根本想象不出 ORB-SLAM2 这样复杂而环环相扣的工程是怎么写出来的（流泪）。

ORB-SLAM2 系列博文

ORB-SLAM2 初体验 —— 配置安装

ORB-SLAM2 论文&代码学习 —— 概览

ORB-SLAM2 论文&代码学习 —— Tracking 线程

ORB-SLAM2 论文&代码学习 —— 单目初始化