ROS-3DSLAM(13):视觉部分visual estimator第七节 factor1

本文链接：https://blog.csdn.net/m0_49088083/article/details/121844987

这篇博客介绍了ROS中3DSLAM的视觉部分，特别是视觉估计器中的factor1。文章讨论了边缘化在限制VIO计算复杂度中的作用，通过边缘化优化滑动窗口中的IMU状态和特征点深度。边缘化计算涉及建立信息矩阵和H矩阵，并解释了边缘化如何影响优化过程。代码分析部分主要集中在VINS-mono的边缘化实现，讨论了残差、误差、范数和雅克比矩阵等概念。

摘要由CSDN通过智能技术生成

2021@SDUSC

2021年12月6日星期一——2021年12月9日星期四

一、背景简介:

上周实在是太忙了，本来还继续学习factor的代码，但是一个接一个的实验和课设打乱了我的计划。还是按照节奏一点一点慢慢来吧。initial的小结也可以先放一放，等到不忙了再补上一篇。

这周的计划是分析factor，首先分析其基本原理，然后结合代码进行深入的实现分析。

factor如上周介绍所说是用来处理后端优化的部分。这部分的优化包含了三部分:

特征点深度
IMU误差
关键帧位姿

我们对于这个文件的分析也便从这三方面着手。

这三个功能分别涵盖了不同的代码文件，为了研究其详细的功能，我们需要回到论文当中去学习。

在这里插入图片描述

公式（截图，出自下文参考博客）

这个公式就是用来处理最小化所有测量残差的先验和Mahalanobis范数之和，得到最大后验估计。

这里面求和的三项就分别是三类残差，分别是：

边缘化计算得到的先验残差：涉及到marginalization.cpp
运动模型的IMU计算中得到的残差：相关代码为imu_factor.h和integration_base.h
视觉的残差，特征点在相机的投影下产生:projection_factor.cpp和projection_td_factor.cpp

这里的第二点可以看到，只有两个头文件，是不能执行的。

这是因为所有的残差优化都是同我们上一个文件夹中的文件estimator.cpp息息相关的。

这次我们分析的对象是第一项，也就是边缘化计算中的残差。

二、基本原理:

论文部分

为了限制基于优化的VIO的计算复杂度，本文引入了边缘化。我们有选择地从滑动窗口中将IMU状态xK和特征λ1边缘化，同时将对应于边缘状态的测量值转换为先验。

如图7所示，当倒数第二帧是关键帧时，它将停留在窗口中，而最旧的帧与其相应的测量值被边缘化。但如果倒数第二帧是非关键帧，我们丢掉视觉测量值，保留连接到这个非关键帧的IMU测量值。为了保持系统的稀疏性，我们不会边缘化非关键帧的所有测量值**。我们的边缘化方案旨在保持窗口中空间分离的关键帧。**这确保了特征三角化有足够的视差，并且最大化了在大激励下获得加速度计测量值的概率。

边缘化是利用Schur补[39]进行的。我们基于与移除状态相关的所有边缘化测量值构造一个新的先验。新的先验项被添加到现有的先验项中。

我们确实注意到，边缘化导致了线性化点的早期固定，这可能导致次优估计结果。然而，由于小型漂移对于VIO来说是可以接受的，我们认为边缘化所造成的负面效果并不重要。

也就是边缘化是对于滑窗法的进一步优化，使得在滑动的过程中使得离开的图像帧能够得到更好的利用。

边缘化计算

根据预测模型和观测模型建立矩阵，建立各个节点的变量之间的信息矩阵。

边缘化是去掉概率图中的某一个节点后矩阵会如何变化。

最终将原来的稀疏矩阵变化成稠密矩阵，增加的稠密部分则是被边缘化掉那个节点传递给当前状态的信息，使得独立的变量变得相关。

步骤：

建立H矩阵：（信息矩阵）Hermite矩阵
去掉某个节点，修改信息矩阵
观察其间的变换，得到结论
应用到更复杂的情境下

三、代码分析:

下面开始对代码的实现进行分析。

VINS-mono的边缘化相关代码在estimator.cpp的Estimator类的optimization()函数中，该函数先会先进行后端非线性优化然后紧接着就是边缘化操作，下面就针对这个函数中的边缘化相关代码进行剖析。

也就是说，在VINS-mono的代码下，marginalization并不是唯一执行运算的单位，我们需要回到主函数当中去研究。

头文件：

在头文件当中，声明了两个结构和一个类。

第一个结构是残差块信息，第二个结构是向量结构？（存疑）。

第三个类是边缘化类，是分析的重点对象。

const int NUM_THREADS = 4;
//描述了残差块的结构
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();
	//各类残差的factor
    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;
    }
};
//描述了线上的结构？我估计是一个维度上的(❌)是线程的意思
struct ThreadsStruct
{
   
    std::vector<ResidualBlockInfo *> sub_factors;
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    std::unordered_map<long, int> parameter_block_size; //global size
    std::unordered_map<long, int> parameter_block_idx; //local size
};
//
class MarginalizationInfo
{
   
  public:
    ~MarginalizationInfo();
    int lo