一、概述

在第十和第十一这两篇文章中，虽然一直在介绍融合，但是各位应该也注意到了一点，就是这个融合是建图任务下的融合，主要原因是因为，那种一次性优化所有内容的融合方法（结构图如下），效率太低了，执行一次优化所需要的时间太长，所以它不适合用来做实时的定位，而建图任务是离线的，所以无所谓。

那么问题来了，在实时的定位中，这个融合该怎么做嘞？

二、滑动窗口简介

要解决上面的问题，有一个简单的思路，就是把老的关键帧从优化模型里剔除掉，因为它们已经被优化很多次了，再优化也不会有太大的变动。这时候，融合的结构图可以变成下面这样。

看起来好像解决了问题，但其实引入了一个新问题，那就是，窗口中只有新的帧，老的帧对新帧的约束随着老帧的删除就直接凭空消失了，这显然是不合理的。所以一个合理的滑动窗口，应该是在删除老的帧的同时，把它的约束也保留下来，这种行为叫做“边缘化”，它是一个很复杂的过程，放在后面介绍。此处暂且只理解这个东西的作用，并且明白它的使用流程（如下图）

三、边缘化原理

优化问题具有如下通用形式：

[公式]

并可拆解成如下形式:

[公式]

拆解的目的是通过一系列操作，把 [公式] (可以当作老的帧)从状态量里删除掉，并把它的约束保留下来。要做到这一步，需要用到舒尔补，我们在滤波器原理推导的时候介绍过这个概念，这里再回顾一下。

给定矩阵

[公式]

它可以通过如下变换，变成上三角矩阵，即

[公式]

其中， [公式] 称为 [公式] 关于 [公式] 的舒尔补。

拆解后的优化问题，可通过舒尔补对矩阵三角化，即

[公式]

进一步化简得

[公式]

此时，可以利用等式第2行直接得到

[公式]

其含义为：此时可以不依赖 [公式] 求解出 [公式] ，若我们只关心 [公式] 的值，则可以把 [公式] 从模型里删除。

四、基于边缘化的滑动窗口实现

明白边缘化的原理以后，我们要看一下它是怎样能够解决滑动窗口里“删除老的帧但又保留它的约束”这个过程的。

首先需要换一种语言重新描述一下窗口模型。

1. 基于地图定位的滑动窗口模型

1）窗口优化模型构成

在优化模型中，优化问题可写成如下形式

[公式]

其中[公式] 是残差， [公式] 是残差关于状态量的雅可比， [公式] 是信息矩阵。

在kitti工程中，基于地图定位的滑动窗口，其残差包括：

a.地图匹配位姿和优化变量的残差

b.激光里程计相对位姿和优化变量的残差

c.IMU预积分和优化变量的残差

d.边缘化形成的先验因子对应的残差

此处先介绍前3项，第4项待边缘化后介绍。

2）地图匹配位姿和优化变量的残差

该残差对应的因子为地图先验因子。一个因子仅约束一个位姿，其模型如下：

残差关于优化变量的雅可比，可视化如下：

因此，对应的Hessian矩阵的可视化为：

3）激光里程计相对位姿和优化变量的残差

该残差对应的因子为激光里程计因子。一个因子约束两个位姿，其模型如下：

残差关于优化变量的雅可比，可视化如下：

因此，对应的Hessian矩阵可视化为：

4）IMU预积分和优化变量的残差

该残差对应的因子为IMU因子。一个因子约束两个位姿，并约束两个时刻 IMU 的速度与 bias。

残差关于优化变量的雅可比，可视化如下：

因此，对应的Hessian矩阵可视化为：

5）完整模型

完整Hessian矩阵，即为以上各因子对应矩阵的累加。

上述过程用公式可表示为：

[公式]

其中

[公式]

[公式]

[公式]

矩阵乘法写成累加形式为：

[公式]

此累加过程，即对应前面可视化时各矩阵叠加。

2. 边缘化过程

1）移除老的帧

假设窗口长度为3，在加入新的帧之前，需要先边缘化掉老的帧，即下图方框中的变量。

用前述公式，可以表示为

[公式]

但是在实际代码中，会把它拆成两步实现。

第一步：使用和要边缘化掉的量无关的因子，构建剩余变量对应的Hessian矩阵。

上标 [公式] 代表是第一步中的变量，包含Hessian矩阵的完整表达式为

[公式]

第二步：挑出和要边缘化掉的量相关的因子，构建待边缘化的Hessian矩阵，并使用舒尔补做边缘化。

上标 [公式] 代表是第二步中的变量，包含Hessian矩阵的完整表达式为

[公式]

这一步形成的约束(上式)就叫先验因子，它包含了边缘化掉的量对剩余变量的约束关系。

最终使用的是两步的叠加，Hessian矩阵叠加的可视化如下

对应的完整公式为

[公式]

其中

[公式]

边缘化之后，模型如下：

注意：边缘化先验因子只有在第一次边缘化之前是不存在的，完成第一次边缘化之后就一直存在，并且随着后续新的边缘化进行，其内容不断更新。

2）添加新的帧

添加新的帧之后，模型如下：

此处直接给出新的Hessian矩阵可视化结果：

此后，随着定位过程的进行，便不断循环“边缘化老帧->添加新帧”的过程，从而维持窗口长度不变。

五、总结

基于边缘化的滑动窗口在代码里实现起来却是不是一件容易的事，就连讲都很困难，上面通过这种可视化的方式把过程讲了，只能算是描述了流程和原理，但是真正的想要写出来，这些还是不够的，我们无法在这里用文字形式把相关的代码部分再一一讲明，只能靠大家自己去对照着看了（可参考的代码有：lio-mapping、VINS等）。