三角化求3D坐标

问题描述

已知两个相机投影矩阵\(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\)的最后一个列向量为方程的解。