VINS-Mono后端知识点汇总

processImage：每帧都干了什么

谁是Featureanager：维护路标点与图像

后端干了啥：详解因子图（视觉的因子图、IMU的因子图、因子图和Hessian矩阵的关系）

optimization：优化了谁？（参与到优化的约束有IMU的约束、重投影的约束、边缘化约束（先验约束））

IMU约束、重投影约束：谁长什么样？

麻烦的边缘化

0、后端预处理

        后端node拿到imu和图像后，做了两件事情：

• IMU预积分：processIMU()
• 将图像往后端优化：processImage()

 0.1 后端优化的关键类estimator

      estimator类在

问题：为什么使用逆深度？ 

1、processImage：对每帧都干了什么？

        

可以看到处理过程基本如下：

        1. f_manager.addFeatureCheckParallax(frame_count, image, td)

本函数有两个作用：①将当前帧看到的特征点image放到特征点管理器f_manager中。

② 同时根据视差进行关键帧的选择，假如当前帧是关键帧，则滑窗内最老的一帧被替换，假如当前帧不是关键帧，则替换滑窗内的次新帧。
        

        image表示当前帧跟踪到上一帧的特征点集合，也就是当前帧观测到的所有的路标点（不包括在当前帧新提取的点，因为当前帧新提取的点的cnt=1，无法完成三角化）。image的数据结构如下：

2. 滑窗优化 solveOdometry()滑窗优化

        对于滑窗优化部分，solveOdometry()函数主要做了两部分工作，一个是对新加入的没有三角化的点进行三角化，另一个是进行真正的滑窗优化optimization()。

2.1 f_manager.triangulate(Ps, tic, ric) 三角化

2.2 optimization()滑窗优化

        在弄清optimization函数如何进行优化时，按照BA优化三要素（误差项、优化变量、协方差矩阵）进行思考，首先明白优化向量是谁：

        优化向量：滑窗内的所有相机状态PVQB、所有3D点的逆深度（以及Camera到IMU的外参等）。

2.2.1 重投影误差：长什么模样

2.2.2 IMU约束：长什么模样

2.2.3 先验约束

        已加入完成视觉约束和IMU约束，剩下的先验约束如何加？

3. solveOdometry()滑窗优化中的边缘化操作

        最老帧需要扔掉M0（V/Ba/Bg），T0（P/Q），以及观测到的路标点也全部断开n×1(λ)，n是观测到的路标点的个数。

optimization()函数中的边缘化操作

3.1 边缘化结果

        在添加先验约束时，需要注意先看optimization()函数是如何添加先验约束的，代码在estimator.cpp中的optimization()函数中，代码如下：

// Step 4 边缘化
// 科普一下舒尔补
TicToc t_whole_marginalization;
if (marginalization_flag == MARGIN_OLD)
{
    // 一个用来边缘化操作的对象
    MarginalizationInfo *marginalization_info = new MarginalizationInfo();
    // 这里类似手写高斯牛顿，因此也需要都转成double数组
    vector2double();

    // 关于边缘化有几点注意的地方
    // 1、找到需要边缘化的参数块，这里是地图点，第0帧位姿，第0帧速度零偏
    // 2、找到构造高斯牛顿下降时跟这些待边缘化相关的参数块有关的残差约束，那就是预积分约束，重投影约束，以及上一次边缘化约束
    // 3、这些约束连接的参数块中，不需要被边缘化的参数块，就是被提供先验约束的部分，也就是滑窗中剩下的位姿和速度零偏

    // 上一次的边缘化结果
    if (last_marginalization_info)
    {
        vector<int> drop_set;
        // last_marginalization_parameter_blocks是上一次边缘化对哪些当前参数块有约束
        for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
        {
            // 涉及到的待边缘化的上一次边缘化留下来的当前参数块只有位姿和速度零偏
            if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
                last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
                drop_set.push_back(i);
        }
        // 处理方式和其他残差块相同
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set);

        marginalization_info->addResidualBlockInfo(residual_block_info);
    }
... ...
};

其中添加的last_marginalization的类型在Estimator类中有定义，定义如下：

MarginalizationInfo *last_marginalization_info;

MarginalizationInfo是vins_estimator/src/factor/marginalization.h文件中定义的类，是边缘化的大管家，代码如下：

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();
    int localSize(int size) const;
    int globalSize(int size) const;
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
    void preMarginalize();
    void marginalize();
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors;
    int m, n;
    std::unordered_map<long, int> parameter_block_size; //global size   // 地址->global size
    int sum_block_size;
    std::unordered_map<long, int> parameter_block_idx; //local size // 地址->参数排列的顺序idx
    std::unordered_map<long, double *> parameter_block_data;    // 地址->参数块实际内容的地址

    std::vector<int> keep_block_size; //global size
    std::vector<int> keep_block_idx;  //local size
    std::vector<double *> keep_block_data;

    Eigen::MatrixXd linearized_jacobians;
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;
};

其中的残差块添加函数addResidualBlockInfo函数中出现残差项因子ResidualBlockInfo，具体来看边缘化的残差因子定义：

        需要解释的是，待marg变量的序号就是滑窗内的编号，比如要边缘化最老帧关键帧，那么就是_drop_set就是0和1。

        回到MarginalizationInfo类，前面说过，这个类是边缘化操作的大管家，它主要包含以下三个函数：

void addResidualBlockInfo() // 将所有参与边缘化的因子加进来
void preMarginalize() // 做预处理
void marginalize() // 边缘化操作

五个变量：

parameter_block_size：每个变量的维度
parameter_block_data：每个变量的数据
parameter_block_idx：每个变量在H矩阵中的索引
m：需要marg掉的变量的总维度
n：需要保留的变量的总维度

3.1.1 5个变量

        单说关系或者数字，可能不太明白，下面举个例子来说，先搞明白5个变量是怎么来的：

 那么对应到上面MarginalizationInfo中的几个变量，关系如下：

 这三个参数涉及的维度也好，数据也好，索引也好，全部是围绕这52个优化变量展开的。计算以下所有变量的Local维度总和，即为H矩阵的维度：

2(M0(V,Ba,Bg)，M1)×3×3 + 11(P(P/Q))×2×3 + 1(Tbc)×6 + 42(λ)×1 = 132，

H矩阵的大小就为132×132。

         那么MarginalizationInfo中的m和n也就不难计算：

        进一步，parameter_block_size、parameter_block_data和parameter_block_idx
的具体内容如下：

前边44个优化变量，维度是57，需要marg掉。

后边12个优化变量，维度是75，需要保存。

3.1.2 3个函数之addResidualBlockInfo()

再说回3个函数：

1. 首先来看添加视觉因子（残差）的函数：

 

        这里就会明白添加的{0，3}是什么意思，代表[Ti，Tj，Tbc，λ]数组中的第0个Ti和第3个λ。

2. 再添加IMU因子：

3.1.3 3个函数之preMarginalize()

        

 

 

3.1.4 3个函数之marginalize()

        

 

 

3.1.2 IMU因子

        IMU因子优化变量：第0帧、第1帧的PVQB。

// 跟构建ceres约束问题一样，这里也需要得到残差和雅克比    IMU因子：第0帧、第1帧的PVQB
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL, vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]}, vector<int>{0, 1});
// 这里就是第0和1个参数块是需要被边缘化的
marginalization_info->addResidualBlockInfo(residual_block_info);

3.1.3 视觉因子

        优化变量：第0帧和共视帧的PQ、外参、逆深度。

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
                                   vector<double *>{para_Pose[imu_i], para_Pose[imu_j], 
                                   para_Ex_Pose[0], para_Feature[feature_index]},
                                   vector<int>{0, 3});  // 这里第0帧和地图点被margin

marginalization_info->addResidualBlockInfo(residual_block_info);

4. 根据删除最老帧还是次新帧，对滑窗进行更新：slideWindow()