视觉里程计中的三角化问题

在视觉里程计中，通常需要通过三角化方法来计算路标点在世界坐标系下坐标。
三角化方法的数学表述如下：

• 考虑某路标点 $y$ 在若干个关键帧 $k=1,\dots ,n$ 中被观察到。
• $y\in {\mathbb{R}}^4$，是齐次坐标。每次的观测值为 $x_k=[u_k,v_k,1{]}^{\top }$，取相机坐标系下的归一化平面坐标，这样的好处是可以不考虑相机的内参。
• 记投影矩阵 $P_k=[R_k|t_k]\in {\mathbb{R}}^{3\times 4}$，为世界坐标系到相机坐标系。
• 投影关系：$$\forall k,{\lambda }_k{x}_k=P_{k}y\text{\hspace{1em}(1)}$$其中，${\lambda }_k$ 为路标点在第 $k$ 帧的深度值。
• 根据式（1）的第三行，有：$${\lambda }_k=P_{k,3}y\text{\hspace{1em}(2)}$$其中$P_{k,3}$ 为$P_k$ 的第3 行。
• 将式（2）代入式（1）的前两行，得到 $$\begin{gathered} u_k{P}_{k,3}y=P_{k,1}y \\ {v}_k{P}_{k,3}y=P_{k,2}y\text{\hspace{1em}(3)} \end{gathered}$$
• 每一次观测将提供两个这样的方程，视 $y$ 为未知量，并将 $y$ 移到等式一侧，得到：$$\left[\begin{array}{c}{u}_1{P}_{1,3}-P_{1,1}\\ v_1{P}_{1,3}-P_{1,2}\\ ⋮\\ u_n{P}_{n,3}-P_{n,1}\\ v_n{P}_{n,3}-P_{n,2}\end{array}\right]y=0\to Dy=0\text{\hspace{1em}(4)}$$于是，$y$ 是系数矩阵D的零空间中的一个非零元素。
• 由于$D\in {\mathbb{R}}^{2n\times 4}$，在观测次数大于等于两次时，很可能 $D$ 满秩，没有非零元素 $y$ 满足式（4）。
• 因此我们将最小二乘问题式（5）的解作为 $y$ 的解。$y$ 是齐次坐标，归一化以后得到路标点在世界坐标系下的坐标。
  $$\underset{y}{\min }f(y)=\underset{y}{\min }{\Vert Dy\Vert }_2^2,s.t.{\Vert y\Vert }_2^2=1\text{\hspace{1em}(5)}$$

下面来证明式（5）的解就是系数矩阵D的最小的奇异值所对应的特征向量：
式（5）即
$$\begin{gathered} \underset{y}{\min }f(y)=\underset{y}{\min }{\Vert Dy\Vert }_2^2=\underset{y}{\min }{y}^T{D}^{T}Dy \\ {\Vert y\Vert }_2^2=y^{T}y=1\text{\hspace{1em}(6)} \end{gathered}$$
对 $D^{T}D$ 进行SVD分解，得到：
$$D^{T}D=U\Sigma U^T=\sum _{i=1}^4{\sigma }_i^2{u}_i{u}_i^T\text{\hspace{1em}(7)}$$其中${\sigma }_i$是矩阵D的奇异值，由大到小排列，$u_i,u_j$ 是正交的单位特征向量，构成${\mathbb{R}}^4$的一组单位正交基。
$$\begin{gathered} {\sigma }_1\ge {\sigma }_2\ge {\sigma }_3\ge {\sigma }_4>0 \\ {u}_i^T{u}_j=\{\begin{array}{ll}1& \text{if }i=j\\ 0& \text{if }i\ne j\end{array}\text{\hspace{1em}(8)} \end{gathered}$$

因此，$y$ 可以表达成 $u_i$ 的一个线性组合。假设 $$y=\sum _{i=1}^4{x}_i{u}_i$$其中$x_i$为未知数，则由式（6）、（7）和（8）可得
$$\begin{gathered} \underset{y}{\min }f(y)=\underset{x_i}{\min }\sum _{i=1}^4{x}_i^2{\sigma }_i^2 \\ \sum _{i=1}^4{x}_i^2=1\text{\hspace{1em}(9)} \end{gathered}$$又由于${\sigma }_i$由大到小排列且大于0，易知
$$\sum _{i=1}^4{x}_i^2{\sigma }_i^2\ge (\sum _{i=1}^4{x}_i^2){\sigma }_4^2={\sigma }_4^2\text{\hspace{1em}(10)}$$ 式（10）中的等号，在$x_1=x_2=x_3=0，x_4=1$时成立。

因此，
$$\underset{y}{\min }f(y)=\underset{x_i}{\min }\sum _{i=1}^4{x}_i^2{\sigma }_i^2={\sigma }_4^2\text{\hspace{1em}(11)}$$
此时
$$y=\sum _{i=1}^4{x}_i{u}_i=u_4\text{\hspace{1em}(12)}$$即，$y$ 是系数矩阵D的最小奇异值所对应的特征向量，证明完毕。

此时的$y\in {\mathbb{R}}^4$是一个4维单位向量，乘以一个比例因子使得第4维等于1之后，取前3维的值，即可得到路标点在世界坐标系下的坐标。