VINS-MONO代码解读---基于滑动窗口的紧耦合后端非线性优化 optimization()+滑动窗slideWindow()

本讲是VINS最核心部分了，前面经历了

1）视觉跟踪feature_tracker、IMU预积分integrationBase类；

2）初始化中SFM纯视觉估计滑动窗中所有帧的位姿和3D路标点深度、SFM与IMU预积分松耦合对齐求解初始化参数。

在完成前面的初始化环节后，本节将进行第3部分，基于滑动窗的紧耦合后端非线性优化：将视觉约束、IMU约束、闭环约束放到一个大的目标函数中进行非线性优化，求解出滑动窗口中所有帧的PVQ、bias等。

在视觉约束和IMU约束中，基本思想是找到优化状态向量，然后通过视觉残差和IMU测量残差分别对状态向量求导，获得视觉和IMU预积分的Jacobian和协方差矩阵。

本文将对Estimator::optimization()函数的代码进行详细讲解。

目录

• 一、VIO残差函数构建

  1、需要优化的状态向量

  2、目标函数

• 二、Estimator::Optimization（）函数

  1、添加ceres参数块,待优化变量 AddParameterBlock

  1、初始化ceres并将 (p,v,q,ba,bg) 15自由度优化变量加入
  
  2、Cam到IMU的外参估计加入
  
  3、滑窗内第一时刻相机到IMU时钟差加入
  
  4、在ceres中，vevtor转double类型
  

  2、添加残差 AddResidualBlock

  1、添加边缘化残差，丢弃帧的约束 MarginalizationFactor
  
  2、添加IMU残差 IMUFactor
  
  3、添加视觉残差 ProjectionTdFactor
  
  4、添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备
  

  3、ceres求解Solver

  4、边缘化处理

  前言：MarginalizationInfo类

  4.1、边缘化最老帧

  1、将上一次先验残差项MarginalizationFactor传递给marginalization_info
  
  2、将第0帧和第1帧间的预积分观测IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中
  
  3、将第一次观测为第0帧的所有路标点对应的视觉观测ProjectionFactor，添加到marginalization_info中
  
  4、preMarginalize()函数：计算每个残差对应的Jacobian，
  
  5、marglinalize()函数：多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差
  
  6、调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座
  

  4.2、边缘化次新帧

  1、保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
  
  2、preMarginalize():计算每个残差对应的Jacobian
  
  3、marginalize():多线程构造先验项舒尔补AX=b
  
  4、调整参数块在下一次窗口中对应的位置（去掉次新帧）
  

• 三、滑动窗 slideWindow()

  1、 MARGIN_OLD 边缘化最老帧

  1、保存最老帧信息到back_R0,back_P0
  
  2、滑窗内0-5所有信息前移
  
  3、第5帧情况更新
  
  4、最老帧之前预积分和图像都删除
  
  5、slideWindowOld()函数，首次在原来最老帧出现的特征点转移到现在现在最老帧
  

  2、MARGIN_SECOND_NEW 边缘化次新帧，但是不删除IMU约束

  1、第5帧图像信息复制到第4帧上
  
  2、更新第5帧
  
  3、当最新一帧(5)不是关键帧时，用于merge滑窗内最新帧(4)
  

一、VIO残差函数构建

1、需要优化的状态向量

滑动窗口内IMU状态（PVQ、加速度bias、陀螺仪bias）、IMU到Camera的外参、m+1个3D路标点逆深度。

第一个式子是滑动窗口内所有状态量，n是关键帧数量，m是滑动窗内所有观测到的路标点总数。λi是第i个特征点的第一个观测对应的逆深度.特征点逆深度为了满足高斯系统。

第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。

第三个式子是相机外参。

注意：xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；

维度是15n+6+m*

IMU状态 xk {P、V、Q、Ba、Bg} 是15维（5*3=15）；相机外参 为 6 维； 特征点逆深度为1维。

2、目标函数

优化函数有三部分：

边缘化残差：从滑动窗中去掉的节点和特征点构成的约束，形成一个先验Prior
IMU残差：相邻两帧IMU产生的residual
视觉重投影误差：单个特征点在两帧之间投影形成residual

二、Estimator::Optimization（）函数

整个滑动窗优化在optimization函数中求解。基于滑动窗口紧耦合的非线性优化，残差项的构造和求解

1.添加要优化的变量 (p,v,q,ba,bg) 一共15个自由度，Cam到IMU外参，时钟差

2.添加残差，残差项分为4块 先验残差+IMU残差+视觉残差+闭环检测残差

3.ceres求解Solve（）函数

4.边缘化操作

1、添加ceres参数块,待优化变量 AddParameterBlock

待优化变量分为三部分：IMU状态xk (p,v,q,ba,bg)、Cam到IMU外参、时钟差

1、初始化ceres并将 (p,v,q,ba,bg) 15自由度优化变量加入
创建一个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); // 柯西核函数

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

Ps、Rs转变成para_Pose 6自由度7DOF（x,y,z,qx,qy,qz,w），Vs、Bas、Bgs转变成para_SpeedBias 9自由度9DOF(vx,vy,vz,bax,bay,baz,bgx,bgy,bgz)

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);
    }

2、Cam到IMU的外参估计加入
camera到IMU的外参也添加到估计 para_Ex_Pose

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");
    }

3、滑窗内第一时刻相机到IMU时钟差加入
滑窗内第一个时刻相机到IMU时钟差，保证传感器同步， para_Td

if (ESTIMATE_TD)
    {
   
        problem.AddParameterBlock(para_Td[0], 1);
        //problem.SetParameterBlockConstant(para_Td[0]);
    }

4、在ceres中，vevtor转double类型

- 从Ps、Rs、Vs、Bas、Bgs转化为para_Pose（6维，相机位姿）和para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）
- 从tic和q转化为para_Ex_Pose （6维，Cam到IMU外参）
- 从dep到para_Feature（1维，特征点深度）
- 从td转化为para_Td（1维，标定同步时间）

void Estimator::vector2double()
{
   
    // 1.遍历滑动窗，IMU的15个自由度的优化变量
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
   
        // P、R
        para_Pose[i][0] = Ps[i].x();
        para_Pose[i][1] = Ps[i].y();
        para_Pose[i][2] = Ps[i].z();
        Quaterniond q{
   Rs[i]};
        para_Pose[i][3] = q.x();
        para_Pose[i][4] = q.y();
        para_Pose[i][5] = q.z();
        para_Pose[i][6] = q.w();
        // V、Ba、Bg
        para_SpeedBias[i][0] = Vs[i].x();
        para_SpeedBias[i][1] = Vs[i].y();
        para_SpeedBias[i][2] = Vs[i].z();
 
        para_SpeedBias[i][3] = Bas[i].x();
        para_SpeedBias[i][4] = Bas[i].y();
        para_SpeedBias[i][5] = Bas[i].z();
 
        para_SpeedBias[i][6] = Bgs[i].x();
        para_SpeedBias[i][7] = Bgs[i].y();
        para_SpeedBias[i][8] = Bgs[i].z();
    }
    // 2.Cam到IMU的外参，6自由度由7个参数表示
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
   
        para_Ex_Pose[i][0] = tic[i].x();
        para_Ex_Pose[i][1] = tic[i].y();
        para_Ex_Pose[i][2] = tic[i].z();
        Quaterniond q{
   ric[i]};
        para_Ex_Pose[i][3] = q.x();
        para_Ex_Pose[i][4] = q.y();
        para_Ex_Pose[i][5] = q.z();
        para_Ex_Pose[i][6] = q.w();
    }
    
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
        
    // 3、保证传感器同步的时钟差
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

2、添加残差 AddResidualBlock

依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类。首先会调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入上面所述的优化变量中

1、添加边缘化残差，丢弃帧的约束 MarginalizationFactor

    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);
    }

2、添加IMU残差 IMUFactor

    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;

3、添加视觉残差 ProjectionTdFactor

    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)