为什么三角化

在SLAM十四讲中，高博对三角化做了比较清晰的介绍，为什么需要进行三角化呢？

我们知道，对于单目视觉SLAM，在我们对特征点进行匹配之后，可以通过相邻帧的特征点之间的对极约束，从而求解两帧之间的相机运动，但是，由于单目没有深度信息，因此无法判断特征点的具体位置，所以我们使用三角化，通过相机的运动来估计特征点的位置。

如何三角化

如何进行三角化？

三角测量起初是高斯提出并应用于天文学中的，例如，我们可以通过看不同季节的星星相对我们的角度，判断该星星距离我们的距离。相同地，我们可以用过特征点在不同帧的位置，从而求得特征点的深度。

如图所示，图左侧为参考帧，图右侧为当前帧，帧间的相机运动为 $T$，如图，理论上，两帧中的光心和特征点的连线（$O_1p_1$和$O_2p_2$）应该相交于点 $P$，但是，实际上，由于受到噪声的影响，两条直线往往无法相交，因此，可以通过最二小乘去求解 。

按照对极几何定义：

$$ s_1x_1=s_2Rx_2+t\tag{1} $$

已知两帧的相对位姿关系 $R,t$，想要求解的是两个特征点的深度 $s_1$和 $s_2$：

$$ s_1x_1^{\land}x_1=0=s_2x_1^{\land}Rx_2+x_1^{\land}t\tag{2} $$

通过式$(2)$ 可以求得 $s_2$的值，然后根据 $s_2$的值得到 $s_1$；

VINS如何进行三角化

在VINS中，同样根据两帧之间的匹配点（这里我们就称为第$I_1$帧和第$I_2$帧），我们通过对极约束得到对应相机运动，如何求解路标点的位置？

（即：已知相邻两帧相机的位姿以及特征点（相机归一化坐标）的匹配关系，求解特征点的世界坐标点。）

第$I_1$帧时，世界坐标系变换到相机坐标系下：

$$ \begin{align*} \tilde{P_c}&=(T_{cw}P_w)_{(1:3)}\\ \begin{bmatrix}x\\y\\z\end{bmatrix} &=\begin{bmatrix}R&t\end{bmatrix}_{3\times4} \begin{bmatrix}X\\Y\\Z\\1\end{bmatrix}_{4\times1}\\ \end{align*}\tag{3} $$

归一化坐标：

$$ \begin{align*} P_c&=(T_{cw}P_w)_{(1:3)}\\ \begin{bmatrix}x/z\\y/z\\1\end{bmatrix} &=a\begin{bmatrix}R&t\end{bmatrix}_{3\times4} [P_w]_{4\times1}\\ \end{align*}\tag{4} $$

由于变换矩阵是$3\times4$ 的矩阵，可以写成如下形式：

$$ \begin{align*} \begin{bmatrix}x/z\\y/z\\1\end{bmatrix} &=a\begin{bmatrix}P_0\\P_1\\P_2\end{bmatrix}_{3\times4}\cdot [P_w]_{4\times1}\\ &=a\begin{bmatrix}P_0P_w\\P_1P_w\\P_2P_w\end{bmatrix}_{3\times1} \end{align*}\tag{5} $$

我们知道，如果 $\vec{a}=k\vec{b}$，则向量$\vec{a}$ 与向量 $\vec{b}$ 平行，由于两个平行的向量的叉积为0，故而：

$$ \begin{align*} \begin{bmatrix}x/z\\y/z\\1\end{bmatrix} \times \begin{bmatrix}P_0P_w\\P_1P_w\\P_2P_w\end{bmatrix}_{3\times1}=0 \end{align*}\tag{6} $$

由叉积的性质：

$$ \begin{align*} \begin{vmatrix} i&j&k\\ x/z&y/z&1\\ P_0P_w&P_1P_w&P_2P_w \end{vmatrix} &\Rightarrow \left\{ \begin{array}{l} [y/z]P_2P_w-P_1P_w=0\\ P_0P_w-[x/z]P_2P_w=0 \\ [x/z]P_2P_w-[y/z]P_0P_w=0\ \ (恒成立) \end{array} \right. \\ &\Rightarrow \left\{ \begin{array}{l} [y/z]P_2P_w-P_1P_w=0\\ P_0P_w-[x/z]P_2P_w=0 \end{array} \right. \\ &\Rightarrow \begin{bmatrix} [y/z]P_2-P_1\\ P_0-[x/z]P_2 \end{bmatrix}P_w=0 \end{align*}\tag{7} $$

同样的对应的第$I_2$帧：

$$ \begin{bmatrix} [y^{\prime}/z^{\prime}]P^{\prime}_2-P^{\prime}_1\\ P^{\prime}_0-[x^{\prime}/z^{\prime}]P^{\prime}_2 \end{bmatrix}P_w=0\tag{8} $$

综合可得：

$$ \begin{bmatrix} [y/z]P_2-P_1\\ P_0-[x/z]P_2\\ [y^{\prime}/z^{\prime}]P^{\prime}_2-P^{\prime}_1\\ P^{\prime}_0-[x^{\prime}/z^{\prime}]P^{\prime}_2 \end{bmatrix}_{4\times4}{P_w}_{4\times1}=0\tag{9} $$

这样就转换成了 $AX=0$ 求解$X$ 的过程，可以使用SVD分解求解；

VINS-Mono三角化代码：

/**
 * @brief VINS三角化
 * 
 * @param Pose0 第一帧的位姿
 * @param Pose1 第二帧的位姿
 * @param point0 第一帧的相机归一化坐标
 * @param point1 第二帧的相机归一化坐标
 * @param point_3d 路标点的世界坐标
 */
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                        Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
    // 通过奇异值分解求解一个Ax = 0得到
    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);
}