问题描述
已知两个相机投影矩阵\(P_1, P_2\)，其中，
\[P_1 = K T_{1w}\\
P_2 = K T_{2w}
\]
\(K\)是相机内参矩阵。
同时，已知一对匹配点的像素坐标\((u_1,v_1), (u_2, v_2)\)，要求出这对匹配对应的3D点的世界坐标。
向量叉乘转换矩阵运算
向量叉乘可以转化为矩阵乘法的形式，
\[a \times b = \hat{a}b =
\begin{bmatrix}
0 & -a_3 & a_2 \\
a_3 & 0 & -a_1 \\
-a_2 & a_1 & 0
\end{bmatrix}
\times
\begin{bmatrix}
b_1 \\
b_2 \\
b_3
\end{bmatrix}
\]
其中，\(\hat{a}\)称为向量\(a\)的反对称矩阵。
DLT方法
DLT（Direct Linear Transform）方法是直接线性变换法的简称。假设3D点的世界坐标的齐次坐标形式为\(X = \begin{bmatrix} X \\ Y \\ Z\\ 1 \end{bmatrix}\)，可以得到下面的等式。
\[\lambda \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} = P_1 X
\]
由此可以得知，\(\begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}\) 与 \(P_1 X\)两个向量共线，叉积为0得到下面的等式。
\[\begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} \times (P_1 X) = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}
\]
向量叉乘转化为矩阵形式
\[\begin{bmatrix}
0 & -1 & v_1 \\
1 & 0 & -u_1 \\
-v_1 & u_1 & 0
\end{bmatrix}
P_1 X = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}
\]
\(P_1\)是一个\(3\times4\)的矩阵，将其拆成三个行向量的形式，
\[\begin{bmatrix}
0 & -1 & v_1 \\
1 & 0 & -u_1 \\
-v_1 & u_1 & 0
\end{bmatrix}
\begin{bmatrix}
p_1 \\
p_2 \\
p_3
\end{bmatrix} X = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} \\
\begin{bmatrix}
-p_2 + v_1 p_3 \\
p_1 - u_1 p_3 \\
-v_1 p_1 + u_1 p_2
\end{bmatrix} X = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}
\]
第一行乘\(-u_1\)，第二行乘\(-v_1\)相加得到第三行，线性相关只保留前两行。对于\(P_2\)和\((u_2, v_2)\)可以得到类似的等式。假设\(P_2\)的行向量为\(\begin{bmatrix} p_1^{'} \\ p_2^{'} \\ p_3^{'} \end{bmatrix}\)，得到等式同样保留前两行，得到下面的等式。
\[\begin{bmatrix}
-p_2 + v_1 p_3 \\
p_1 - u_1 p_3 \\
-p_2^{'} + v_2 p_3^{'} \\
p_1^{'} - u_2 p_3^{'}
\end{bmatrix} X = \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} \\
\]
得到
\[A X = 0
\]
使用SVD的方法解方程，将\(A\)进行SVD分解\(A = UDV^{T}\)，根据SVD分解的性质，矩阵\(V\)的最后一个列向量为方程的解。