VINS-Mono 代码解析六、边缘化

一、理论部分

理论部分就不详细说了，看这个连接： https://blog.csdn.net/hltt3838/article/details/109455065

 

简单介绍

 1、优化变量：

para_Pose             （6维，相机位姿）、
para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）、
para_Ex_Pose     （6维、相机IMU外参）、
para_Feature      （1维，特征点深度）、
para_Td                 （1维，标定同步时间）
五部分组成，在后面进行边缘化操作时这些优化变量都是当做整体看待

last_marginalization_parameter_blocks ：Xb变量，也是我们要优化的变量；
last_marginalization_info                                ：是Xb对应的测量Zb，也是先验残差（细品一下）

 

2、思路：
 

1.当次新帧为关键帧时,MARGIN_OLD,将 marg 掉最老帧,及其看到的路标点和相关联的 IMU 数据,

将其转化为先验信息加到整体的目标函数中:
1) 把上一次先验项中的残差项(尺寸为 n)传递给当前先验项,并从中去除需要丢弃的状态量;
2) 将 滑 窗 内 第 0 帧 和 第 1 帧 间 的 IMU 预 积 分 因 子 ( pre_integrations[1] ) 放 到marginalization_info 中,

即图 中上半部分中 x 0 和 x 1 之间的表示 IMU 约束的黄色块;
3) 挑选出第一次观测帧为第 0 帧的路标点,将对应的多组视觉观测放到 marginalization_info中,

即图 11 中上半部分中 x 0 所看到的红色五角星的路标点;
4) marginalization_info->preMarginalize():得到每次 IMU 和视觉观测(cost_function)对应的参数块(parameter_blocks),

雅可比矩阵(jacobians),残差值(residuals);
5) marginalization_info->marginalize():多线程计整个先验项的参数块,雅可比矩阵和残差值,
即计算公式(42),其中与代码对应关系为:

2.当次新帧不是关键帧时,MARGIN_SECOND_NEW, 我们将直接扔掉次新帧及它的视觉观测边,而不对次新帧进行 marg,

因为我们认为当前帧和次新帧很相似,也就是说当前帧跟路标点之间的约束和次新帧与路标点的约束很接近,直接丢弃并

不会造成整个约束关系丢失过多信息。但是值得注意的是,我们要保留次新帧的 IMU 数据,从而保证 IMU 预积分的连贯性。
通过以上过程先验项就构造完成了,在对滑动窗口内的状态量进行优化时,把它与 IMU残差项和视觉残差项放在一起优化,

从而得到不丢失历史信息的最新状态估计的结果。

 

3、 marginalization_factor.h

#pragma once

#include <ros/ros.h>
#include <ros/console.h>
#include <cstdlib>
#include <pthread.h>
#include <ceres/ceres.h>
#include <unordered_map>

#include "../utility/utility.h"
#include "../utility/tic_toc.h"

const int NUM_THREADS = 4;


//这个类保存了待marg变量（xm）与相关联变量（xb）之间的一个约束因子关系 --- Zm
struct ResidualBlockInfo
{
    ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)
        : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}

    void Evaluate();//调用cost_function的evaluate函数计算残差 和 雅克比矩阵

    ceres::CostFunction *cost_function;      //对应ceres中表示约束因子的类
    ceres::LossFunction *loss_function;
    std::vector<double *> parameter_blocks;  //该约束因子相关联的参数块变量，优化变量数据。
    std::vector<int> drop_set;               //parameter_blocks中待marg变量的索引

    double **raw_jacobians;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
    Eigen::VectorXd residuals;

    int localSize(int size)
    {
        return size == 7 ? 6 : size;
    }
};


//使用多线程来构建增量方程的H,g, 仔细理解为何可以多线程进行并行的矩阵构建? FEJ?
struct ThreadsStruct
{
    std::vector<ResidualBlockInfo *> sub_factors;
    Eigen::MatrixXd A; //对应增量方程的H，Hx=g
    Eigen::VectorXd b; //对应增量方程的g，
    
    //每个优化变量快的变量大小，IMU为类，【7，9，7，9】
    std::unordered_map<long, int> parameter_block_size; //global size
    std::unordered_map<long, int> parameter_block_idx;  //local size
};



class MarginalizationInfo
{
  public:
    MarginalizationInfo(){valid = true;};
    ~MarginalizationInfo();
    
    int localSize(int size)  const;
    int globalSize(int size) const;
    
    //添加残差块相关信息（优化变量，待marg的变量）
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
    
    //计算每个残差对应的雅克比，并更新parameter_block_data。
    void preMarginalize();
    
     //pos为所有变量维度，ｍ为需要marg掉的变量，ｎ为需要保留的变量
    void marginalize();
    
    //开启多线程构建信息矩阵H和b ，同时从H,b中恢复出线性化雅克比和残差
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors;//残差块，包括视觉，惯导，边缘化残差
    
    //这里将参数块分为Xm,Xb,Xr；Xm表示被marg掉的参数块，Xb表示与Xm相连接的参数块，Xr表示剩余的参数块
    int m, n;
    
    //被marg掉的约束边相关联的参数块 Xb 
    std::unordered_map<long, int> parameter_block_size;      //<优化变量内存地址,localSize>
    int sum_block_size;
    std::unordered_map<long, int> parameter_block_idx;       //<优化变量内存地址,在矩阵中的id>
    std::unordered_map<long, double *> parameter_block_data; //<优化变量内存地址,数据>

    //他们是进行边缘化之后保留下来的各个优化变量的长度，
    std::vector<int> keep_block_size; 
    std::vector<int> keep_block_idx;   
    std::vector<double *> keep_block_data;//就是参数的先验值x0,prior value

    //分别指的是边缘化之后从信息矩阵恢复出来雅克比矩阵和残差向量
    Eigen::MatrixXd linearized_jacobians; //H
    Eigen::VectorXd linearized_residuals; //b
    const double eps = 1e-8;
    bool valid;

};

class MarginalizationFactor : public ceres::CostFunction
{
  public:
    MarginalizationFactor(MarginalizationInfo* _marginalization_info);
    
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    MarginalizationInfo* marginalization_info;
};

 

4 、marginalization_factor.cpp 代码中有几个变量需要提前说明:

举例说明,当第一次 marg 掉最老帧时,parameter_block_size 为所有变量及其对应的
localSize,  parameter_block_data 为对应的数据(double*类型)

 

二、程序部分

个人而言，边缘化的原理并不难理解，也可能是因为看了很多的缘故！ 在看这部分程序之前，强烈推荐先把 Ceres 

这个工具熟悉，连接如：

https://blog.csdn.net/hltt3838/article/details/108629948?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522160517521419195264760092%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=160517521419195264760092&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_v1~rank_blog_v1-1-108629948.pc_v1_rank_blog_v1&utm_term=ceres&spm=1018.2118.3001.4450

 

https://blog.csdn.net/hltt3838/article/details/109577950

 

边缘化部分在  optimization 函数里面，当执行完后端优化后，紧接着就会执行边缘化，两个过程都需要添加优化变量和残差，

但区别是边缘化的变量少一点！主要是构建模块的过程！

/----------------------------------------------------------------------------------------------程序----------------------------------------------------------------------------------------/

一、如果边缘化 “老帧”

【1】首先添加上一次先验残差项

// --------------------------------------marginalization-----------------------------------------
    //添加残差以及优化变量的方式和后端线性优化中添加的方式相似，因为边缘化类应该就是仿照ceres写的
    TicToc t_whole_marginalization;
    
       //一、边缘化老帧，那就要考虑老帧的情况！
    if (marginalization_flag == MARGIN_OLD)
    {
        MarginalizationInfo *marginalization_info = new MarginalizationInfo();//先验信息
        
        vector2double();  

       //【1】首先添加上一次先验残差项
        if (last_marginalization_info && last_marginalization_info->valid)
        {
            vector<int> drop_set;
        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);
            }
                 
            MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
            last_marginalization_parameter_blocks,drop_set); 
            
            //调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入优化变量中
            marginalization_info->addResidualBlockInfo(residual_block_info);
        }

【2】添加IMU的marg信息：第0帧和第1帧之间的IMU预积分值以及第0帧和第1帧相关优化变量

if(USE_IMU)
        {
            if (pre_integrations[1]->sum_dt < 10.0)
            {
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
                 
                //0和1是para_Pose[0], para_SpeedBias[0]，需要marg的变量
                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});
                
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
        }

【3】最后添加第一次观测滑窗中第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 < 4)
                    continue;

                ++feature_index;

                int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
                
           //特征点的起始帧必须是首帧，因此后面用来构建marg矩阵的都是和第一帧有共视关系的滑窗帧，因为 marginalization_flag == MARGIN_OLD
                if (imu_i != 0)
                    continue;

                 //得到该Feature在起始帧下的归一化坐标
                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)//不需要起始观测帧
                    {
                        Vector3d pts_j = it_per_frame.point;
                        ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(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);
                        
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function, vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
                        vector<int>{0, 3});
                        
                        marginalization_info->addResidualBlockInfo(residual_block_info);
                    }
                    
                    if(STEREO && it_per_frame.is_stereo)
                    {
                        Vector3d pts_j_right = it_per_frame.pointRight;
                        if(imu_i != imu_j)
                        {
                            ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
                            it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
                            
                            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},vector<int>{0, 4});
                            
                            marginalization_info->addResidualBlockInfo(residual_block_info);
                        }
                        else
                        {
                            ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
                            it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
                            
                            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,vector<double *>{para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},vector<int>{2});
                            
                            marginalization_info->addResidualBlockInfo(residual_block_info);
                        }
                    }
                }
            }
        }

理解：

上面三步通过调用 addResidualBlockInfo() 已经确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置；

为什么要添加这些量呀？因为在k时刻这些量是我们要边缘化掉的变量，残差；但是k+1时刻就是我们的先验，这不正是我们初衷嘛！

需要注意的是这些都是和第一帧图像有关的！因为我们边缘化的是最老帧！三部分的流程都是一样的，如下：

 

第一步 定义损失函数，对于先验残差就是MarginalizationFactor，对于IMU就是IMUFactor，对于视觉就是ProjectionTdFactor，

这三个损失函数的类都是继承自ceres的损失函数类ceres::CostFunction，里面都重载了函数

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

这个函数通过传入的优化变量值 parameters，以及先验值（对于先验残差就是上一时刻的先验残差last_marginalization_info，

对于IMU就是预计分值pre_integrations[1]，对于视觉就是空间的的像素坐标pts_i, pts_j）可以计算出各项残差值residuals，

以及残差对应个优化变量的雅克比矩阵jacobians； 比如   MarginalizationFactor

class MarginalizationFactor : public ceres::CostFunction
{
  public:
    MarginalizationFactor(MarginalizationInfo* _marginalization_info);
    
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    MarginalizationInfo* marginalization_info;
};

第二步定义ResidualBlockInfo，其构造函数如下：

ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)

这一步是为了将不同的损失函数_cost_function以及优化变量_parameter_blocks统一起来再一起添加到marginalization_info中。

变量_loss_function是核函数，在VINS-mono的边缘化中仅仅视觉残差有用到核函数，另外会设置需要被边缘话的优化变量的位置

_drop_set，这里对于不同损失函数又会有不同：对于先验损失，其待边缘化优化变量是根据是否等于para_Pose[0]或者

para_SpeedBias[0]，也就是说和第一帧相关的优化变量都作为边缘化的对象。
对于IMU，其输入的_drop_set是vector{0, 1}，也就是说其待边缘化变量是para_Pose[0], para_SpeedBias[0]，也是第一帧相关的

变量都作为边缘化的对象，这里值得注意的是和后端优化不同，这里只添加了第一帧和第二帧的相关变量作为优化变量，因此边缘

化构造的信息矩阵会比后端优化构造的信息矩阵要小对于视觉，其输入的_drop_set是vector{0, 3}，也就是说其待边缘化变量是

para_Pose[imu_i]和para_Feature[feature_index]，从这里可以看出来在VINS-mono的边缘化操作中会不仅仅会边缘化第一帧相关的优化变量，

还会边缘化掉以第一帧为起始观察帧的路标点。

 

第三步是将定义的residual_block_info添加到marginalization_info中，通过下面这一句

marginalization_info->addResidualBlockInfo(residual_block_info);

addResidualBlockInfo() 这个函数的实现如下：

void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
    
    factors.emplace_back(residual_block_info);

     //总残差快
    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;
    
    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();

    //这里应该是优化的变量
    for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
    {
        double *addr = parameter_blocks[i]; //指向数据的指针
        int size = parameter_block_sizes[i];//因为仅仅有地址不行，还需要有地址指向的这个数据的长度
        parameter_block_size[reinterpret_cast<long>(addr)] = size;//将指针强转为数据的地址
    }

    //这里应该是待边缘化的变量
    for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
    {
        //这个是待边缘化的变量的id
        double *addr = parameter_blocks[residual_block_info->drop_set[i]];
        
        //将需要marg的变量的id存入parameter_block_idx
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
    }
}

目的：

 将不同损失函数对应的优化变量、边缘化位置存入到 parameter_block_sizes 和 parameter_block_idx中，这里注意的是执行到这一步，

parameter_block_idx 中仅仅有待边缘化的优化变量的内存地址的key，而且其对应value全部为0；

 

 

【4】调用 preMarginalize() 进行预处理

上面通过调用 addResidualBlockInfo() 已经确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置，

下面就需要调用 preMarginalize() 进行预处理，preMarginalize() 实现如下：

void MarginalizationInfo::preMarginalize()
{
    //遍历所有factor，在前面的addResidualBlockInfo中会将不同的残差块加入到factor中。
    for (auto it : factors)
    {
        it->Evaluate();//利用多态性分别计算所有状态变量构成的残差和雅克比矩阵
        
        //遍历所有参数块
        std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();
        for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
        {
            //优化变量的地址
            long addr = reinterpret_cast<long>(it->parameter_blocks[i]);
            int size = block_sizes[i];
            
    //将factor中的参数块复制到parameter_block_data中，parameter_block_data是整个优化变量的数据
            if (parameter_block_data.find(addr) == parameter_block_data.end())
            {
              double *data = new double[size];                              //会在析构函数中删除
              memcpy(data, it->parameter_blocks[i], sizeof(double) * size); //重新开辟一块内存
              parameter_block_data[addr] = data;//通过之前优化变量的数据的地址和新开辟的内存数据进行关联
            }
        }
    }
}

it->Evaluate()  这一句里面其实就是调用各个损失函数中的重载函数Evaluate()，这个函数前面有提到过：

 

【5】调用marginalize()

前面已经将数据都准备好了，下面通过调用marginalize()函数就要正式开始进行边缘化操作了，实现如下：

void MarginalizationInfo::marginalize()
{
    int pos = 0;
    
    for (auto &it : parameter_block_idx)//遍历待marg的优化变量的内存地址
    {
        it.second = pos;
        pos += localSize(parameter_block_size[it.first]);
    }

    m = pos;//需要marg掉的变量个数

    
    for (const auto &it : parameter_block_size)//计算除了边缘化之外要保留的参数块
    {
        //如果这个变量不是是待marg的优化变量。
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
        {
            parameter_block_idx[it.first] = pos;//就将这个待marg的变量id设为pos
            pos += localSize(it.second);        //pos加上这个变量的长度
        }
    }//上面的操作就会将所有的优化变量进行一个伪排序 

    n = pos - m;//要保留下来的变量个数
    
    if(m == 0)
    {
        valid = false;
        printf("unstable tracking...\n");
        return;
    }

    TicToc t_summing;
    Eigen::MatrixXd A(pos, pos);//整个矩阵大小 --- 没有边缘化之前的矩阵
    Eigen::VectorXd b(pos);
    A.setZero();
    b.setZero();
     

    TicToc t_thread_summing;
    pthread_t tids[NUM_THREADS];
    ThreadsStruct threadsstruct[NUM_THREADS];//携带每个线程的输入输出信息
    
    //为每个线程均匀分配factor。
    int i = 0;
    for (auto it : factors)
    {
        threadsstruct[i].sub_factors.push_back(it);
        i++;
        i = i % NUM_THREADS;
    }
    
    //构造4个线程，并确定线程的主程序
    for (int i = 0; i < NUM_THREADS; i++)
    {
        TicToc zero_matrix;
        threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
        threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
        threadsstruct[i].parameter_block_size = parameter_block_size;
        threadsstruct[i].parameter_block_idx = parameter_block_idx;
        
        //分别构造矩阵
        int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
        if (ret != 0)
        {
            ROS_WARN("pthread_create error");
            ROS_BREAK();
        }
    }
    
       //将每个线程构建的A和b加起来
       for( int i = NUM_THREADS - 1; i >= 0; i--)  
       {
          pthread_join( tids[i], NULL ); //阻塞等待线程完成
          A += threadsstruct[i].A;
          b += threadsstruct[i].b;
       }
  
    /*代码中求Amm的逆矩阵时，为了保证数值稳定性，做了Amm=1/2*(Amm+Amm^T)的运算，Amm本身是一个对称矩阵，所以  等式成立。接着对Amm进行了特征值分解,再求逆，更加的快速*/
    Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);

    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
   

    // 设x_{m}为要被marg掉的状态量，x_{r}是与x_{m}相关的状态量，所以在最后我们要保存的是x_{r}的信息
    //
    //      |      |    |          |   |
    //      |  Amm | Amr|  m       |bmm|        |x_{m}|
    //  A = |______|____|      b = |__ |       A|x_{r}| = b
    //      |  Arm | Arr|  n       |brr|
    //      |      |    |          |   |
    //  使用舒尔补:
    //  C = Arr - Arm*Amm^{-1}Amr
    //  d = brr - Arm*Amm^{-1}bmm
    
    //舒尔补，上面这段代码边缘化掉xm变量，保留xb变量 
    Eigen::VectorXd bmm = b.segment(0, m);
    Eigen::MatrixXd Amr = A.block(0, m, m, n);//0，m是开始的位置，m,m是开始位置后的大小
    Eigen::MatrixXd Arm = A.block(m, 0, n, m);
    Eigen::MatrixXd Arr = A.block(m, m, n, n);
    Eigen::VectorXd brr = b.segment(m, n);
    A = Arr - Arm * Amm_inv * Amr; 
    b = brr - Arm * Amm_inv * bmm;//这里的A和b是marg过的A和b,大小是发生了变化的
    
    
    
    //下面就是更新先验残差项
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);//求更新后 A特征值
    
    Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
    
    Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));

    //求取特征值及其逆的均方根
    Eigen::VectorXd S_sqrt = S.cwiseSqrt();
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();

    //分别指的是边缘化之后从信息矩阵A和b中恢复出来雅克比矩阵和残差向量；
    //两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去
    linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
    linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;
}

子函数   void* ThreadsConstructA(void* threadsstruct)             

void* ThreadsConstructA(void* threadsstruct)
{
    ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);
    
    //遍历该线程分配的所有factors，所有观测项
    for (auto it : p->sub_factors)
    {
        //遍历该factor中的所有参数块，五个参数块，分别计算，理解！
        for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
        {
             //得到参数块的大小
            int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];
            int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];
            
            if (size_i == 7)//7或在9，但是=7时候姿态四元数最后一项是0，9=V + bg +ba
                size_i = 6;
            
            //只提取local size部分，对于pose来说，是7维的,最后一维为0，这里取左边6维
            //P.leftCols(cols) = P(:, 1:cols)，取出从1列开始的cols列
            Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
            for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
            {
                int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
                int size_j = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])];
                if (size_j == 7)
                    size_j = 6;
                Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
                
                //对应对角区域，H*X=b, A代表H矩阵
                if (i == j)
                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                else
                {
                    //对应非对角区域
                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                    p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();
                }
            }
            //求取g，Hx=g，都是根据公式来写程序的！
            p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
        }
    }
    return threadsstruct;
}

功能：

函数会通过多线程快速构造各个残差对应的各个优化变量的信息矩阵（雅克比和残差前面都已经求出来了），如下图所示：

这里构造信息矩阵时采用的正是 parameter_block_idx 作为构造顺序，自然而然地将待边缘化的变量构造在矩阵的左上方；

 

【6】滑窗预移动

//这里仅仅将指针进行了一次移动，指针对应的数据还是旧数据，调用的 slideWindow() 才能实现真正的滑窗移动
 
        std::unordered_map<long, double *> addr_shift;
        for (int i = 1; i <= WINDOW_SIZE; i++)//从1开始，因为第一帧的状态不要了
        {
            //这一步的操作指的是第i的位置存放的的是i-1的内容，这就意味着窗口向前移动了一格
            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
            if(USE_IMU)
                addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
        }
        
        for (int i = 0; i < NUM_OF_CAM; i++)
            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];

        addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];

        
        //根据地址来得到保留的参数块
        vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

        if (last_marginalization_info)//Zb
            delete last_marginalization_info;                     //删除掉上一次的marg相关的内容
        last_marginalization_info = marginalization_info;         //marg相关内容的递归
        last_marginalization_parameter_blocks = parameter_blocks; //优化变量的递归，这里面仅仅是指针
        
    }

这一部分应该很容易明白；当边缘化帧的 “新帧”就不说了，把上面的看懂了，估计那个也会了

/----------------------------------------------------------------------------------------------程序----------------------------------------------------------------------------------------/