VINS-Mono源码解读(五):后端优化

优化的内容包括三部分,分别是特征点深度、IMU误差、关键帧姿态。所以优化的残差函数为

具体解释为:

1)marginalization residual 为边缘化计算得到的先验误差

2)IMU residual 是运动模型的误差, 每相邻的两帧IMU之间产生一个residual.

3)visual residual 是视觉的误差, 单个特征点l在相机cj下的投影会产生一个residual.

明白了这一点,就完成了后端优化理论部分的理解,下面就是代码部分。对应以上这三点,系统中相关的文件如下:

  1. marginalization residual 包含的文件为

1)vins_estimator/src/factor/marginalization_factor.cpp:边缘化的具体实现

2)vins_estimator/src/estimator.cpp(部分):

  • 函数optimization负责利用边缘化残差构建优化模型
  • 函数slideWindow负责维护滑动窗口

2. IMU residual 包含的文件为

1)VINS-Mono/vins_estimator/src/factor/imu_factor.h

2)VINS-Mono/vins_estimator/src/factor/integration_base.h

3)vins_estimator/src/estimator.cpp(部分)

3. visual residual包含的文件为

1)VINS-Mono/vins_estimator/src/factor/projection_factor.cpp

2)VINS-Mono/vins_estimator/src/factor/projection_td_factor.cpp

3)vins_estimator/src/estimator.cpp(部分)

由于marginalization residual 已经在边缘化那一节讲过,IMU residual 在预积分那一节讲过,visual residual相关的视觉残差内容和以前介绍ORB SLAM2时候的内容大同小异,所以我们在此只介绍vins_estimator/src/estimator.cpp中关于优化流程的代码,确切的说是optimization这个函数。我们分段介绍

1. 初始化ceres

ceres::Problem problem;
    ceres::LossFunction *loss_function;
    loss_function = new ceres::CauchyLoss(1.0);

2. 加入待优化参数项

先添加优化参数量, ceres中参数用ParameterBlock来表示, 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维).

for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }

    //ESTIMATE_EXTRINSIC!=0则camera到IMU的外参也添加到估计
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        if (!ESTIMATE_EXTRINSIC)
        {
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }
    //相机和IMU硬件不同步时估计两者的时间偏差
    if (ESTIMATE_TD)
    {
        problem.AddParameterBlock(para_Td[0], 1);
        //problem.SetParameterBlockConstant(para_Td[0]);
    }

3. 加入误差项

依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor

//添加边缘化残差
    if (last_marginalization_info)
    {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

    //添加IMU残差
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        int j = i + 1;
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }
    int f_m_cnt = 0;
    int feature_index = -1;

    //添加视觉残差
    for (auto &it_per_id : f_manager.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;
 
        ++feature_index;

        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
        
        Vector3d pts_i = it_per_id.feature_per_frame[0].point;

        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;
            if (imu_i == imu_j)
            {
                continue;
            }
            Vector3d pts_j = it_per_frame.point;
            if (ESTIMATE_TD)
            {
                    ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                     it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                                     it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                    problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
                    /*
                    double **para = new double *[5];
                    para[0] = para_Pose[imu_i];
                    para[1] = para_Pose[imu_j];
                    para[2] = para_Ex_Pose[0];
                    para[3] = para_Feature[feature_index];
                    para[4] = para_Td[0];
                    f_td->check(para);
                    */
            }
            else
            {
                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
            }
            f_m_cnt++;
        }
    }

4. ceres求解

其实在3和4之间还有闭环的factor,闭环以后再讲,所以这里就先不提了。

ceres::Solver::Options options;

options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
if (marginalization_flag == MARGIN_OLD)
    options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
    options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);