ORB_SLAM2关键算法分析1——基础矩阵F和单应矩阵H初始化位姿

1.基础矩阵和单应矩阵分析
基础矩阵和单应矩阵的求法在高博的视觉SLAM十四讲中已经说的很明白了，我就直接将公式搬上来：
F矩阵：

通过8点法求得F矩阵后，需要对F矩阵进行SVD分解得到t，R。由于F是t和R矩阵相乘得到，如果观测到的特征点（8点法的点）共面或者相机发生纯旋转，就会出现降维，也就是退化情况，会将噪声的影响放大，最终的结果不稳定。其实很好理解，观测点共面或者纯旋转时t为0，这时正推F也为零，但如果你反推先计算出来F矩阵（不为零），要SVD分解出t，R，你得到的t（一定不为零），R必然不准确，也就可以理解成受噪声影响较大。
注意E矩阵5个自由度，原因是由于秩2约束（减去3个自由度）和缺少尺度因子（减去一个自由度）。这也就是为什么单目初始化的时候无法获得系统尺度的原因，F矩阵7个自由度，下面H矩阵也是一样的情况，缺少尺度。

H矩阵：
p_2=Hp_1=K(R-(tn^t)/d)K^(-1) p_1
通过4点法可以求得H矩阵，由H矩阵的公式可以知道，H是R和t相加减得到，可以直接使用解析法得到R和t。（就是个二元一次方程）当观测点共面或者相机发生纯旋转时，t为0时H不为零，直接等于R，所以H矩阵很适合共面和纯旋转问题，没有将噪声项放大。
注意H矩阵8个自由度。
由前面的分析可以分两种情况
（1）相机发生纯旋转
此时t实际上为0，直接使用H矩阵解算旋转，其对应的重投影误差比较小，如果使用F矩阵，重投影误差肯定较大，且解算出的t不为零，与真实0值有差距。
（2）观测点共面
此时t实际上不为零，如果使用H矩阵计算，会得到t和R，如果使用F矩阵计算，会放大噪声项，重投影误差较大。
因此在ORB_SLAM2中采用同时计算F和H矩阵，选取重投影误差较小的一项作为计算结果。

2.ORB_SLAM2中H矩阵求解位姿
2.1求解单应矩阵归一化直接线性法DLT（注意和SLAM十四讲中不同）
（1）首先定义单应矩阵和对应像素点间的方程如下：

将上式的两个像素点归一化除以z，得到归一化像素坐标下的方程，将方程展开：


将上面的公式整理成矩阵形式：


到这里需要注意使用的是归一化8点法求H矩阵，因此的AH=0是一个超定方程，其解不唯一，那么只能对A进行最小二乘求解最优值。采用SVD分解法求这个最小二乘问题：

每个奇异值都是一个残差项，（这里也表明奇异值不会小于零，因为残差都是大于等于零）奇异值按照降序排列，因此第9个奇异值最小，其含义就是最优的残差，因此其对应的奇异值向量就是最优值，即最优解。很显然右奇异向量VT是一个9*9的向量，其对应的第9个奇异向量就是最优解。下面证明为什么VT的第9个奇异向量就是最优解。
2.2最优解推导
将Ah=0求代价函数：

这是一个二次函数类型，最优值是导数为0处：

假设这里h的模值为1，即ATA=0，且是一个9*9的方阵。为了使代价函数最优，即是倒数趋于0，即是得到ATA的最小特征值。也是是在||h||=1的约束下，其最小二乘解为矩阵ATA最小特征值所对应的特征向量。
问题就变成了求ATA的最小特征值向量
ATA=(V*DT*UT)(U*DV*T)=V(DTD)*VT
可见ATA的特征向量就是VT的特征向量。因此ORB_SLAM2中求解得到VT之后取出最后一行奇异值向量作为H的最优值，然后整理成3维矩阵形式。（其实其他行的奇异值向量都是一个解，但是不是最优解）

2.2单应矩阵恢复R和t
待续。。。
通过RANSAC方法选取最优结果集合中的最优值。

3.ORB_SLAM2中F矩阵求解位姿
3.1归一化8点法求F基础矩阵
整个过程和求H矩阵相同，也是归一化8点法求F，取ATA最小特征值对应的特征向量。但是由于基本矩阵是奇异的，表现秩为2，但是直接SVD（最小二乘方法）分解得到的矩阵并没办法满足这一性质，因此采取强迫约束，定义一个矩阵F‘来代替F矩阵，其中detF’=0,下是范数||F-F’||达到最小，实现方式就是ORB_SLAM2中所示，再次使用SVD分解上面得到的矩阵F，即F=U*D*VT，其中D=diag(r,s,t),满足r>=s>=t,t的值最接近与零，因此设定D‘=diag(r,s,0)，令F’=U*D’*VT，这样使得范数||F-F’||最小。
这里再提一下归一化8点法，其实就是一个简单的变换（包括平移和缩放），将极大的改善问题的求解，提高结果的稳定性。在ORB_SLAM2中定义的T1和T2就是这个变换，相当于每个点都乘以这个变换，因此在结果的最后也需要在乘回来。

3.2基础矩阵恢复R，t
待续。。。