VINS-Mono代码学习记录（九）--processImage()(滑窗优化边缘化部分）

上一节结束了初始化的内容，根据这张流程图：
接下来继续学习其中的solveOdometry( )函数，这一块就开始优化啦，VINS-Mono采用的是ceres来进行优化的。
在solveOdometry( )函数中，首先需要了解的是三角化triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])，这一部分需要先看理论知识，不然就会一头雾水，在这里，直接贴上我查找的对于三角化的比较好理解的解释：
参考链接： https://blog.csdn.net/qq_37611824/article/details/93210012
参考链接2： https://blog.csdn.net/u012101603/article/details/79714332

代码部分为：

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<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();//(u,v,1)坐标
            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);//构建svd_A每帧的第一个约束
            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);//构建svd_A每帧的第二个约束

            if (imu_i == imu_j)
                continue;
        }
        ROS_ASSERT(svd_idx == svd_A.rows());
        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;
        }

    }
}

看到一个对于理论推导的介绍，
参考链接： https://blog.csdn.net/qq_41839222/article/details/93593844
参考链接：https://blog.csdn.net/q597967420/article/details/76099443
optimization()中部分注释如下：

void Estimator::optimization()
{
    //[1]创建一个ceres Problem实例, loss_function定义为CauchyLoss.
    ceres::Problem problem;
    ceres::LossFunction *loss_function;
    //loss_function = new ceres::HuberLoss(1.0);
    loss_function = new ceres::CauchyLoss(1.0);

    //[2]添加优化参数量, ceres中参数用ParameterBlock