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

最新推荐文章于 2025-06-04 10:52:50 发布

原创

最新推荐文章于 2025-06-04 10:52:50 发布 · 6.6k 阅读

99 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶

本文深入解析了视觉惯性里程计(VINS)中边缘化过程的理论与实现细节，包括边缘化前后信息矩阵的变化、多线程并行计算策略及边缘化后的滑窗预移动等内容。

“ 在优化完成后，滑动窗口前，必须进行边缘化项的操作”

一、理论部分

VINS 细节系列 - 窗口优化_努力努力努力-优快云博客

一个边缘化的例子

下面我们用一个具体例子来形象说明边缘化过程及其导致的矩阵稠密现象(fill-in)。设有 四个相机位姿 xpi ,

以及 6 个路标点xmk (路标点用 xyz 的参数化), 相机与路标点的边表示一次观测, 相邻相机之间的边表示 IMU 约束,

相互关系如下:

下面试图将 xp1 给 marg 掉,然后再将 xm1 给 marg 掉, 看看 H 矩阵会如何变化

图(12-a) 表示原始的 H 矩阵,注意这里的 左上角为路标点相关部分, 而右下角是 pose 相关部分

图(12-b)是把 H 矩阵中跟x p1 相关的部分移动到 H 矩阵左上角

其中各矩阵的维度已在上式标明,也就是说图(12-c)的矩阵大小为 32×32。我们可以看到,图(c)相对于图(b)变得更稠密了,

即 marg 掉一个 pose,会使得剩余的 H 矩阵有 3 个地方被 fill-in,如图(c)中颜色加重区域。

这时图关系则变为:

注意,观察图 16 可发现这时的 xm1 、xm2 和 xm3 彼此之间已经产生了新的约束关系,且xp2 也与xm1 产生了新的约束关系。

图(12-d)是 marg 掉xm1 后的 H 矩阵,详细如下所示:

对应的图关系如下:

我们发现, marg 掉xm1 后,并没有是 H 矩阵更稠密,这是因为xm1 之前并未与其他pose 有约束关系,即并未被观察到,

因此如果 marg 掉那些不被其他帧观测到的路标点,不会显著地使 H 矩阵变得稠密。而要 marg 掉的路标点中,对于那

些被其他帧观测到路标点,要么就别设置为 marg, 要么就宁愿丢弃,这就是 OKVIS 中用到的策略

简单介绍

1、优化变量：

序号变量维度

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

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

2、思路：

边缘化策略

边缘化分两种情况，每种情况有各自的流程

a. 如果次新帧是关键帧，则将边缘化最老帧，及其看到的路标点和IMU数据，转化为先验。具体流程为：

1）将上一次先验残差项传递给marginalization_info

2）将第0帧和第1帧间的IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中

3）将第一次观测为第0帧的所有路标点对应的视觉观测，添加到marginalization_info中

4）计算每个残差，对应的Jacobian，并将各参数块拷贝到统一的内存（parameter_block_data）中

5）多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差

6）调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座

b. 如果次新帧不是关键帧，此时具体流程为：

1）保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info

2）premargin：计算每个残差，对应的Jacobian，并更新parameter_block_data

3）marginalize：构造先验项舒尔补AX=b的结构，计算Jacobian和残差

4）调整参数块在下一次窗口中对应的位置（去掉次新帧）

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

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

二、程序部分

2.1 把下面这个图搞明白！

2.2 看 "marginalization_factor.h" 中类和结构体的定义

1、class ResidualBlockInfo

类描述：这个类保存了待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::LossFunction *loss_function;
    std::vector<double *> parameter_blocks;
    std::vector<int> drop_set;

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

成员变量	描述
cost_function	对应ceres中表示约束因子的类
parameter_blocks	该约束因子相关联的参数块变量
drop_set	parameter_blocks中待marg变量的索引

2、class MarginalizationInfo

类描述：这个类保存了优化时上一步边缘化后保留下来的先验信息

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;
    //这里将参数块分为Xm,Xb,Xr,Xm表示被marg掉的参数块，Xb表示与Xm相连接的参数块，Xr表示剩余的参数块
    //那么m=Xm的localsize之和，n为Xb的localsize之和，pos为（Xm+Xb）localsize之和
    int m, n;      
    std::unordered_map<long, int> parameter_block_size; //global size 将被marg掉的约束边相关联的参数块，即将被marg掉的参数块以及与它们直接相连的参数快
    int sum_block_size;
    std::unordered_map<long, int> parameter_block_idx; //local size  
    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 =