1.SLAM中的高博高博滑窗（Sliding window）

SLAM中的滑窗（Sliding window）

       本文按照以下顺序组织

       1. BA回顾

       1.1 问题构建

       以重投影误差为例，设[公式] 为在位姿 [公式] 处观测到路标点 [公式] 产生的源码数据，则整体的高博高博代价函数（Cost Function）为[1]：[公式] 将自变量定义成所有待优化的变量：[公式] 给代价函数中的自变量一个增量[公式] ，并对目标函数线性化，源码可以得到：[公式] 其中，高博高博[公式] 表示整个代价函数在当前状态下对相机姿态的源码rumi策略源码偏导数，而 [公式] 表示整个代价函数在当前状态对路标点的高博高博偏导。而 [公式] 和 [公式] 则是源码整体代价函数对整体变量的导数，它是高博高博由每个误差项的导数 [公式] 和 [公式] “拼凑”起来的。然后就是源码解增量方程：[公式] 以GN方法为例，[公式]，高博高博然后 [公式] 矩阵为：[公式] 1.2 问题求解 由于代价函数中的源码误差项[公式] 只描述了在 [公式] 看到 [公式] 这件事，所以该误差项对应的高博高博雅可比具有特殊的形式：[公式] 导致[公式] 具有一定的稀疏性。 [公式] 最终结构如图1所示。源码 于是高博高博可以将[公式] 分成四块，对应的线性方程组也可以由（3）变为下式：[公式] 使用边缘化（Marginalization）可以加速求解（4）的过程：[公式] 整理得：[公式] 这样，方程组第一行变成了与[公式] 无关的项，因此可以先求解第一行的热血军团源码增量方程，然后再代入（6）中反求第二行的增量方程。其中， [公式] 就称为 [公式] 在 [公式] 中的舒尔补（Schur complement）。

       2. 为什么需要滑窗

       在SLAM过程中，随着关键帧和路标点的增多，后端BA的计算效率会不断下降。为了避免这种情况，便使用了滑窗（Sliding window）将待优化的关键帧限制在一定的数量来控制BA的规模。

       3. 滑窗会带来什么问题

       区别于全局BA，滑窗的窗口结构变化可以分为以下两部分讨论[1]： 新增关键帧的操作和全局BA相同，不再赘述。下面讨论在窗口中删除一个旧的关键帧。 首先要明确的是，在窗口中删除关键帧的技术并不是字面上的直接删除，而是前面提到的边缘化，因为直接丢掉变量，就导致损失了信息，限时秒杀 源码关键帧1可能能更多地约束相邻的关键帧，直接丢掉的方式就破坏了这些约束。但是边缘化也带来了一些问题：首先，边缘化会将被边缘化掉的变量所带有的信息转化为先验增加到H矩阵中（Fill-in），这样会破坏BA的稀疏结构；而且，在滑动窗口中使用标准的边缘化操作会导致在新窗口中，与被边缘化的变量相关的变量会在不同的线性化点计算雅可比（因为每次BA的线性化点不同），破坏了系统的一致性。

       3.1 Fill-in问题[2][3] 无论是全局BA还是滑窗优化，在进行舒尔消元后，[公式] 矩阵不可避免地发生了改变，以边缘化掉某些变量。如在（6）中，舒尔消元后求解 [公式] 后会代回（5）中求解 [公式] ，因此舒尔消元操作在下一次线性化前并没有引入先验信息。但在窗口优化中，被边缘化掉变量在后续不会再进行更新，dpdk 源码分析因此会对相关块 Fill-in，产生先验信息，所以需要额外的操作处理先验块。

       3.2 一致性问题[2][4][5] 四张能量图中，第一张是说明能量函数E由两个同样的非线性函数E1 和E2 组成，我们令函数E=0，这时方程的解为xy=1xy=1，对应图中深蓝色的一条曲线。第二张能量函数图中的E1′​对应函数E1​在点(0.5,1.4)处的二阶泰勒展开，第三张能量函数图中的E2′​对应函数E2​在点(1.2,0.5)处的二阶泰勒展开。注意这两个近似的能量函数E1′​和E2′​是在不同的线性点附近对原函数展开得到的。最后一张图就是把这个近似得到的能量函数合并起来，对整个系统E的二阶近似。从第四个能量函数图中，我们发现一个大问题，能量函数为0的解由以前的一条曲线变成了一个点，不确定性的纪委举报源码东西变得确定了，专业的术语叫不可观的状态变量变得可观了，说明我们人为的引入了错误的信息。

       假设窗口中要边缘化掉的变量为[公式] ，与 [公式] 有约束关系的变量为 [公式] ，其余变量为 [公式] 。则边缘化前窗口中的变量分为三部分 [公式] 。在边缘化 [公式] 后， [公式] 与 [公式] 的约束关系转化为先验添加到 [公式] 中（可以看作是求H矩阵时利用到了 [公式] 的信息，然后求出来的增量带有先验信息），[公式] 产生了一些偏移变成了 [公式]，则边缘化后的窗口为 [公式] 。那么问题就来了，边缘化前我们是在 [公式] 这一状态下线性化。但是边缘化之后，我们的状态变成了 [公式] 。如果这时候再对 [公式] 线性化，会导致 [公式] 和 [公式] 的线性化点不一致，引入错误信息。

       总的来说，在滑窗边缘化过程中，需要不断迭代计算H矩阵和残差b，而迭代过程中，在窗口中状态变量会被不断更新。但是对于相同状态的两个不同估计被用来计算估计器的雅可比矩阵导致了估计的协方差矩阵掺入了人为的信息，降低了估计的精度，最终导致一致性问题。详细的推导可以参考[4]。

       4. 如何解决上述问题

       4.1 谨慎选择边缘化的变量 由于边缘化一个变量仅会对与它直接相关变量所在的行列Fill-in，所以我们需要慎重地选择需要边缘化的变量，以保持[公式] 矩阵的稀疏性。在[3]中高博提到：对被边缘化的关键帧x1观测到的y1-y4，存在以下几种情况： a. 路标只在x1中观测到，在其余帧中都不出现。那么可以直接丢掉这个路标，不对窗口产生任何影响。这个路标是孤立的。 b. 路标在x2-x4中看到，但是在未来看不到。（这里只是这样假设，实际取决于前端的实现方式。像vins那种光流前端不会追踪已经丢失的特征点，所以你可以这样子假设。）那么可以把这个路标也同时边缘化。路标边缘化时会产生Pose-Pose部分的先验，所以变成了Pose部分的先验信息。 c. 路标在x2-x4中看到，并且在将来也可能看到。那么这个路标不应该被边缘化，因为我们之后还需要更新它的估计值。此时理论上我们只能保持Landmark-Landmark当中的稠密结构，但是在工程中，比方说在[2]里，作者认为可以简单地丢掉x1对这个路标的观测（相当于认为x1没看到它）。于是我们就以较小的代价保持了Landmark部分的对角结构。这种情况下你什么都不用干。 作者：半闲居士 链接： zhihu.com/question/... 来源：知乎 著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

       4.2 使用FEJ（First Esitmate Jacobians）保持一致性[6][7][8] 既然在边缘化过程，初始化点会变，那么就固定它来求雅可比矩阵，即：对全部将被边缘化掉的变量和边缘化变量牵扯到的变量，只在迭代的最开始计算一次雅各比矩阵，后续边缘化的时候都用这个雅各比矩阵，不再会因变量值改变重新计算雅各比矩阵。为什么FEJ可以保持一致性？贺博在[6]中理论地推导了一遍：评论处有问题。Xm对应要删去的变量，Xb对应和Xm有直接联系的变量，Xr对应剩余的变量。Zr对应的残差中有约束Xb和Xr的，比如上面的Z。在滑窗优化时，先验部分对Xb的雅克比是固定的，那Zr部分对Xb的雅克比是不是也要固定？并且取相同的点？或者上面的公式c(x) = cm(x) + cr(x)中，cm(x)是先验约束，cr(x)是投影残差约束，cm(x)会产生Xb的雅克比， cr(x)也会有Xb的雅克比，是不是这两部分的雅克比都要固定，并且是同一个线性化点？而贺博也给出了自己的回答：很好的问题，这个问题我跟若干个人讨论个过，大家有不同的理解。之前大家都倾向于所有涉及xb的地方都固定，包括zr部分。但是okvis和vins的代码中貌似只需要固定先验部分对xb的雅克比，zr部分不需要固定。okvis的代码我没看（别人看了），vins中只固定了先验部分，zr部分没固定。具体的深入验证可能需要自己写代码进行验证。另外OKVIS中的FEJ的实现可以参考[7]和[8]，VINS-Mono代码分析总结[9]中也提到了FEJ。

       补充 边缘化来自高斯推断[]得到的条件概率和边缘概率。具体推导见图5。[4]中补充了一致性的定义：A recursive estimator is termed consistent when the state estimation errors are zero mean, and their covariance equals the one reported by the estimator [, Section 5.4]