三角化

以第一幅图的相机坐标系为参考系，已知空间点X在两幅图像中的投影分别是$(x_1,y_1),(x_2,y_2)$，相机的内参矩阵K，以及第一帧相机坐标系到第二帧的旋转和平移$R_{21},t_{21}$.求空间点X的坐标。

解：

整个方法流程跟求单应矩阵类似，都是用了DLT，先都写成齐次坐标的形式：

$$s_1\left[\begin{matrix} x_1\\ y_1\\ 1 \end{matrix}\right]=KX=K\left[\begin{matrix} I|0 \end{matrix}\right]\left[\begin{matrix} X\\ 1 \end{matrix}\right]\\ s_2\left[\begin{matrix} x_2\\ y_2\\ 1 \end{matrix}\right]=K(R_{21}X+t_{21})=K[R_{21}|t_{21}]\left[\begin{matrix} X\\ 1 \end{matrix}\right]$$

令

$$P_1=K[I|0]\\ P_2=K[R_{21}|t_{21}]$$

则

$$\left[\begin{matrix} x_1\\ y_1\\ 1 \end{matrix}\right]=P_1\left[\begin{matrix} X\\ 1 \end{matrix}\right]\\ \left[\begin{matrix} x_2\\ y_2\\ 1 \end{matrix}\right]=P_2\left[\begin{matrix} X\\ 1 \end{matrix}\right]$$

叉乘，写成齐次方程的形式：

$$\left[\begin{matrix} x_1\\ y_1\\ 1 \end{matrix}\right]_{\times}P_1\left[\begin{matrix} X\\ 1 \end{matrix}\right]=0\\ \left[\begin{matrix} 0&-1&y_1\\ 1&0&-x_1\\ -y_1&x_1&0 \end{matrix}\right]\left[\begin{matrix} --p_0--\\ --p_1--\\ --p_2-- \end{matrix}\right]X=0\\ \left[\begin{matrix} -p_1+y_1p_2\\ p_0-x_1p_2\\ -y_1p_0+x_1p_1 \end{matrix}\right]X=0$$

第三行可由前两行线性变换得到，所以可以把第三行去掉，只看前两行，对于第二幅的点也可得到两个方程，所以一共可以得到4组齐次方程，其系数矩阵记为A，通过SVD分解$A=U \sum V^T$，最小特征值对应的特征向量就是解，即V的最后一列列向量$v$，$v/v[3]$的前三个元素即为空间点的空间坐标。