VINS-Mono 代码解析六、边缘化（2）理论和代码详解

最新推荐文章于 2024-05-16 18:43:30 发布

他人是一面镜子，保持谦虚的态度

最新推荐文章于 2024-05-16 18:43:30 发布

阅读量2.9k

点赞数 5

分类专栏： VINS-Mono and Fusion 程序解读文章标签： VINS-mono

原文链接：https://blog.csdn.net/weixin_44580210/article/details/95748091/?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-0.control&spm=1001.2101.3001.4242

版权

VINS-Mono and Fusion 程序解读专栏收录该内容

20 篇文章 107 订阅

订阅专栏

1. 边缘化理论

边缘化相关的理论主要是参考高博和贺博的课程以及三篇参考文献：

《The Humble Gaussian Distribution》
《Exactly Sparse Extended Information Filters for Feature-Based SLAM》
《Consistency Analysis for Sliding-Window Visual Odometry》

1.1 为什么要进行边缘化操作？

首先我们知道，如果仅仅从前后两帧图像来计算相机变换位姿，其速度快但是精度低，而如果采用全局优化的方法（比如Bundle Adjustment），其精度高但是效率低，因此前辈们引入了滑窗法这样一个方法，每次对固定数量的帧进行优化操作，这样既保证了精度又保证了效率。既然是滑窗，在滑动的过程中必然会有新的图像帧进来以及旧的图像帧离开，所谓边缘化就是为了使得离开的图像帧得到很好的利用。

1.2 怎样进行边缘化呢？

说明： $\Lambda$ 的证明在最后；

注意：以上的步骤就是为了证明舒尔补的公式！

在这里插入图片描述

1.3 在实际的边缘化操作中有什么需要注意的吗？

一个比较值得注意的问题是新老信息融合的问题，也就是FEJ算法的使用，如下所示:

在这里插入图片描述

2. 代码剖析

上面理论搞清楚了其实只是第一步，由于VINS-mono优化的变量较多，VINS-mono的边缘化操作实际上要复杂很多，VINS-mono的边缘化相关代码在estimator.cpp的Estimator类的optimization()函数中，该函数先会先进行后端非线性优化然后紧接着就是边缘化操作，下面就针对这个函数中的边缘化相关代码进行剖析。

2.1 优化变量分析

首先我们确定下参与边缘化操作的变量有哪些，这个可以从vector2double()函数中看出来，因为ceres中变量必须用数组类型，所以需要这样一个函数进行数据类型转换，如下：

void Estimator::vector2double()
{
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        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();

        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();
    }
    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);
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

可以看出来，这里面生成的优化变量由：

para_Pose（6维，相机位姿）、
para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）、
para_Ex_Pose（6维、相机IMU外参）、
para_Feature（1维，特征点深度）、
para_Td（1维，标定同步时间）

五部分组成，在后面进行边缘化操作时这些优化变量都是当做整体看待。

2.1 MarginalizationInfo类分析

然后，我们先看下和边缘化类MarginalizationInfo

class MarginalizationInfo
{
  public:
    ~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();
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors;//所有观测项
    int m, n;//m为要边缘化的变量个数，n为要保留下来的变量个数
    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; //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;
};

先说变量，这里有三个unordered_map相关的变量分别是：

parameter_block_size、
parameter_block_idx、
parameter_block_data，

他们的key都同一是long类型的内存地址，而value分别是，各个优化变量的长度，各个优化变量的id以及各个优化变量对应的double指针类型的数据。

对应的有三个vector相关的变量分别是：

keep_block_size、
keep_block_idx、
keep_block_data，

他们是进行边缘化之后保留下来的各个优化变量的长度，各个优化变量在id以各个优化变量对应的double指针类型的数据
还有

linearized_jacobians、
linearized_residuals，

分别指的是边缘化之后从信息矩阵恢复出来雅克比矩阵和残差向量

2.3 第一步：调用addResidualBlockInfo()

对于函数我们直接看optimization中的调用会更直观，首先会调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入上面所述的优化变量中：

首先添加上一次先验残差项：（上一次待边缘化的参数与其他参数的约束关系）

VINS-Mono 代码解析六、边缘化（3）_努力努力努力-CSDN博客

if (last_marginalization_info)
{
	vector<int> drop_set;
	for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)//last_marginalization_parameter_blocks是上一轮留下来的残差块
	{
	    if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
	        last_marginalization_parameter_blocks[i] == para_SpeedBias[0])//需要marg掉的优化变量，也就是滑窗内第一个变量
	        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);
}

然后添加第0帧和第1帧之间的IMU预积分值以及第0帧和第1帧相关优化变量

{
    if (pre_integrations[1]->sum_dt < 10.0)
    {
        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的原因是0和1是para_Pose[0], para_SpeedBias[0]是需要marg的变量
        marginalization_info->addResidualBlockInfo(residual_block_info);
    }
}

最后添加第一次观测滑窗中第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 >= 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;//这里是从特征点的第一个观察帧开始
        if (imu_i != 0)//如果第一个观察帧不是第一帧就不进行考虑，因此后面用来构建marg矩阵的都是和第一帧有共视关系的滑窗帧
            continue;

        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)
            {
                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.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                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);
            }
            else
            {
                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和3的原因是，para_Pose[imu_i]是第一帧的位姿，需要marg掉，而3是para_Feature[feature_index]是和第一帧相关的特征点，需要marg掉
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
        }
    }
}

上面添加残差以及优化变量的方式和后端线性优化中添加的方式相似，因为边缘化类应该就是仿照ceres写的，我们可以简单剖析下上面的操作；

第一步定义损失函数，对于先验残差就是MarginalizationFactor，对于IMU就是IMUFactor，对于视觉就是ProjectionTdFactor，这三个损失函数的类都是继承自ceres的损失函数类ceres::CostFunction，里面都重载了函数；

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

第二步定义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的边缘化中仅仅视觉残差有用到couchy核函数，另外会设置需要被边缘话的优化变量的位置_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;//parameter_blocks里面放的是marg相关的变量
    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++)//这里应该是待边缘化的变量
    {
        double *addr = parameter_blocks[residual_block_info->drop_set[i]];//这个是待边缘化的变量的id
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;//将需要marg的变量的id存入parameter_block_idx
    }
}

这里其实就是分别将不同损失函数对应的优化变量、边缘化位置存入到parameter_block_sizes和parameter_block_idx中，这里注意的是执行到这一步，parameter_block_idx中仅仅有待边缘化的优化变量的内存地址的key，而且其对应value全部为0；

2.4 第二步：调用preMarginalize()

上面通过调用addResidualBlockInfo()已经确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置，下面就需要调用preMarginalize()进行预处理，preMarginalize()实现如下：

void MarginalizationInfo::preMarginalize()
{
    for (auto it : factors)//在前面的addResidualBlockInfo中会将不同的残差块加入到factor中
    {
        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];
            if (parameter_block_data.find(addr) == parameter_block_data.end())//parameter_block_data是整个优化变量的数据
            {
                double *data = new double[size];
                memcpy(data, it->parameter_blocks[i], sizeof(double) * size);//重新开辟一块内存
                parameter_block_data[addr] = data;//通过之前的优化变量的数据的地址和新开辟的内存数据进行关联
            }
        }
    }
}

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

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。此外这里会给parameter_block_data赋值，这里引用崔华坤老师写的《VINS 论文推导及代码解析》中的例子
在这里插入图片描述

parameter_block_sizes中的key值就是上表中的左边第一列，value值就是上表中的中间一列（localSize）
parameter_block_data中的key值就是上表中的左边第一列，value值就是上表中的右边第一列（double*的数据）

2.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)
    {
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())//如果这个变量不是是待marg的优化变量
        {
            parameter_block_idx[it.first] = pos;//就将这个待marg的变量id设为pos
            pos += localSize(it.second);//pos加上这个变量的长度
        }
    }

    n = pos - m;//要保留下来的变量个数

    //通过上面的操作就会将所有的优化变量进行一个伪排序，待marg的优化变量的idx为0，其他的和起所在的位置相关

    TicToc t_summing;
    Eigen::MatrixXd A(pos, pos);//整个矩阵A的大小
    Eigen::VectorXd b(pos);
    A.setZero();
    b.setZero();
   
    TicToc t_thread_summing;
    pthread_t tids[NUM_THREADS];
    ThreadsStruct threadsstruct[NUM_THREADS];
    int i = 0;
    for (auto it : factors)//将各个残差块的雅克比矩阵分配到各个线程中去
    {
        threadsstruct[i].sub_factors.push_back(it);
        i++;
        i = i % NUM_THREADS;
    }
    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();
        }
    }
    for( int i = NUM_THREADS - 1; i >= 0; i--)  
    {
        pthread_join( tids[i], NULL ); 
        A += threadsstruct[i].A;
        b += threadsstruct[i].b;
    }

    //TODO
    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();

    //舒尔补
    Eigen::VectorXd bmm = b.segment(0, m);
    Eigen::MatrixXd Amr = A.block(0, m, m, n);
    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);//求特征值
    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();
}

第一步，秉承这map数据结构没有即添加，存在即赋值的语法，上面的代码会先补充parameter_block_idx，前面提到经过addResidualBlockInfo()函数仅仅带边缘化的优化变量在parameter_block_idx有key值，这里会将保留的优化变量的内存地址作为key值补充进去，并统一他们的value值是其前面已经放入parameter_block_idx的优化变量的维度之和，同时这里会计算出两个变量m和n，他们分别是待边缘化的优化变量的维度和以及保留的优化变量的维度和。

第二步，函数会通过多线程快速构造各个残差对应的各个优化变量的信息矩阵（雅克比和残差前面都已经求出来了），然后在加起来，如下图所示：
在这里插入图片描述

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

第三步，函数会通过shur补操作进行边缘化，然后再从边缘化后的信息矩阵中恢复出来雅克比矩阵linearized_jacobians和残差linearized_residuals，这两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去。

2.6 第四步：滑窗预移动

在optimization的最后会有一部滑窗预移动的操作，就是下面这一段代码

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];//因此para_Pose这些变量都是双指针变量，因此这一步是指针操作
    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];
if (ESTIMATE_TD)
{
    addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
}
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

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

值得注意的是，这里仅仅是相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合后面调用的slideWindow()函数才能实现真正的滑窗移动，此外last_marginalization_info就是保留下来的先验残差信息，包括保留下来的雅克比linearized_jacobians、残差linearized_residuals、保留下来的和边缘化有关的数据长度keep_block_size、顺序keep_block_idx以及数据keep_block_data。
last_marginalization_info就是保留下来的滑窗内的所有的优化变量

这里需要明确一个概念就是，边缘化操作并不会改变优化变量的值，而仅仅是改变了优化变量之间的关系，而这个关系就是通过信息矩阵体现的。

备注： $\Lambda$ 的证明；

toy example 2

他人是一面镜子，保持谦虚的态度

关注

5
点赞
踩
39

收藏

觉得还不错? 一键收藏
0
评论
VINS-Mono 代码解析六、边缘化（2）理论和代码详解

1. 边缘化理论边缘化相关的理论主要是参考高博和贺博的课程以及三篇参考文献：《The Humble Gaussian Distribution》《Exactly Sparse Extended Information Filters for Feature-Based SLAM》《Consistency Analysis for Sliding-Window Visual Odometry》1.1 为什么要进行边缘化操作？首先我们知道，如果仅仅从前后两帧图像来计算相机变换位姿，
复制链接

扫一扫