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$ 矩阵
可以写成
$\left[\begin{array}{cc}R& t\end{array}\right]={\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\end{array}\right]}_{3\times 4Matrix}$
$p_0,p_1,p_2属于1\times 4$
与点$p=[x,y,z,1]$相乘则有
${\left[\begin{array}{ccc}x& y& 1\end{array}\right]}^T=a{\left[\begin{array}{c}{p}_0\cdot p\\ p_1\cdot p\\ p_2\cdot p\end{array}\right]}_{3\times 1Matrix}$
两个向量满足 $\vec{c}=a\cdot \vec{b}$ 那么则证明这两个向量平行
两个平行向量的叉乘为0，因为$\vec{c}\times \vec{b}=|\vec{c}||\vec{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$ ③
根据${\left[\begin{array}{ccc}x& y& 1\end{array}\right]}^T=a{\left[\begin{array}{c}{p}_0\cdot p\\ p_1\cdot p\\ p_2\cdot p\end{array}\right]}_{3\times 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求解去右奇异矩阵最后一个向量即可，具体原因见上一节。