出问题 初始化ucosiii_[ORB-SLAM2]单目初始化

ORB-SLAM提出一种自动初始化流程，能够根据场景自动的选择模型（Homography or Fundamental），当初始化质量不好的时候则延迟初始化。
本文对初始化过程中的诸多细节进行了总结。
本文属于个人记录，比较乱。

1. 初始化流程

Step 0. 选定一个参考帧，提取ORB特征

选择标准：提取到的ORB特征数量足够多>100个

Step 1. 匹配当前帧与参考帧的ORB特征

1. 当前帧提取的特征数量足够多>100，否则回到第0步。
2. 利用DBOW2的词袋加速特征匹配过程。
3. 如果匹配点数太少<100，则回到第0步。
4. 上面的条件通过之后，进行初始化

Step 2. 同步计算两个模型Homography和Fundamental

Homography模型

基于RANSAC框架，采用4对点计算。

Fundamental模型

基于RANSAC框架，采用8对点计算。

这里，RANSAC的迭代次数是固定的。每次迭代，都为H模型和F模型计算一个分数。

其中，

为

或者

。

为距离阈值，根据95%的

测试设置。

（两个自由度），

（1个自由度）。这里假设标准差为1个pixel。

这里的

与

相同。TODO没看明白。

完成所有的迭代步骤之后，选择分数最高的那一个作为

和

的结果。

Step 3. 模型选择：Homography or Fundamental

计算比例：

如果

就选择Homography为模型，否则就选择Fundamental为模型。

TODO:为什么阈值是0.45也没有看明白。

Step 4. 计算位姿和地图点

根据选择的模型恢复出相对位姿和3D地图点。

Step 5. Bundle Adjustment.

最后在来一个BA。BA大家都很清楚了。

这里有几个关键点：1. 如何估计单应矩阵、基础矩阵。2. 如何从单应矩阵和基础矩阵中分解出旋转R和平移t。

---

2. 单应矩阵 Homography Matrix

在第一帧坐标系下，3D点

的空间位置为

。这些3D点分布在一个平面上，平面的方程为

整理一下：

点投影到第2帧上

由于

都是齐次坐标，所以H是可以随意尺度缩放的。也就是，对于平面场景，两幅图像上的2D点的齐次坐标之间的关系可表示为

将（8）式展开有

可以确定两个方程

解法1：

由于H是尺度缩放，可以通过调整

使得

，这样（9）式变为

由以上推到分析可知，1个点对可以提供两个方程，而我们有8个参数需要求解，这样4个点对就可以求解出

矩阵。多于四个点对就形成了一个超定方程

可以计算一个最小二乘解

，或者利用QR分解

，还有很多解法，请参考

http:// eigen.tuxfamily.org/dox /group__LeastSquares.html 。

解法2:

（9）式具有9个变量，4对点可以确定8个方程：

我们可以获得一个通解

。是一个up to scale的解。实际上我们的H矩阵也是up to scale的。其中

就是M的对应与M的最小奇异值的奇异向量，可以通过SVD分解去求解。也就对应于

最小特征值的特征向量。

ORB-SLAM里面使用的就是这种解法。在ORB-SLAM中的RANSAC中使用了8对点进行计算，主要是为了与F矩阵的求解一致。

上述两种方法的结果没有太大差别，基本上是一致的。值得说明的是，ORB-SLAM在计算Homography的时候对2D特征点进行了归一化。

变成均值为0，方差为1的标准正态分布。作用是什么呢？是不是为了计算稳定呢？可能是吧。

---

3. 基础矩阵 Fundamental Matrix

双视对极几何的产物，还是同一个3D点P，投影到两个相机之下。

将转换到相机坐标系下归一化的坐标有

(12)式子两边右叉乘

，在整理一下

其中的

为本质矩阵，Essential Matrix。描述了两个相机之间归一化相机坐标之间的关系。

再把

和

替换回像素坐标有

其中的

即为基础矩阵，Fundamental Matrix。描述了两个相机之间，像素坐标之间的关系。

同样的

矩阵也是up to scale的。因此，它也只有8个自由度。一般把第9个量固定为1，或者求一个9量的通解。一个（14）可以提供一个方程，那么8个点对就可以计算出

矩阵。解法与单应矩阵相同。具体的，展开（14）式有

于是，大于等于8个点形成了方程

利用第2节中的解法2可以获得

矩阵。

---

4. 分数计算

对应单应矩阵，计算2D点之间马氏距离的平方

服从2自由度的

分布。（3）式中的

和

都取5.99。

对于基础矩阵，计算的误差为点到极线的距离的平方。具体的，在相机2上的极线方程为

距离的平方为

如果假设距离服从均值为0，方差为1个pixel的正态分布，那么

是服从1个自由度的

分布的，因此

取的是3.84。

用来判定内点和外点。

TODO: 但是为什么

都取的5.99呢？这个不知道为什么。

---

5. 由基础矩阵F分解分解出旋转和平移

可参考《MVG》中文版的174页，英文版258页。

已知基础矩阵F，可以获得Essential Matrix矩阵

下面根据Essential Matrix分解出R和t。我们知道E的成分有一个反对称矩阵S和一个正交矩阵R，外加一个scale组成。

实际上只有5个自由度。s可以放到反对称矩阵里面。

先给出两个矩阵

W为正价矩阵，Z为反对称矩阵。

对E进行SVD分解为

总共可以得到四个解，分别是

然后根据：

1. 点在相机的前面，重投影误差足够小，恢复3D点。
2. 根据恢复出3D点的多少来找一个最优解。
3. 如果有多个最优解则放弃这次计算。
4. 如果只有一个最优解，且恢复出了足够的3D点，并且有足够的视差（这里用的是地图点指向两个帧的夹角）就接收。

TODO: 详细解读如何求解。

---

6. 由单应矩阵H恢复出R和t

按照ORB-SLAM的做法，可以恢复8个解，然后从8个解中选择一个最优解。

然后根据恢复出的3D点个数，视差等信息筛选出最优解，要求

1.最优个数 > 0.7 次优个数

2. 最优的视差，大于最小要求的视差

3. 最优的个数，大于最小设置的个数

4. 最优个数，大于匹配点个数*0.9

TODO: 详细解读如何求解。

---

----更多SLAM文章----

杨小东：SLAM轨迹真值获取装置

杨小东：[PnP]PnP问题之EPnP解法

杨小东：[PnP] PnP问题之DLT解法

杨小东：[ORB-SLAM2]卡方分布(Chi-squared)外点(outlier)剔除

杨小东：[ORB-SLAM2] ORB特征提取策略对ORB-SLAM2性能的影响

杨小东：[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM

杨小东：[PR-2] PF 粒子滤波/蒙特卡罗定位

----相关代码----

ydsf16 - Overview​github.com