VINS-Fusion源码逐行解析：三角化函数triangulate()和triangulatePoint()

我们再来看一下三角化计算点深度的函数，三角化函数都是老朋友了，大家都看过很多遍了，看完这个明天开始看优化函数

该函数使用两种方法来计算特征点深度，第一种是双目或多帧三角化，第二种是svd分解来计算深度第二个方法的目的是提供一种冗余机制，以确保即使第一个方法未能提供可靠的深度估计，也能通过SVD提供一个合理的估计值。SVD方法通过最小化重投影误差来估计深度，具有较强的鲁棒性，特别是在噪声存在的情况下。

我们来看一下源码：

//使用两种方法来计算特征点深度，第一种是双目或多帧三角化
//第二种是svd分解来计算深度第二个方法的目的是提供一种冗余机制，以确保即使第一个方法未能提供可靠的深度估计，也能通过SVD提供一个合理的估计值。
//SVD方法通过最小化重投影误差来估计深度，具有较强的鲁棒性，特别是在噪声存在的情况下
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
{
    // 遍历所有特征点
    for (auto &it_per_id : feature)
    {
        // 如果特征点的深度已经估计过了，则跳过
        if (it_per_id.estimated_depth > 0)
            continue;

        // 如果是立体相机且该特征点有立体匹配信息
        if(STEREO && it_per_id.feature_per_frame[0].is_stereo)
        {
            int imu_i = it_per_id.start_frame;
            // 构建左相机的投影矩阵
            Eigen::Matrix<double, 3, 4> leftPose;
            //计算相机的平移向量，通过将 IMU 的平移量转换到相机坐标系中得到相机的平移向量。
            Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
            //计算相机的旋转矩阵，通过将 IMU 的旋转矩阵转换到相机坐标系中得到相机的旋转矩阵。
            Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
            leftPose.leftCols<3>() = R0.transpose();
            leftPose.rightCols<1>() = -R0.transpose() * t0;
            //cout << "left pose " << leftPose << endl;

            // 构建右相机的投影矩阵
            Eigen::Matrix<double, 3, 4> rightPose;
            Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[1];
            Eigen::Matrix3d R1 = Rs[imu_i] * ric[1];
            rightPose.leftCols<3>() = R1.transpose();
            rightPose.rightCols<1>() = -R1.transpose() * t1;
            //cout << "right pose " << rightPose << endl;

            // 获取左、右相机的特征点坐标
            Eigen::Vector2d point0, point1;
            Eigen::Vector3d point3d;
            point0 = it_per_id.feature_per_frame[0].point.head(2);
            point1 = it_per_id.feature_per_frame[0].pointRight.head(2);
            //cout << "point0 " << point0.transpose() << endl;
            //cout << "point1 " << point1.transpose() << endl;

            // 三角化计算特征点的3D坐标
            triangulatePoint(leftPose, rightPose, point0, point1, point3d);
            Eigen::Vector3d localPoint;
            localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();
            double depth = localPoint.z();
            // 判断深度是否有效并进行更新
            if (depth > 0)
                it_per_id.estimated_depth = depth;
            else
                it_per_id.estimated_depth = INIT_DEPTH;
            /*
            Vector3d ptsGt = pts_gt[it_per_id.feature_id];
            printf("stereo %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
                                                            ptsGt.x(), ptsGt.y(), ptsGt.z());
            */
            continue;
        }
        // 如果特征点在多帧中被观测到
        else if(it_per_id.feature_per_frame.size() > 1)
        {
            int imu_i = it_per_id.start_frame;
            // 构建初始帧的投影矩阵
            Eigen::Matrix<double, 3, 4> leftPose;
            Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
            Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
            leftPose.leftCols<3>() = R0.transpose();
            leftPose.rightCols<1>() = -R0.transpose() * t0;

            imu_i++;
            // 构建下一帧的投影矩阵
            Eigen::Matrix<double, 3, 4> rightPose;
            Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_i] * ric[0];
            rightPose.leftCols<3>() = R1.transpose();
            rightPose.rightCols<1>() = -R1.transpose() * t1;

            // 构建下一帧的投影矩阵
            Eigen::Vector2d point0, point1;
            Eigen::Vector3d point3d;
            point0 = it_per_id.feature_per_frame[0].point.head(2);
            point1 = it_per_id.feature_per_frame[1].point.head(2);
            // 三角化计算特征点的3D坐标
            triangulatePoint(leftPose, rightPose, point0, point1, point3d);
            Eigen::Vector3d localPoint;
            localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();
            double depth = localPoint.z();
            // 判断深度是否有效并进行更新
            if (depth > 0)
                it_per_id.estimated_depth = depth;
            else
                it_per_id.estimated_depth = INIT_DEPTH;
            /*
            Vector3d ptsGt = pts_gt[it_per_id.feature_id];
            printf("motion  %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
                                                            ptsGt.x(), ptsGt.y(), ptsGt.z());
            */
            continue;
        }
        // 更新特征点在帧中的观测数量
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        //只有在至少有4个观测（帧）中出现的特征点才会被处理。
        if (it_per_id.used_num < 4)
            continue;

        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        // 构建SVD矩阵
        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        // 构建初始帧的投影矩阵
        Eigen::Matrix<double, 3, 4> 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<double, 3, 4> P;
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            // 归一化特征点
            Eigen::Vector3d f = it_per_frame.point.normalized();
            // 构建SVD矩阵
            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;
        }
        // 断言SVD矩阵的行数正确
        ROS_ASSERT(svd_idx == svd_A.rows());
        // 进行SVD分解
        Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(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;

        // 如果深度小于阈值，使用初始深度
        if (it_per_id.estimated_depth < 0.1)
        {
            it_per_id.estimated_depth = INIT_DEPTH;
        }

    }
}

在其内部triangulatePoint()函数，很简单：利用两个投影矩阵（Pose0 和 Pose1）以及在这两个视角下观测到的特征点（point0 和 point1）来三角化得到3D点 point_3d，直接看源码即可

//这个函数利用两个投影矩阵（Pose0 和 Pose1）以及在这两个视角下观测到的特征点（point0 和 point1）来三角化得到3D点 point_3d
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)
{
     创建一个 4x4 的矩阵，并初始化为全零
    Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero();
    // 填充设计矩阵的每一行
    // 第一行：point0[0] * Pose0 第三行 - Pose0 第一行
    design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
    // 第二行：point0[1] * Pose0 第三行 - Pose0 第二行
    design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
    // 第三行：point1[0] * Pose1 第三行 - Pose1 第一行
    design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
    // 第四行：point1[1] * Pose1 第三行 - Pose1 第二行
    design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
    // 创建一个 4 维向量来存储三角化后的点
    Eigen::Vector4d triangulated_point;
    // 对矩阵进行 SVD 分解，提取 V 矩阵的最后一列
    triangulated_point =
              design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
    // 将齐次坐标转换为 3D 点坐标
    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);
}