VINS-mono 学习之 三角化

参考：https://blog.csdn.net/u013517182/article/details/52151960

https://blog.csdn.net/kokerf/article/details/72844455

感谢前辈们的分享，对我理解VINS代码有很大的帮助。

三角化（Triangulate）是计算机视觉中的重要的一部分，通过特征匹配的二维坐标点和图像的位姿（R，t）得到三维空间点的位置。

放一张示意图:

根据几何关系，假设两帧图像上匹配特征点的齐次坐标为x, x'。三维空间点齐次坐标记为X。两帧图像对应的投影矩阵为P，P’。可以得到：

x = P X         x' = P' X

使用叉乘得到AX = 0的形式（可以使用SVD的方法计算）：

x×(P X) = 0          x'×(P' X) = 0

其中对x = (x, y, z)分析，将P和P’按行拆开，得到：

由此可以得到三个方程，由于第三个方程可以由前两个方程得到，因此只需要考虑前两个方程。每对匹配的特征（x和x'）都会得到四个方程，表示为AX =0 的形式，

通常由于匹配的特征点对较多，这是一个超定方程，一般无法求得精确解，我们其实需要求一个使AX最小的解。通过对A的SVD分解，最后得到的是A的最小奇异值对应的单位奇异矢量，若X = (x, y, z, w)，深度则为z/w。

在VINS-mono中，对滑动窗口中所有帧中depth未知的特征点进行三角化。对应于代码如下：

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;

        if (it_per_id.estimated_depth > 0)
            continue;
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        ROS_ASSERT(NUM_OF_CAM == 1);
        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        Eigen::Matrix P0;
        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
        P0.leftCols<3>() = Eigen::Matrix3d::Identity();
        P0.rightCols<1>() = Eigen::Vector3d::Zero();

        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;

            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix P;
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            Eigen::Vector3d f = it_per_frame.point.normalized();
            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);

            if (imu_i == imu_j)
                continue;
        }
        ROS_ASSERT(svd_idx == svd_A.rows());
        Eigen::Vector4d svd_V = Eigen::JacobiSVD(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
        double svd_method = svd_V[2] / svd_V[3];
        //it_per_id->estimated_depth = -b / A;
        //it_per_id->estimated_depth = svd_V[2] / svd_V[3];

        it_per_id.estimated_depth = svd_method;
        //it_per_id->estimated_depth = INIT_DEPTH;
        //ROS_INFO("before <> feature id %d , start_frame %d, used num %d, depth %f ", it_per_id.feature_id, it_per_id.start_frame, it_per_id.used_num,it_per_id.estimated_depth);  //zss


        if (it_per_id.estimated_depth < 0.1)
        {
            it_per_id.estimated_depth = INIT_DEPTH;
        }

    }
}