图解vins后端边缘化

后端代码逻辑

vins后端大部分代码集中在optimization函数中，而此函数中的边缘化占据相当一部分内容。

对边缘化后Hδx=b的处理

因为对于ceres，传入的是雅可比和残差进行迭代，所以首先需要对边缘化后的hessian矩阵进行分解，得到雅可比，又因为残差e0是不断变化的，所以对b在b0*处展开，然后经过运算得到残差ep。

边缘化中H(x1,x2)=b中两个x的选取

以vins举例，假如次新帧为关键帧，此时需要边缘化掉最老帧，需要注意两个x需要有联系。x1为想要边缘化掉的帧的位姿，路标点，速度，x2为后一帧的相应量。

VINS边缘化步骤总结

对以上步骤的详细解释与推导可以参考这篇博客

vins边缘化具体实现

假设滑动窗口有五个帧，每个帧要优化的变量为para_Feature，para_Ex_Pose，para_Pose，para_SpeedBias。
定义三个unordered_map分别为 地址-变量块维度、地址-变量块id、地址-数据映射。
主要解释一下中间的parameter_block_idx数据结构，存放的是数据块的id，最下面三个黑色字体存储的是将要边缘化掉的变量对应的参数块，这里para_Pose中的旋转存储的是三个自由度，而不是有四个自由度的四元数。可以看到将要边缘化掉的变量为16维，上面的红色参数块儿为与边缘化参数块儿相关的参数块。
小结：此次边缘化（图中所示）被边缘化的参数块的维度是16，和这些参数块有关的参数块维度是27，总共维度是43。

然后，通过parameter_block_idx和参数块来构建A矩阵和b矩阵，A矩阵的行和列是按照索引id展开的。注意，被边缘化的参数块分布在左上角。

首先是IMU约束，最右边的图示中可看出，和第0帧之间有IMU约束的只有第1帧，相应的参数块为para_Pose[0]，para_Pose[1]，para_SpeedBias[0]，para_SpeedBias[1]。在A和b矩阵中对应相应的蓝色块。

然后是视觉约束，由于与第0帧存在共视关系的有第1帧和第2帧，所以视觉约束共有两个，本图展示的是第一个视觉约束，相应的参数块为para_Pose[0]，para_Pose[1]，para_Ex_Pose[0]，para_Feature[1]。在A和b矩阵中对应相应的蓝色块。

视觉约束2：表示第2帧与第1帧存在共视关系，其他的与第一个视觉因子类似。

经过以上步骤得到了三个A矩阵和b矩阵，将其累加在一起，得到一个总的A矩阵和b矩阵。

然后就是对总的A矩阵和b矩阵进行边缘化操作，可以将图中的各个矩阵块与上方的公式一一对应。

上图即舒尔补的公式及其对应的矩阵块的图示。可以看到，经过舒尔补的H矩阵变为了稠密矩阵。

然后就对H矩阵SVD分解反推出雅可比矩阵和残差e0。

---

以上过程将最老帧边缘化掉，暂且称作第一次边缘化（可以分类为第一类边缘化，因为这种边缘化之前没有边缘化先验），接下来还有第二类边缘化（这类边缘化有边缘化先验），即将特征点2和第一帧边缘化掉。

同样，给出这三个数据结构，其意义不再赘述。

上图为IMU约束。

视觉约束

这一个步骤是第二类边缘化独有的，即需要处理上一步的边缘化先验信息。以上图为例，第0帧的边缘化后得到的信息带有第1帧和第2帧的pose，第1帧的速度和偏移量，外参。将其扩展后，得到第二步边缘化所需要的A矩阵和b矩阵。

以上得到的三组H矩阵和b矩阵相加。

H矩阵和b矩阵块图示与公式的对应关系。图中的左上角为要边缘化掉的变量。

舒尔操作，得到新的H矩阵和b矩阵，然后对齐进行分解。

---

以上就是VINS边缘化的全部逻辑。都以边缘化最老帧举例，即当最新帧为关键帧，marginalization_flag = MARGIN_OLD。

有一点需要注意：Hessian矩阵在最大似然（MLE）问题中被认为约等于信息矩阵，所以一般也会将Hessian矩阵直接当做信息矩阵对待。
关于信息矩阵和hessian矩阵，可以参考这篇文章

vins边缘化代码逻辑基本如上，具体代码操作基本就是围绕三个map数据结构取数据-计算-存数据展开。

如有误欢迎批评指正。