sliding window："Marginalization"，"Schur complement"，"First estimate jacobin"

[1]知行合一2

SLAM中的marginalization 和 Schur complement

SLAM的Bundle Adjustment上，随着时间的推移，路标特征点(landmark)和相机的位姿pose越来越多，BA的计算量随着变量的增加而增加，即使BA的H矩阵是稀疏的，也吃不消。因此，我们要限制优化变量的多少，不能只一味的增加待优化的变量到BA里，而应该去掉一些变量。那么如何丢变量就成了一个很重要的问题！比如有frame1,frame2,frame3 以及这些frame上的特征点pt1…ptn。新来了一个frame4，为了不再增加BA时的变量，出现在脑海里的直接做法是把frame1以及相关特征点pt直接丢弃，只优化frame2,frame3,frame4及相应特征点。然而，这种做法好吗？

Gabe Sibley [2]在他们的论文中就明确的说明了这个问题。直接丢掉变量，就导致损失了信息，frame1可能能更多的约束相邻的frame，直接丢掉的方式就破坏了这些约束。在SLAM中，一般概率模型都是建模成高斯分布，如相机的位姿都是一个高斯分布，轨迹和特征点形成了一个多元高斯分布p(x1,x2,x3,pt1…)，然后图优化或者BA就从一个概率问题变成一个最小二乘问题。因此，从这个多元高斯分布中去掉一个变量的正确做法是把他从这个多元高斯分布中marginalize out.

这marginalize out具体该如何操作呢？Sliding widow Filter [2]中只是简单的一句应用Schur complement（舍尔补）. 我们知道SLAM中的图优化和BA都是最小二乘问题，如下图所示(ref.[1])

构建出来的Hx=b是利用了marg变量的信息，也就是说我们没有人为的丢弃约束，所以不会丢失信息，但是计算结果的时候，我们只去更新了我们希望保留的那些变量的值。在slam的过程中，BA不断地加入新的待优化的变量，并marg旧的变量，从而使得计算量维持在一定水平。

marginalize的本意是:只更新部分变量，而不是所有变量。

一句话总结如下：要把一部分变量从多元高斯分布从分离出来，需要把协方差矩阵也给分开，然而协方差矩阵大家都知道，众多变量之间息息相关，不能简单地说协方差矩阵的哪一块就是谁的，需要用到schur complement （舍尔补）来分割。

[2]VINS-Mono代码分析总结

4.4 系统边缘化

4.4.1 边缘化的定义和目的

边缘化(marginalization)的过程就是将滑窗内的某些较旧或者不满足要求的视觉帧剔除的过程，所以边缘化也被描述为将联合概率分布分解为边缘概率分布和条件概率分布的过程(说白了，就是利用shur补减少优化参数的过程)。

利用Sliding Window做优化的过程中，边缘化的目的主要有两个：

滑窗内的pose和feature个数是有限的，在系统优化的过程中，势必要不断将一些pose和feature移除滑窗。
如果当前帧图像和上一帧添加到滑窗的图像帧视差很小，则测量的协方差(重投影误差)会很大，进而会恶化优化结果。LIFO导致了协方差的增大，而恶化优化结果。

直接进行边缘化而不加入先验条件的后果：

无故地移除这些pose和feature会丢弃帧间约束，会降低了优化器的精度，所以在移除pose和feature的时候需要将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中。
在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失(参考[6])，尤其是系统在进行退化运动时(如无人机的悬停和恒速运动)。一般来说只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的客观性。

4.4.2 两种边缘化措施

这两种边缘化的措施主要还是针对悬停和恒速运动等退化运动。

4.4.3 舒尔补边缘化优化状态量

4.4.5 滑窗优化(Slide window)

按理说是不应该把滑窗当做一小节来讲的，边缘化、舒尔补都属于滑窗的范围，但前面已经总结了。

而且正如贺一加师兄在其博客中提到的，滑窗的三大法宝"Marginalization"，"Schur complement"，"First estimate jacobin"，这些在前面也提到了。

关于这部分内容，可以参考这几篇博客，讲的真是太好了。okvis1，okvis2，知行合一1，知行合一2