多視幾何——三角化求解3D空間點坐標

VINS-Mono / VINS-Fusion中triangulatePoint()函數通過三角化求解空間點坐標，代碼所體現的數學描述不是很直觀，查找資料，發現參考文獻[1]對這個問題進行詳細解釋，記錄筆記以備忘。

1. VINS-Mono中相關代碼

void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                        Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
{
    Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero();
    design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
    design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
    design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
    design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
    Eigen::Vector4d triangulated_point;
    triangulated_point =
              design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
    point_3d(0) = triangulated_point(0) / triangulated_point(3);
    point_3d(1) = triangulated_point(1) / triangulated_point(3);
    point_3d(2) = triangulated_point(2) / triangulated_point(3);
}

2. 數學推導

先介紹一個重要的向量叉乘向矩陣運算轉換的性質[4],

\[\mathbf{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}\begin{bmatrix}b_1\\b_2 \\b_3\end{bmatrix} \tag{2-1} \]

其中，\(\mathbf{\hat{a}}\)表示向量\(\mathbf{a}\)對應的反對稱矩陣(skew-symmetric matrix)。
對於，\(X\)為三維空間點在世界坐標系下的齊次坐標\(X=\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\)，和 \(T=\begin{bmatrix}\mathbf{r_1} \\ \mathbf{r_2} \\ \mathbf{r_3}\end{bmatrix}=\begin{bmatrix}R &\mathbf{t}\end{bmatrix}\)為世界坐標系到相機坐標系的變換。和 \(\mathbf{x}=\begin{bmatrix}u\\v\\1\end{bmatrix}\)為歸一化平面坐標，\(\lambda\)為深度值，有：

\[\lambda \mathbf{x} = TX \\ \Rightarrow \lambda\mathbf{x}\times TX=0 \\ \Rightarrow \mathbf{\hat{x}}TX=0 \\ \tag{2-2} \]

借助式(2-1)展開，有：

\[\begin{align} \mathbf{\hat{x}}TX &= \begin{bmatrix}0 &-1 &v \\ 1 &0 &-u \\ -v &u &0\end{bmatrix} \begin{bmatrix}\mathbf{r_1} \\ \mathbf{r_2} \\ \mathbf{r_3}\end{bmatrix}X\\ &=\begin{bmatrix}-\mathbf{r_2}+v\mathbf{r_3} \\ \mathbf{r_1}-u\mathbf{r_3} \\ -v\mathbf{r_1} + u\mathbf{r_2}\end{bmatrix}X\\ \tag{2-3} \end{align} \]

其中\(\begin{bmatrix}-\mathbf{r_2}+v\mathbf{r_3} \\ \mathbf{r_1}-u\mathbf{r_3} \\ -v\mathbf{r_1} + u\mathbf{r_2}\end{bmatrix}\)，第一行\(\times -u\),第二行\(\times -v\),再相加，即可得到第三行，因此，其線性相關，保留前兩行即可，有

\[\begin{bmatrix}-\mathbf{r_2}+v\mathbf{r_3} \\ \mathbf{r_1}-u\mathbf{r_3} \end{bmatrix}X=0 \tag{2-4} \]

因此，已知一個歸一化平面坐標\(\mathbf{x}\)和變換\(T_{cw}\),可以構建兩個關於\(X\)的線性方程組。有兩個以上的圖像觀測，即可求出\(X\)

\[\begin{bmatrix}-\mathbf{r_2}^{(1)}+v^{(1)}\mathbf{r_3}^{(1)} \\ \mathbf{r_1}^{(1)}-u^{(1)}\mathbf{r_3}^{(1)} \\ -\mathbf{r_2}^{(2)}+v^{(2)}\mathbf{r_3}^{(2)} \\ \mathbf{r_1}^{(2)}-u^{(2)}\mathbf{r_3}^{(2)} \\ \vdots\end{bmatrix}X=0 \tag{2-5} \]

上述方程沒有非零解，使用SVD求最小二乘解，解可能不滿足齊次坐標形式（第四個元素為1），因此，

\[X = \begin{bmatrix}x\\y\\z\\1\end{bmatrix} = \begin{bmatrix}x_0/x_3\\x_1/x_3\\x_2/x_3\\x_3/x_3\end{bmatrix} \tag{2-6} \]

求得空間點坐標，但是這個解幾何意義不明確[1],屬於代數最小誤差解。不等價於最小重投影誤差，也不是最小化3D點距離誤差。詳細解釋參考[1]中PPT。

3. 擴展

在VINS-Mono中給出了歸一化平面坐標\(\mathbf{x}=\begin{bmatrix}u\\v\\1\end{bmatrix}\)，如果只是給出像素坐標\(\mathbf{x'}=\begin{bmatrix}u'\\v'\\1\end{bmatrix}\)，並且已知相機內參\(K\)，求解3D點坐標方式類似。

\[\begin{align} &\space\lambda\mathbf{x}' = KTX \\ \Rightarrow &\space\lambda\mathbf{x}'\times KTX=0 \\ \Rightarrow &\space\mathbf{\hat{x}'}KTX=0 \\ \Rightarrow &\space\mathbf{\hat{x}'}PX=0 \\ \tag{3-1} \end{align} \]

其中，$\mathbf{\hat{x}'}PX=0 $的形式與式(2-3)的形式一致，同理可以得到式(2-4)的方程。

4. 總結

在多視幾何的公式推導中，多次使用"共線向量叉乘等於0"的性質，將等式的某一項置為0，式(2-2)中應用了該性質，在本質矩陣推導中，也應用了該性質，如本質矩陣的前兩步推導：

\[\begin{align} X_2 &= RX_1 + T \\ \hat{T}X_2 &= \hat{T}RX_1+\hat{T}T \\ \tag{4-1} \end{align} \]

在多視幾何公式推導中，另一個常用的性質是"互相垂直的兩個向量點乘為零"，在此不再展開。

參考文獻

[1] Lecture 7.2 Triangulation https://www.uio.no/studier/emner/matnat/its/UNIK4690/v16/forelesninger/lecture_7_2-triangulation.pdf