文章目录

• 1.边缘化倒数第二帧
• • 1.1.判断边缘化倒数第二帧的条件是否满足
  • • 1.1.1.倒数第二帧的IMU预积分约束
    • 1.1.2.倒数第二帧的上一次边缘化先验约束
  • 1.2.判断此次边缘化的丢弃参数块
  • 1.3.构造边缘化的残差块并进行边缘化
• 2.边缘化最老帧的时候，如果也存在上一次边缘化的信息
• 3.滑窗操作
• • 3.1.边缘化掉最老帧的滑窗操作
  • • 3.1.1.遍历滑窗中所有关键帧依次交换
    • 3.1.2.对滑窗中的最后位置赋值为倒数第二帧状态
  • 3.2.边缘化掉倒数第二帧的滑窗操作

1.边缘化倒数第二帧

1.1.判断边缘化倒数第二帧的条件是否满足

之所以边缘化倒数第二帧，是因为当前非线性优化处理的是滑窗中的最新帧，也就是倒数最后一帧。而在这之前当这帧图像刚进来的时候会判断上一帧是否是关键帧。判断上一帧的原因是：如果上一帧是关键帧，那么保留在滑窗里，把滑窗里最老帧删除；如果不是关键帧，那么为了对最新状态做出反应，我肯定要保留当前传入的图像在滑窗中，而把倒数第二帧删除掉。

然后再看这里所说的条件：

• 有上一次边缘化的信息

• 上次边缘化中保留下来的状态有倒数第二帧的参数块，这说明上次边缘化对倒数第二帧形成了先验约束，现在我要把倒数第二帧边缘化掉，那么对导数第二帧的约束就要再次边缘化，转化成对其他帧的约束。这样我才能在后面的滑窗中把倒数第二帧直接删除，而不损失约束信息。

1.1.1.倒数第二帧的IMU预积分约束

对于IMU预积分来说，倒数第二帧的IMU预积分虽然在本次的非线性优化中构成了倒数第二帧和倒数第三帧之间的约束，因此在非线性优化中起到了约束作用。但是现在要删除倒数第二帧，此时只需要把倒数第二帧的IMU预积分和最后一帧的IMU预积分合成一个即可，下次非线性优化的时候就可以当做一个IMU预积分约束来使用了，如下图所示。

问题思考：还不是很明白为什么这里的操作和边缘化最老帧不同？为什么不考虑IMU预积分和视觉重投影的约束？

我觉得本质上还是因为倒数第二帧被判断为不是关键帧，所以它的这些约束都是虚假的、无效的，因此我们处理的方式就是当做没有它，然后把倒数第二帧的IMU预积分进行累加，而它看到的地图点在后面滑窗的时候也就是把它对地图点的观测从地图点管理器中直接删除即可，就相当于倒数第二帧没有出现过。

1.1.2.倒数第二帧的上一次边缘化先验约束

但是如果上一次边缘化的信息对倒数第二帧也有约束，那么情况就不一样了。因为这样相当于上一次边缘化对关键帧的先验约束有一部分被分摊到了倒数第二帧上，如果我们还是像上面处理IMU预积分和视觉重投影那样简单的直接把倒数第二帧删除的话，那么相当于上次边缘化分摊在它身上的先验约束也直接删除了，而实际上我们知道它的这部分先验约束应该是属于其他关键帧的，而它不是关键帧却在上次边缘化的时候占用了一些关键帧的先验约束。如下图所示，相当于上次边缘化对关键帧的先验约束中，有一部分被划分到了倒数第二帧这个非关键帧上。

所以这次要边缘化掉倒数第二帧，我们就要把倒数第二帧的先验约束（即图中T3T_3T3​的先验约束，灰色大三角）边缘化掉（分解掉），返回到原来的几个关键帧的先验约束上（变成小三角）。如下图所示：

代码如下：

1.2.判断此次边缘化的丢弃参数块

1.判断上次边缘化留下的状态中是否有零偏BBB

因为边缘化中和零偏有关的是IMU预积分，而这只可能在边缘化老帧的时候对第1帧产生约束，所以不可能对倒数第二帧的零偏产生约束，所以这里判断一下只是更加严谨。

2.判断状态是位姿，则此次边缘化丢弃

正常状况下就是上次边缘化留下的是由于上次的最老帧和本次的倒数第二帧之间有共视地图点，由视觉重投影约束造成了对倒数第二帧的位姿的先验约束。因此本次我们就要边缘化丢弃掉倒数第二帧的位姿的先验约束。

1.3.构造边缘化的残差块并进行边缘化

构造边缘化的残差块代码如下：

这里乍一看不太好理解，实际上我们就把上次的边缘化构成的先验约束也看成和IMU预积分一样的残差块即可，因为我们上次边缘化得到了对保留下来的状态的残差和雅克比，这跟IMU预积分是一样的。比如还是以上面的例子为例，上次边缘化得到的雅克比矩阵以及对应的HHH矩阵如下：

所以我们这次边缘化倒数第二帧的时候，可以按照如下步骤：

• 排序状态向量：把本次要边缘化掉的参数块（状态变量）像之前边缘化最老帧一样挪动到最前面

• 重新构造雅克比矩阵：利用上次边缘化的得到的雅克比矩阵中，残差对每一个参数块的小雅克比，重新构造本次状态变量排序后的新雅克比矩阵，结果如下：
  

• 重新构造HHH矩阵：重新构造的雅克比矩阵相乘得到重新构造的HHH矩阵，结果如下：

• 进行舒尔补得到舒尔补之后的H′,g′H', g'H′,g′矩阵

• 分解H′,g′H', g'H′,g′得到这次边缘化掉倒数第二帧的位姿得到的新的雅克比矩阵J′J'J′和残差e′e'e′。

对应的代码和之前一样，先预处理，然后边缘化：

2.边缘化最老帧的时候，如果也存在上一次边缘化的信息

这里的处理跟边缘化倒数第二帧类似。

因为目前我们是在边缘化最老帧，那么就要看最老帧的所有约束。首先就是之前讲的IMU预积分约束和视觉重投影约束，然后如果上次也进行过边缘化，并且上次边缘化中留下来的状态变量中有属于当前的最老帧的状态变量呢？如下图所示：

其实由图中就可以看出来，这个和边缘化倒数第二帧非常相似。目前我们的最老帧T1T_1T1​中有上一次边缘化的先验约束，那么这次边缘化的时候就把它当成一个先验残差和雅克比也边缘化掉即可。所以思想和边缘化倒数第二帧是一模一样的。

代码如下：

3.滑窗操作

3.1.边缘化掉最老帧的滑窗操作

3.1.1.遍历滑窗中所有关键帧依次交换

代码如下：

3.1.2.对滑窗中的最后位置赋值为倒数第二帧状态

注意：这一步骤就是滑窗操作的精髓所在！

之前我们已经把本次非线性优化、边缘化最老帧的相关计算完成了，但是实际存储在滑窗中的状态变量还没有进行滑窗操作，所以上面依次交换前后两帧，将滑窗中的所有帧都向前移动了一个位置，然后滑窗中的最老帧被抛弃掉了。

此时我们会发现滑窗中的最后一个位置变成了最老帧，显然这个是不正确的。实际上我们应该把滑窗中最后一个位置的状态清空，然后等待这下一帧图像的到来，继续处理。但是如果这样操作的话，下一帧图像和对应的一堆IMU数据到来的时候，首先要进行预积分，注意预积分的同时我们会计算滑窗中最后一个位置使用预积分得到的预测状态，见estimator::processIMU中的操作：

自然我们要从上一次非线性优化之后得到的最新的关键帧的位姿上使用IMU积分进行预测，但是如果之前我们是把滑窗最后一个位置清空的话，现在我们就要在第一次IMU积分的时候从滑窗中倒数第二个位置中读取上一次非线性优化得到的最新的关键帧的位姿然后积分，然后积分结果再存储到滑窗中的最后一个位置上。

这样做显然是可以的，但是我们要判断是不是IMU第一次积分，如果是还要从倒数第二个滑窗位置中读取上一次优化后的最新状态，太麻烦了。所以更简单的方式就是我们在上一次非线性优化、边缘化最老帧、将最老帧滑出窗口之后，此时滑窗中最后一个位置的数据直接赋值为本次一堆操作后的最新帧的位姿，这样在下一次图像到来的时候，就可以直接基于滑窗中最后一个位置的状态（就是上一次非线性优化得到的最新状态）进行IMU积分递推了，这样编程实现更加简单。如下图所示：

代码如下：

3.2.边缘化掉倒数第二帧的滑窗操作

这种情况就比较简单了，主要分为三个步骤：

• 把倒数第二帧的预积分和最后一帧的预积分合并成一个预积分，如下图所示

• 把最后一帧移到倒数第二帧的位置；

• 同理为了方便下次运行把滑窗中的最后一帧的位置的数据赋值为当前最新的状态，即刚才移动完之后目前位置的倒数第二帧的状态。

代码如下：

细节思考：某一帧的预积分，是这一帧相对前一帧的预积分，还是这一帧相对后一帧的预积分？

解答：是这一帧相对前一帧的预积分。因为从我们上面的讲解，每次滑动窗口前移滑窗中的关键帧之后，都会把当前最新的状态（也就是此时滑窗中的倒数第二帧）赋值给滑窗中最后一个位置，然后下次运行的时候就相当于从滑窗中的倒数数第二帧的状态开始进行IMU积分递推预测最新状态，同理IMU预积分也相当于是相对滑窗中倒数第二帧的状态进行预积分。