ORB-SLAM2系列第三章—— 地图初始化

---

前言

迎浏览我的SLAM专栏，包括slam安装运行、代码注释、原理详解，一起加油淦穿SLAM。

---

一、为什么需要地图初始化？

在ORB-SLAM2中初始化和使用的传感器类型有关，其中单目相机模式初始化相对复杂，需要运行一段时间才能成功初始化。而双目相机、
RGB-D相机模式下比较简单，一般从第一帧开始就可以完成初始化。
为什么不同传感器类型初始化差别这么大呢？
我们从最简单的RGB-D相机初始化来说，因为该相机可以直接输出RGB图像和对应的深度图像，所以每个像素点对应的深度值是确定的，也
就是说，我在第一帧提取了特征点后，特征点对应的三维点在空间的绝对坐标是可以计算出来的（需要用到内参）。
对于双目相机来说，也可以通过第一帧左右目图像立体匹配来得到特征点对应的三维点在空间的绝对坐标。因为第一帧的三维点是作为地图
来实现跟踪的，所以这些三维点我们也称为地图点。所以理论来说，双目相机、RGB-D相机在第一帧就可以完成初始化。
而对于单目相机来说，仅仅有第一帧无法得到三维点，想要初始化，需要像双目相机那样去进行立体匹配。

二、多视图几何基础

1.对极约束示意图

2.H矩阵求解原理

等式左边两项分别用A, X表示，则有:
AX = 0
一对点提供两个约束等式，单应矩阵H总共有9个元素，8个自由度（尺度等价性），所以需要4对点提供8个约束方程就可以求解。

3.哪个奇异向量是最优解？

为什么VT的第9个奇异向量就是最优解？
Ah=0 对应的代价函数

最优解是导数为0

问题就转换为求ATA的最小特征值向量

可见 的特征向量就是 的特征向量。因此求解得到V 之后取出最后一行奇异值向量作为f的最优值，然后整理成3维矩阵形式。（其实其他行的奇异值向量也是一个解，但是不是最优解）

3.求解基础矩阵F

推导F矩阵约束方程

4.SVD

SVD分解结果

假设我们使用8对点求解，A 是 8x9 矩阵，分解后

U 是左奇异向量，它是一个8x8的 正交矩阵，
V 是右奇异向量，是一个 9x9 的正交矩阵， 是V的转置
D是一个8 x 9 对角矩阵，除了对角线其他元素均为0，对角线元素称为奇异值，一般来说奇异值是按照从大到小的顺序降序排列。因为每个
奇异值都是一个残差项，因此最后一个奇异值最小，其含义就是最优的残差。因此其对应的奇异值向量就是最优值，即最优解。
VT中的每个列向量对应着D中的每个奇异值，最小二乘最优解就是VT对应的第9个列向量，也就是基础矩阵F的元素。这里我们先记做
Fpre，因为这个还不是最终的F。

F矩阵秩为2
基础矩阵 F 有个很重要的性质，就是秩为2，可以进一步约束求解准确的F
上面的方法使用 对应的第9个列向量构造的Fpre 秩通常不为2，我们可以继续进行SVD分解。

其最小奇异值人为置为0，这样F矩阵秩为2

此时的F就是最终得到的基础矩阵。

5.单目投影恢复3D点

等式左边两项分别用A, X表示，则有

SVD求解，右奇异矩阵的最后一行就是最终的解。

---

三、 卡方检验

1.为什么要引用卡方检验?

以特定概率分布为某种情况建模时，事物长期结果较为稳定，能够清晰进行把握。比如抛硬币实验。
但是期望与事实存在差异怎么办？偏差是正常的小幅度波动？还是建模错误？此时，利用卡方分布分析结果，排除可疑结果。
简单来说：当事实与期望不符合情况下使用卡方分布进行检验，看是否系统出了问题，还是属于正常波动.

检查实际结果与期望结果之间何时存在显著差异。
1、检验拟合程度：也就是说可以检验一组给定数据与指定分布的吻合程度。如：用它检验抽奖机收益的观察频数与我们所期望的吻合程
度。
2、检验两个变量的独立性：通过这个方法检查变量之间是否存在某种关系。

2.卡方分布假设检验步骤？

1、确定要进行检验的假设（H0）及其备择假设H1.
2、求出期望E.
3、确定用于做决策的拒绝域（右尾）.
4、根据自由度和显著性水平查询检验统计量临界值.
5、查看检验统计量是否在拒绝域内.
6、做出决策.
决策原则：
如果位于拒绝域内我们拒绝原假设H0，接受H1。
如果不在拒绝域内我们接受原假设H0，拒绝H1
检验统计量38.272 > 9.49 位于拒绝域内

检验统计量拒绝域内外判定：
1、求出检验统计量a
2、通过自由度和显著性水平查到拒绝域临界值b
3、a>b则位于拒绝域内，反之，位于拒绝域外。

这就不写例子了，详见张宇

3.ORB-SLAM2中的卡方检测剔除外点策略

误差的定义：
就特征点法的视觉SLAM而言，一般会计算重投影误差。具体而言，记U为特征点的2D位置，U-为由地图点投影到图像上的2D位置。重投影误差为

重投影误差服从高斯分布

其中

而

为多维标准正态分布。
也就是说不同金字塔层提取的特征，计算的重投影误差都被归一化了，或者说去量纲化了，那么，我们只用一个阈值就可以了。
可见：
金字塔层数越高，图像分辨率越低，特征提取的精度也就越低，因此协方差越大
单目投影为2自由度，在0.05的显著性水平（也就是95%的准确率）下，卡方统计量阈值为5.99
双目投影为3自由度，在0.05的显著性水平（也就是95%的准确率）下，卡方统计量阈值为7.81
双目匹配到的特征点在右图中的x坐标为 ，重投影后计算得到特征点左图的x坐标 ，根据视差

从而得到重投影后右图中特征点x坐标

disparity就是另一个自由度。
LocalMapping.cc 里面

const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
const float invz1 = 1.0/z1;
if(!bStereo1)
{
float u1 = fx1*x1*invz1+cx1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）自由度2
if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
continue;
}
else
{
float u1 = fx1*x1*invz1+cx1;
float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1; // 根据视差公式计算假想的右目坐标
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
// 自由度为3
if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
continue;
}

---

四、单目SFM地图初始化

归一化对应函数Initializer::Normalize

1.为什么要归一化？

Ah=0
矩阵A是利用8点法求基础矩阵的关键，所以Hartey就认为，利用8点法求基础矩阵不稳定的一个主要原因就是原始的图像像点坐标组成的系
数矩阵A不好造成的，而造成A不好的原因是像点的齐次坐标各个分量的数量级相差太大。基于这个原因，Hartey提出一种改进的8点法，在
应用8点法求基础矩阵之前，先对像点坐标进行归一化处理，即对原始的图像坐标做同向性变换，这样就可以减少噪声的干扰，大大的提高8
点法的精度。
预先对图像坐标进行归一化有以下好处：

• 能够提高运算结果的精度
• 利用归一化处理后的图像坐标，对任何尺度缩放和原点的选择是不变的。归一化步骤预先为图像坐标选择了一个标准的坐标系中，消除了坐标变换对结果的影响。
  归一化操作分两步进行，首先对每幅图像中的坐标进行平移（每幅图像的平移不同）使图像中匹配的点组成的点集的形心（Centroid）移动
  到原点；接着对坐标系进行缩放使得各个分量总体上有一样的平均值，各个坐标轴的缩放相同的

使用归一化的坐标虽然能够在一定程度上消除噪声、错误匹配带来的影响，但还是不够的。
参考：
https://www.cnblogs.com/wangguchangqing/p/8214032.html

2.具体归一化操作

疑问：变换矩阵T为何这样？

答案：就是把上述变换用矩阵表示了而已

3.检查位姿的有效性

---

五、 双目地图初始化：稀疏立体匹配

1.双目相机

2.稀疏立体匹配原理

函数ComputeStereoMatches()
两帧图像稀疏立体匹配
*输入：两帧立体矫正后的图像对应的orb特征点集
*过程：
1.行特征点统计
2.粗匹配
3.精确匹配SAD
4.亚像素精度优化
5.最有视差值/深度选择
6.删除离缺点（outliers）
*输出：稀疏特征点视差图/深度图和匹配结果

3.亚像素插值