SLAM--三角化测量

单目相机中2d特征点大多是通过三角测量求解出地图点的世界坐标

VINS中的三角测量

1. 推导

3D点齐次坐标$[x,y,z,1{]}^T$，3D点在图像中的投影：
$$\lambda \left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\underset{\mathbf{P}}{\underbrace{\mathbf{K}[\mathbf{R}|\mathbf{t}]}}\left[\begin{array}{l}x\\ y\\ z\\ 1\end{array}\right]$$
$$\lambda \mathbf{u}=\mathbf{P}\mathbf{X}$$
两边同时叉乘u，则有：
$${\mathbf{u}}^{\wedge }\mathbf{P}\mathbf{X}=0$$
展开得：
$$\left[\begin{array}{ccc}0& -1& v\\ 1& 0& -u\\ -v& u& 0\end{array}\right]\left[\begin{array}{l}{\mathbf{P}}_1\\ {\mathbf{P}}_2\\ {\mathbf{P}}_3\end{array}\right]\mathbf{X}=0$$
$$\{\begin{array}{l}\left(v{\mathbf{P}}_3-{\mathbf{P}}_2\right)\mathbf{X}=0\\ \left({\mathbf{P}}_1-u{\mathbf{P}}_3\right)\mathbf{X}=0\\ \left(u{\mathbf{P}}_2-v{\mathbf{P}}_1\right)\mathbf{X}=0\end{array}$$
由于（1）式和（2）式可以得到（3）式，故一帧图像可以得到2个独立线性方程，则两帧可以形成四个方程：
$$\underset{\mathbf{P}}{\underbrace{\left[\begin{array}{l}{v}_1{\mathbf{P}}_3^1-{\mathbf{P}}_2^1\\ u_1{\mathbf{P}}_3^1-{\mathbf{P}}_1^1\\ v_2{\mathbf{P}}_3^2-{\mathbf{P}}_2^2\\ u_2{\mathbf{P}}_3^2-{\mathbf{P}}_1^2\end{array}\right]}}\mathbf{X}=0$$

• 这里可以使用SVD求解，齐次坐标X即为H的最小奇异值的奇异向量。

2. 代码

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);
}