ORBSLAM2单目初始化过程

转自博客：https://blog.csdn.net/zhubaohua_bupt/article/details/78560966

ORB单目模式的初始化过程可以分为以下四个阶段：

1 通过匹配选取两个可以作为起始两帧的初始帧

2 根据匹配计算两帧之间的位姿

3 三角化测量初始的特征点云深度，进而获得点云地图。

4 BA优化初始点云

在初始化后，单目模式和双目及RGBD模式一样，都是通过PNP来计算位姿。

下面，说一下初始化算法的步骤：

第一阶段：选取两个可以作为起始两帧的初始帧

这一阶段做的工作是，选取是两个特征点数目大于100的两个连续帧，并进行匹配，

只有当前后帧匹配点对比较多时（代码>100），才认为这两帧可以进行初始化并记录下来两帧的匹配关系，

接下来开始尝试求取两帧之间的位姿。

否则从头开始，重新开始尝试初始化。

第二阶段：根据匹配计算两帧之间的位姿

ORB实现的是自动初始化，也就是，无论场景平面，还是普通场景，都能完成初始化工作。

其做法是同时计算适用于平面场景的单应性矩阵（H）和适用于普通场景的基础矩阵(F),

方法是：

首先，由抽样点对，计算出单次抽样的H和F矩阵（DLT，八点法）；通过若干次RANSAC抽样，

计算出最优的H和F矩阵；

然后，通过一定机制比较H和F矩阵，选择最优的矩阵，恢复出来帧间位姿。

并行计算两个模型：

在两个线程上并行计算单映射矩阵模型和基础矩阵模型：

为了使两个模型的计算流程尽量一样，用于计算H和F矩阵的迭代循环次数预先设置成一样（代码中为200次），

每次用迭代计算的匹配特征点对也是一样的。基础矩阵是8个特征点对，单映矩阵是其中的4个特征点对。

从若干次RANSAC中，计算出最优的H和F矩阵：

每次迭代中，给每个模型M（H表示单映射，F表示基本矩阵）计算一个分值SM：

其中

TM是无效数据的排除阈值，

它的依据是卡方测试的95%（TH=5.99, TF=3.84，假设在测量误差上有1个像素的标准偏差）。

TM等于TH，这样，对于同一个重投影误差d，两个模型求取的分值相同。

模型M越准，所有匹配点对的重投影误差越小，SM的值就越大。

因此SM的值反映了一个这样的问题，当其越大时，模型M越准确。

我们从单映射矩阵和基本矩阵的计算中选择分值最高的，分别记作为SH和SF，

但如果两个模型分值都不高（意味着没有足够的局内点），就重新选择第二帧，重新匹配并尝试初始化。

模型选择(用基础矩阵还是用单应性矩阵？)

如果场景是平面，近平面或存在低视差的情况，则可以通过单映矩阵来求解。

同样地，我们也可以找到一个基础矩阵，但问题是基础矩阵不能够很好的约束该问题，而且从基础矩阵中计算得到的运动结果是错误的。在这种情况下，我们应该选择单映矩阵才能保证地图初始的正确性，或者如果检测到低视差的情况则不进行初始化工作。

另一方面，对于非平面场景且有足够的视差的情况则可以通过基础矩阵来计算，

而在这种情况下单映矩阵只有基于平面点或者低视差的匹配点才能找到。因此，

在这种情况下我们应该选择基础矩阵。我们利用如下强大的启发式进行计算：

如果RH>0.45 ,这表示二维平面和低视差的情况，我们将选择计算单应矩阵。其他的情况，我们选择基础矩阵。

从模型恢复位姿

一旦选择好模型，我们就可以获得相应的运动状态。如果选择单映矩阵，我们按照Faugeras等人发表的论文中提到的方法，提取8种运动假设，该方法提出用cheriality测试来选择有效解。

然而，如果在低视差的情况下，这些测试就会失效，因为云点很容易在相机的前面或后面移动，会导致选解错误。我们提出的方法是直接按这8种解将二维点三角化，然后检查是否有一种解可以使得所有的云点都位于两个相机的前面，且重投影误差较小。如果没有一个最优的解，我们就不执行初始化，否则重新选择第二帧，重新匹配并尝试初始化。

这种方法使初始化程序在低视差和两个交叉的视图情况下更具鲁棒性，这也是我们整个算法体现鲁棒性的关键所在。

在基本矩阵的情况下，我们使用标定矩阵K用下式将其转换为本证矩阵：

然后奇异值分解方法计算4个运动解，然后就像上文中叙述的一样，我们将四个解用于三角化特征点，以选择正解。

第三阶段：三角化测量初始的特征点云深度，进而获得点云地图。

一旦恢复出两帧之间位姿（单目无尺度），就可以求解匹配点对的深度（无尺度），求解时可以以任意一帧为基坐标系，由于这里是要以做SLAM，所以以第一帧为世界坐标系。代码中采用多视图几何书中11.2线性三角形法中的齐次方法（DLT）进行三角测量恢复匹配点对应的三维信息。

基本原理如下，已知一组匹配点分别在两幅图像中的二维坐标，利用叉乘为0可以代表共线的几何属性和帧间位姿，构建一个AX=0的方程，其中X是大小4x1向量，是该匹配点对所对应的三维点的齐次表示。然后通过SVD分解来求方程AX=0，对此方程SVD分解本身具有最小二乘效果，可以解决在解方程时，由位姿噪声带来的问题。其基本思想和http://blog.csdn.net/zhubaohua_bupt/article/details/74926111一致。

第四阶段: 对以上恢复的点云和位姿做 Bundleadjustment

最后执行一个全局BA，以优化初始重构得到的点云地图。

以上内容来自于ORBSLAM论文和源码。