VINS-Mono+Fusion源码解析系列（十七）：非线性优化

1. solveOdometry

（1）大致思路

​ 按照后端流程，VIO初始化initialStructure完成之后，执行非线性优化solveOdometry。大致步骤如下：

• 在保证滑窗中帧数已满的前提下，若已完成初始化，再一次执行三角化操作f_manager.triangulate。将滑窗中的特征点尽可能多地恢复出对应的3D点，获取多帧之间更多的约束，进而得到更多的优化观测量, 使得优化结果更加鲁棒(实现过程与初始化中的一样)。
• 执行非线性优化主体函数optimization。

（2）代码实现

void Estimator::solveOdometry()
{
   
    // 保证滑窗中帧数满了
    // 与前面初始化阶段的判断重复, 因为滑窗未填满, 初始化就没有完成, 自然就无法进行后端的非线性优化阶段
    if (frame_count < WINDOW_SIZE)
        return;
    // 其次要求初始化完成
    // 同样是重复判断, 因为在进入当前的solveOdometry()之前, solver_flag就已经是NON_LINEAR
    if (solver_flag == NON_LINEAR)
    {
   
        TicToc t_tri;
        // 先把应该三角化但是没有三角化的特征点三角化, 获取多帧之间更多的约束
        // 进而获取更多的优化观测量, 使得优化结果更加鲁棒  (这一步在前面初始化中已完成)
        f_manager.triangulate(Ps, tic, ric);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());
        optimization();  // 进行优化
    }
}

2. 基于ceres的非线性优化特点

	ceres自动求导	ceres解析求导
残差、雅可比的计算方式	只需要定义残差计算方式	既需要定义残差的计算方式，又需要定义残差对各个待优化参数的雅可比矩阵
costFunction的继承方式	AutoDiffCostFunction	SizedConstFunction
如何提供残差（雅可比）	自定义一个重载了括号运算符的模板函数，用来提供残差的计算方式	重载Evaluate函数来计算残差和雅可比

3. 核心优化函数optimization

（1）大致流程

• 定义ceres的problem以及核函数loss_function
• 将待优化的参数块添加到problem中，主要有如下参数块：
  • 滑窗中每一帧的位姿、速度以及bias，其中需要自定义姿态的更新方法PoseLocalParameterization。
  • 相机与IMU之间的外参，其中根据ESTIMATE_EXTRINSIC来决定是否需要将外参fix住。
  • 传感器时延参数。
  • 还需要添加地图点的逆深度参数，由于地图点数量较多，频繁添加会导致耗时增加。因此，在将视觉重投影约束添加problem中时会添加逆深度。
• 将待优化参数量由eigen类型转为double类型，便于进行ceres-solver优化。
• 将约束添加到problem中，主要有如下约束：
  • 边缘化约束。
  • IMU预积分约束。
  • 视觉重投影约束。
  • 回环检测约束。
• 执行ceres-solver优化，并将优化后的参数量由double类型转为eigen类型。

（2）代码实现

/**
 * @brief 进行非线性优化
 * 
 */
void Estimator::optimization()
{
   
    // 借助ceres进行非线性优化
    // 由于初始化只进行一次, 因此可使用自动求导; 而在后端优化中, 由于实时性的要求, 通常使用解析求导
    ceres::Problem problem;
    // 定义核函数, 目的是用于剔除outlier, 降低outlier的点的权重
    ceres::LossFunction *loss_function;
    loss_function = new ceres::CauchyLoss(1.0);
    // Step 1 定义待优化的参数块，类似g2o的顶点
    // 参数块 1： 滑窗中的位姿、速度和bias，共11帧
    for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
   
        // 由于姿态不满足正常的加法，也就是李群上没有加法，因此需要自己定义它的加法
        // 具体是在PoseLocalParameterization中的子类local_parameterization中的plus里面实现的
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        // 将滑窗中所有参数块添加到problem中
        // 参数1：所添加的参数块  参数2：所添加的参数块大小  参数3：参数块中自增更新的方式
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }
    // 参数块 2： 相机与imu之间的外参
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
   
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        // 外参para_Ex_Pose[i]同样通过local_parameterization来更新，并加入到problem中参与优化
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        if (!ESTIMATE_EXTRINSIC)
        {
   
            ROS_DEBUG("fix extinsic param");
            // 如果不需要优化外参就将外参设置为fix，不参与优化
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }
    // 传感器的时间同步
    if (ESTIMATE_TD)
    {
   
        // 传感器的时间优化符合广义加法，通过ceres自带的更新方法添加到problem中
        problem.AddParameterBlock(para_Td[0], 1);
    }
    // 实际上还有地图点，其实频繁的(不需要自定义加法，只用广义加法的)参数块不需要调用AddParameterBlock
    // 在后面增加残差块接口时(AddResidualBlock)会添加所要优化的参数块，然后在costFunction中会添加对应参数块的大小
    TicToc t_whole, t_prepare;
    // eigen -> double  便于ceres solver求解, 求解完之后还需要转换回eigen
    vector2double();
    
    /**************** 边缘化 ***************/
    // 添加边缘化约束: 上一次的边缘化结果作为这一次的先验约束
    if (last_marginalization_info)
    {
   
        // 创建边缘化约束
        MarginalizationFactor *marginalization_factor = new 
            							MarginalizationFactor(last_marginalization_info);
        // 将边缘化约束的cost Function(marginalization_factor)添加到优化问题problem中
        // 参数1: costFunction    参数2: 核函数(NULL)    参数3: 上一次边缘化的约束
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }
    
    // 将imu预积分的约束添加到problem中参与优化
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
   
        int j = i + 1;
        // 时间过长这个预积分约束就不可信了  若预积分的累积积分的时间跨度超过10s，则不能形成约束
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        // 预积分类IMUFactor中实现的是残差，残差对待优化变量的雅可比以及对应的协方差矩阵
        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
        // costFunction ———— 相应的参数块
        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;
    // 将视觉重投影约束添加到problem中参与优化
    for (auto &it_per_id : f_manager.feature)
    {
   
        // 当前特征点在滑窗中被看到的帧数
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        // 进行特征点有效性的检查
        // 当前特征点至少被滑窗中的2帧所看到，且被看到的初始帧不能是倒数第3帧
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
 
        ++feature_index;
        // 第一个观测到这个特征点的帧idx  逆深度是与这一帧所绑定的
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
        // 特征点在第一个帧下的归一化相机系坐标
        Vector3d pts_i = it_per_id.feature_per_frame[0].point;
        // 遍历看到这个特征点的所有KF
        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
   
            imu_j++;  // 此前在imu_i基础上倒退1  
            if (imu_i == imu_j) // 自己跟自己不能形成重投影
            {
   
                continue;
            }
            // 取出另一帧的归一化相机坐标
            Vector3d pts_j = it_per_frame.point;
            // 带有时间延时的是另一种形式
            if (ESTIMATE_TD)
            {
   
				// 特征点在起始帧下的归一化点pts_i，在当前帧下的归一化点pts_j，
                // 特征点在起始帧下的速度，特征点在当前帧下的速度
                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