多视几何——三角化求解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 imes b = hat{a}b}=egin{bmatrix}0 &-a_3 &a_2 \ a_3 &0 &-a_1 \ -a_2 &a_1 &0end{bmatrix}egin{bmatrix}b_1\b_2 \b_3end{bmatrix} ag{2-1} ]

其中，(mathbf{hat{a}})表示向量(mathbf{a})对应的反对称矩阵(skew-symmetric matrix)。
对于，(X)为三维空间点在世界坐标系下的齐次坐标(X=egin{bmatrix}x\y\z\1end{bmatrix})，和 (T=egin{bmatrix}mathbf{r_1} \ mathbf{r_2} \ mathbf{r_3}end{bmatrix}=egin{bmatrix}R &mathbf{t}end{bmatrix})为世界坐标系到相机坐标系的变换。和 (mathbf{x}=egin{bmatrix}u\v\1end{bmatrix})为归一化平面坐标，(lambda)为深度值，有：

[lambda mathbf{x} = TX \ Rightarrow lambdamathbf{x} imes TX=0 \ Rightarrow mathbf{hat{x}}TX=0 \ ag{2-2} ]

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

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

其中(egin{bmatrix}-mathbf{r_2}+vmathbf{r_3} \ mathbf{r_1}-umathbf{r_3} \ -vmathbf{r_1} + umathbf{r_2}end{bmatrix})，第一行( imes -u),第二行( imes -v),再相加，即可得到第三行，因此，其线性相关，保留前两行即可，有

[egin{bmatrix}-mathbf{r_2}+vmathbf{r_3} \ mathbf{r_1}-umathbf{r_3} end{bmatrix}X=0 ag{2-4} ]

因此，已知一个归一化平面坐标(mathbf{x})和变换(T_{cw}),可以构建两个关于(X)的线性方程组。有两个以上的图像观测，即可求出(X)

[egin{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)} \ vdotsend{bmatrix}X=0 ag{2-5} ]

上述方程没有非零解，使用SVD求最小二乘解，解可能不满足齐次坐标形式（第四个元素为1），因此，

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

求得空间点坐标，但是这个解几何意义不明确[1],属于代数最小误差解。不等价于最小重投影误差，也不是最小化3D点距离误差。详细解释参考[1]中PPT。

3. 扩展

在VINS-Mono中给出了归一化平面坐标(mathbf{x}=egin{bmatrix}u\v\1end{bmatrix})，如果只是给出像素坐标(mathbf{x'}=egin{bmatrix}u'\v'\1end{bmatrix})，并且已知相机内参(K)，求解3D点坐标方式类似。

[egin{align} &spacelambdamathbf{x}' = KTX \ Rightarrow &spacelambdamathbf{x}' imes KTX=0 \ Rightarrow &spacemathbf{hat{x}'}KTX=0 \ Rightarrow &spacemathbf{hat{x}'}PX=0 \ ag{3-1} end{align} ]

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

4. 总结

在多视几何的公式推导中，多次使用"共线向量叉乘等于0"的性质，将等式的某一项置为0，式(2-2)中应用了该性质，在本质矩阵推导中，也应用了该性质，如本质矩阵的前两步推导：

[egin{align} X_2 &= RX_1 + T \ hat{T}X_2 &= hat{T}RX_1+hat{T}T \ ag{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