VINS-Mono-VIO初始化 （二：SFM中的三角化方法）

前面预积分对IMU的数据进行预处理，现在需要对视觉的信息进行处理，在VINS中视觉初始化的处理就是使用SFM，但是这里的三角化他没有用opencv给的函数，而是用自己的方法进行三角化。

这里SFM的方式就是现在滑窗里面找到枢纽帧，然后枢纽帧和最后一帧进行三角化获得3D点，然后通过PNP计算滑窗中其他关键帧的位姿，同时也三角化出更多新的点，顺序是先从枢纽帧向右再向左，然后再遍历只被中间帧看到的点进行三角化

三角化公式推导

公式如下
$[x,y,z{]}^T=[R,t]\cdot p$
其中 $p$ 是世界点 $p=[x,y,z,1]$ ，为了和变换矩阵齐次所以要有第4维为1，$[x,y,z]$ 是在各自相机系下的观测，[R,t] 是两帧间的位姿变换关系，这里面总会有一帧定义为单位阵的，这样才能作为这个3D点的世界系。由于各自相机系下的观测一般都是归一化相机坐标系，所以 $z=1$，所以公式变成
$[x,y,1{]}^T=a\cdot [R,t]\cdot p$
这个a系数可以理解为尺度的倒数 $\frac{1}{s}$ ，就是为了让公式成立的，就是右边变换完后还进行了归一化操作
$\left[\begin{array}{cc}R& t\end{array}\right]属于3\times 4$ 矩阵
可以写成
[ R t ] = [ p 0 p 1 p 2 ] 3 × 4 M a t r i x \begin{bmatrix} R &t\end{bmatrix}=\begin{bmatrix} p_{0} \\ p_{1} \\p_{2}\end{bmatrix}_{3×4 Matrix} [R​t​]= ​p0​p1​p2​​ ​3×4Matrix​
$p_0,p_1,p_2属于1\times 4$
与点$p=[x,y,z,1]$相乘则有
[ x y 1 ] T = a [ p 0 ⋅ p p 1 ⋅ p p 2 ⋅ p ] 3 × 1 M a t r i x \begin{bmatrix} x&y&1 \end{bmatrix}^{T}=a\begin{bmatrix} p_{0}·p\\p_{1}·p\\p_{2}·p\end{bmatrix}_{3×1Matrix} [x​y​1​]T=a ​p0​⋅pp1​⋅pp2​⋅p​ ​3×1Matrix​
两个向量满足 $c=a\cdot b$ 那么则证明这两个向量平行
两个平行向量的叉乘为0，因为$c\times b=|c||b|sin(\theta )$，都平行了这个θ肯定为0，所以整体为0，对上面两个向量进行叉乘则有
$y{p}_{2}p-p_{1}p=0$
$p_{0}p-x{p}_{2}p=0$
$x{p}_{1}p-y{p}_{0}p=0$
把 $p$ 提出来得到
$(y{p}_2-p_1)p=0$
$(p_0-x{p}_2)p=0$
$(x{p}_1-y{p}_0)p=0$ ③
根据 [ x y 1 ] T = a [ p 0 ⋅ p p 1 ⋅ p p 2 ⋅ p ] 3 × 1 M a t r i x \begin{bmatrix} x&y&1 \end{bmatrix}^{T}=a\begin{bmatrix} p_{0}·p\\p_{1}·p\\p_{2}·p\end{bmatrix}_{3×1Matrix} [x​y​1​]T=a ​p0​⋅pp1​⋅pp2​⋅p​ ​3×1Matrix​有 $x=a{p}_{0}p$ ，$y=a{p}_{1}p$，把这两个 $x,y$ 代入到③中
则有 $a{p}_0{p}_1-a{p}_1{p}_0=0$，这里是向量点乘，这个式子必然成立，所以这个式子就没有意义了，公式就变成
$(y{p}_2-p_1)p=0$
$(p_0-x{p}_2)p=0$
一个位姿就会得到两个这个公式，三角化需要两帧，有两个位姿，所以代码中有4行这个公式
$(y{p}_2-p_1)\in 1\times 4$
如果一个特征点可以被多帧看到的话，左边这个矩阵还能继续扩大，整个问题就是求解 $Ax=0$ 的问题，用SVD求解去右奇异矩阵最后一个向量即可，具体原因见上一节。