前端：新路标点的三角化

已知：若干个关键帧中，某些像素点对应了同一个新路标点（换言之，某一个路标点被若干个关键帧观测到）（像素点之间的对应关系或关联，可以通过光流跟踪、特征匹配等得到）。
将该已知数学化：已知第$k$帧上该像素的归一化平面坐标为$P_{c_k}=[u_k,v_k,1{]}^T,(k=1,2,\cdots ,n)$，且已知第$k$帧（相对于世界坐标系的）位姿$T_k=[R_k,t_k{]}_{3\times 4}$，求该新路标点的世界坐标系下的坐标$P_w=[X,Y,Z,1{]}^T$（取齐次坐标）。
【注意】此处得位姿是$T_{cw}$，我们知道在【VINS课件及代码】中位姿得定义是$T_{wc}$，因此需要一步转换！！$R_{cw}=R_{wc}^T,t_{cw}=-R_{wc}^T{t}_{wc}$

---

根据投影关系：
$${\lambda }_k{P}_{c_k}=T_k{P}_w,(k=1,2,\cdots ,n)$$

式中，${\lambda }_k$为第$k$帧坐标系下新路标点的深度值，是未知量（但同时我们不关心，应该消去该未知量）。
展开：
$$\begin{array}{rl}{\lambda }_k\left[\begin{array}{c}{u}_k\\ v_k\\ 1\end{array}\right]& =\left[\begin{array}{cccc}{T}_{k,11}& T_{k,12}& T_{k,13}& T_{k,14}\\ T_{k,21}& T_{k,22}& T_{k,23}& T_{k,24}\\ T_{k,31}& T_{k,32}& T_{k,33}& T_{k,34}\end{array}\right]P_w\\ \left[\begin{array}{c}{\lambda }_k{u}_k\\ {\lambda }_k{v}_k\\ {\lambda }_k\end{array}\right]& =\left[\begin{array}{c}{T}_{k,1}^T\\ T_{k,2}^T\\ T_{k,3}^T\end{array}\right]P_w\end{array}$$

消去${\lambda }_k$得：
$$\begin{array}{rl}\left[\begin{array}{c}{u}_k{T}_{k,3}^{T}y\\ v_k{T}_{k,3}^{T}y\end{array}\right]& =\left[\begin{array}{c}{T}_{k,1}^T\\ T_{k,2}^T\end{array}\right]P_w\\ \left[\begin{array}{c}{u}_k{T}_{k,3}^T-T_{k,1}^T\\ v_k{T}_{k,3}^T-T_{k,2}^T\end{array}\right]P_w& =0\end{array}$$

每一帧可以贡献上述两个方程，将$n$帧都罗列出来，如下：
$$\begin{array}{rl}\left[\begin{array}{c}{u}_1{T}_{1,3}^T-T_{1,1}^T\\ v_1{T}_{1,3}^T-T_{1,2}^T\\ u_2{T}_{2,3}^T-T_{2,1}^T\\ v_2{T}_{2,3}^T-T_{2,2}^T\\ \cdots \\ \cdots \\ u_n{T}_{n,3}^T-T_{n,1}^T\\ v_n{T}_{n,3}^T-T_{n,2}^T\end{array}\right]P_w& =0\\ D{P}_w& =0\end{array}$$

解上述线性方程即可求得新路标点得齐次坐标$P_w$。

---

【注】：因为我们确定有非零解，故应该有$rank(D)=3$；在非常理想情况下（即观测无噪声、位姿估计准确）或许可以实现。但是！！ 实际情况却是观测有噪声，位姿估计有误差，故一般情况下是$rank(D)=4$，此时去求解该齐次方程，只能得到零解！！

那么，如何才能从含有一定“噪声”的方程组$D{P}_w=0$中，求解得到非零解P_w呢？
【转化】为寻找最小二乘解：
$$\underset{y}{\min }\Vert Dy{\Vert }_2^2,s.t.\Vert y\Vert =1$$

该式的含义为，寻找使得$Dy$的二范数取得最小值得$y$，且这个$y$得模长为1（该条件主要是为了规避$y$为零解得情况）。

此处直接给出结论：$y=U_4$即为该最小二乘问题得解。
其中$U_4$为对$D^{T}D$进行奇异值分解得到的奇异值${\sigma }_4$对应的向量（或酉矩阵第4列）。

---

取得$y=U_4$，只需同除以第四个元素，转化为齐次坐标即可。

【对奇异值得一些理解】

每个矩阵$A$都可以表示为一系列秩为1的“小”矩阵的和，而奇异值则是“小”矩阵前的权重，衡量了某个“小”矩阵对于$A$的权重。

奇异值分解和特征值分解的区别：
所有的矩阵都可以进行奇异值分解，而只有方阵才可以进行特征值分解。
当所给的矩阵是对称的方阵时，$A^T=A$，奇异值分解和特征值分解的结果是相同的。即，对称矩阵的特征值分解是所有奇异值分解的一个特例。但是二者还是存在一些小的差异，奇异值分解需要对奇异值从大到小的排序，且奇异值均大于等于零。
Matlab代码：
对于特征值分解 [v,d] = eig(A) , 即 A = v*d*inv(v)；
对于奇异值分解,其分解的基本形式为 [u,s,v] = svd(C), C = u*s*v'.
若C阵为对称的方阵, 则有 u = v; 所以有 C = v*s*v'。