VINS FUNSION Optimization

Optimization位于VINS_FUNSION的estimator模块，具体是根据外参初始值，进行全局优化。

Optimization代码逻辑参考下图：

优化参数：

包括了滑动窗口内的所有相机状态、相机到IMU的外参、和3D点的逆深度。

目标函数：

其中，IMU测量残差：

视觉测量残差：

边缘化的先验信息：（没看懂）

 

 

代码详解：

/**
 * 滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
*/
void Estimator::optimization()
{
    TicToc t_whole, t_prepare;
    // 滑窗中的帧位姿、速度、偏置、外参、特征点逆深度等参数，转换成数组
    vector2double();

    ceres::Problem problem;
    ceres::LossFunction *loss_function;
    //loss_function = NULL;
    loss_function = new ceres::HuberLoss(1.0);
    //loss_function = new ceres::CauchyLoss(1.0 / FOCAL_LENGTH);
    //ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
    
    /**
     * Step1. 调用AddParameterBlock，显式添加待优化变量（类似于g2o中添加顶点），需要固定的顶点固定一下
    */

    // 遍历滑窗，添加位姿、速度、偏置参数
    for (int i = 0; i < frame_count + 1; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        if(USE_IMU)
            problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }
    // 如果不使用IMU，固定第一帧位姿，IMU下第一帧不固定
    if(!USE_IMU)
        problem.SetParameterBlockConstant(para_Pose[0]);

    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        // 添加相机与IMU外参
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        // 估计外参
        if ((ESTIMATE_EXTRINSIC && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) || openExEstimation)
        {
            //ROS_INFO("estimate extinsic param");
            openExEstimation = 1;
        }
        else
        {
            //ROS_INFO("fix extinsic param");
            // 不估计外参的时候，固定外参
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);
        }
    }
    // 添加相机与IMU时差
    problem.AddParameterBlock(para_Td[0], 1);

    if (!ESTIMATE_TD || Vs[0].norm() < 0.2)
        problem.SetParameterBlockConstant(para_Td[0]);

    /**
     * Step2. 调用AddResidualBlock，添加各种残差数据（类似于g2o中的边）
    */

    // (1) 添加先验残差，通过Marg的舒尔补操作，将被Marg部分的信息叠加到了保留变量的信息上
    if (last_marginalization_info && last_marginalization_info->valid)
    {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }
    // (2) 添加IMU残差
    if(USE_IMU)
    {
        for (int i = 0; i < frame_count; i++)
        {
            int j = i + 1;
            if (pre_integrations[j]->sum_dt > 10.0)
                continue;
            // 前后帧之间建立IMU残差
            IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
            // 后面四个参数为变量初始值，优化过程中会更新
            problem.AddResidualBlock(imu_factor, NULL, para_Pose