三角化

三角化

向量叉乘向矩陣運算轉換

$a \times b=\hat ab= \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}$
其中，$\hat a$表示向量$a$對應的反對稱矩陣。

三角化推導

已知三維空間點在世界座標系下的齊次座標：
$X= \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$
世界座標系到相機座標系的變換：
$T=\begin{bmatrix} R & T \end{bmatrix}=\begin{bmatrix} r_1 \\ r_2 \\ r_3 \end{bmatrix}$
$x$爲相機歸一化平面座標：
$x=\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}$
$\lambda$爲深度值，已知以上條件有：
$\lambda x = TX \Rightarrow \lambda x \times TX = 0 \Rightarrow \hat x TX = 0$
展開上式得：
$\hat x T X = \begin{bmatrix} 0 & -1 & v \\ 1 & 0 & -u \\ -v & u & 0 \end{bmatrix} \begin{bmatrix} r_1 \\ r_2 \\ r_3 \end{bmatrix} X = \begin{bmatrix} -r_2 + vr_3 \\ r_1 - ur_3 \\ -vr_1 + ur_2 \end{bmatrix} X$
其中：
$\begin{bmatrix} -r_2 + vr_3 \\ r_1 - ur_3 \\ -vr_1 + ur_2 \end{bmatrix}$
第一行 $\times (-u)$,第二行 $\times(-v)$ ,再相加，即可得到第三行，因此，其線性相關，保留前兩行即可，有
$\begin{bmatrix} -r_2 + vr_3 \\ r_1 - ur_3 \end{bmatrix}X = 0$
因此，已知一個歸一化平面座標$x$和變換矩陣$T$可以構建兩個關於$X$的線性方程組。有兩個以上的圖像觀測即可求出$X$
$\begin{bmatrix} -r_2^{\left( 1\right)} + v^{\left(1\right)} r_3^{\left(1\right)}\\ r_1^{\left(1\right)} - u^{\left(1\right)}r_3^{\left(1\right)} \\ -r_2^{\left( 2\right)} + v^{\left(2\right)} r_3^{\left(2\right)}\\ r_1^{\left(2\right)} - u^{\left(2\right)}r_3^{\left(2\right)} \\ \vdots \end{bmatrix}X=0$
上式方程沒有非零解，使用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}$

代碼：

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
    Matrix4d design_matrix = 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);
    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);
}