滑动窗口及边缘化直观理解

本文简要介绍VIO常用的滑动窗口及边缘化的原理，以及存在问题。

边缘化其实思想并不复杂，能够引入一定先验信息的同时，也会带来一些弊端，本文以简单示例，附加简单清晰的原理讲解滑动窗口及边缘化的原理。

滑动窗口是一种动态管理优化问题规模的技术，旨在维持实时性。其核心思想是仅保留最近一段时间内的状态变量（如相机位姿、路标点、IMU参数等），移除旧状态以避免计算量无限增长。

• 窗口更新策略：

  • 新增状态：当新数据（如图像、IMU测量）到达时，将新状态（如位姿 $T_{k+1}$、路标点 $p_{new}$）加入窗口。
  • 移除旧状态：当窗口容量达到上限时，按特定规则（如时间顺序或信息量评估）选择旧状态进行边缘化。

• 窗口大小选择：

  • 静态窗口：固定大小（如10帧），适合计算资源受限场景。
  • 动态窗口：根据场景复杂度或运动速度调整大小（如运动剧烈时增大窗口）。

问题例子example

设一个观测系统，对应的图模型如下：

图中：
圆圈：表示顶点，需要估计的变量。即该图模型中，需要求解的
连线：表示关系边，表示各个顶点(待求解变量)之间的观测约束(残差)。

根据该图模型建立的最小二乘问题如下：

$$\mathbf{\xi }=\underset{\mathbf{\xi }}{\mathrm{argmin}}\frac{1}{2}\sum _i\Vert {\mathbf{r}}_i{\Vert }_{{\mathbf{\Sigma }}_i}^2$$
其中：
$$\mathbf{\xi }=\left[\begin{array}{c}{\mathbf{\xi }}_1\\ {\mathbf{\xi }}_2\\ ...\\ {\mathbf{\xi }}_6\end{array}\right],\mathbf{r}=\left[\begin{array}{c}{\mathbf{r}}_{12}\\ {\mathbf{r}}_{13}\\ {\mathbf{r}}_{14}\\ {\mathbf{r}}_{15}\\ {\mathbf{r}}_{56}\end{array}\right]$$

$\mathbf{\xi }$表示待求解的变量，对应图中的圆圈节点，图中共6个节点。
$\mathbf{r}$表示观测约束，下标表示产生联系的两个节点。图中共有5个观测。

求解思路

对于上述最小二乘问题，使用高斯牛顿法进行迭代求解时，其变量更新求解公式如下：
$$\underset{\mathrm{H}or\mathbf{\Lambda }}{\underbrace{{\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{J}}}\delta \mathbf{\xi }=\underset{\mathbf{b}}{\underbrace{-{\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{r}}}\text{\hspace{1em}(1)}$$

$\mathbf{J}$魏雅可比矩阵。雅可比矩阵行与观测数(边线约束)相关，列与求解变量数相关。

$$\mathbf{J}=\frac{\partial \mathbf{r}}{\partial \mathbf{\xi }}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{12}}{\partial \mathbf{\xi }}\\ \frac{\partial {\mathbf{r}}_{13}}{\partial \mathbf{\xi }}\\ \frac{\partial {\mathbf{r}}_{14}}{\partial \mathbf{\xi }}\\ \frac{\partial {\mathbf{r}}_{15}}{\partial \mathbf{\xi }}\\ \frac{\partial {\mathbf{r}}_{56}}{\partial \mathbf{\xi }}\end{array}\right]=\left[\begin{array}{c}{\mathbf{J}}_1\\ {\mathbf{J}}_2\\ {\mathbf{J}}_3\\ {\mathbf{J}}_4\\ {\mathbf{J}}_5\end{array}\right],{\mathbf{J}}^{\top }=\left[\begin{array}{ccccc}{\mathbf{J}}_1^{\top }& {\mathbf{J}}_2^{\top }& {\mathbf{J}}_3^{\top }& {\mathbf{J}}_4^{\top }& {\mathbf{J}}_5^{\top }\end{array}\right]$$
将上述雅可比矩阵带入公式$(1)$可得参数更新的残差累加形式：
$$\sum _{i=1}^5{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{J}}_i\delta \mathbf{\xi }=-\sum _{i=1}^5{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{r}}_i$$

由于一个观测约束通常几个(通常为2个)状态量，如在某帧相机下对一个特征点的观测就是一个约束，这个约束涉及到的变量为一个特征点和一个相机位姿。从而雅可比矩阵通常是稀疏矩阵，无关的节点对应项为0。
如：
$$\begin{array}{rl}& {\mathbf{J}}_2=\frac{\partial {\mathbf{r}}_{13}}{\partial \mathbf{\xi }}=\left[\begin{array}{cccccc}\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_1}& 0& \frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_3}& 0& 0& 0\end{array}\right]\\ & {\mathbf{H}}_2={\mathbf{J}}_2^{\top }{\mathbf{\Sigma }}_2^{-1}{\mathbf{J}}_2=\left[\begin{array}{cccccc}(\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_1}{)}^{\top }{\mathbf{\Sigma }}_2^{-1}\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_1}& 0& (\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_1}{)}^{\top }{\mathbf{\Sigma }}_2^{-1}\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_3}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ (\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_3}{)}^{\top }{\mathbf{\Sigma }}_2^{-1}\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_1}& 0& (\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_3}{)}^{\top }{\mathbf{\Sigma }}_2^{-1}\frac{\partial {\mathbf{r}}_{13}}{\partial {\mathbf{\xi }}_3}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\end{array}$$

同理，可以得知${\mathbf{H}}_1,{\mathbf{H}}_3,{\mathbf{H}}_4,{\mathbf{H}}_5$也是稀疏的。

进一步，将五个残差的信息矩阵加起来，得到$\mathbf{H}={\sum }_{i=1}^5{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{J}}_i\delta \mathbf{\xi }$，对应的$b={\sum }_{i=1}^5{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{r}}_i$该矩阵的稀疏性如下图所示。

边缘化

滑动窗口方法会仅保留一定数量的最近帧（通常是当前帧和若干个前一帧），而将较旧的帧从优化中移除。这样做的目的是减少计算负担，保持实时性能，同时避免过度积累历史信息。
边缘化（Marginalization）是与滑动窗口密切相关的技术，主要用于处理窗口内移除的旧帧或者不再重要的路标点。在SLAM中，当一个变量被从滑动窗口中移除时，其状态信息不能完全丢弃，而是通过边缘化技术将其状态融合到其他变量中，以保留系统的长期一致性。边缘化通过数学手段对被移除的帧进行降维处理，减少对计算资源的需求，同时确保优化问题的解仍然尽可能准确。

边缘化原理

边缘化即将旧的变量移出窗口状态有两种理解方法，一种可以从概率的角度去分析，参考链接。另一种是基于Schur补的思想。参考链接。

以下从schur补来说明边缘化的原理：即通过消元法将需要marg的变量抹除。
设滑窗中的优化变量为$\xi =[{\xi }_m,{\xi }_r{]}^{\mathrm{T}}$，对应的Hessian矩阵$H$和雅可比矩阵$J$如下
$$H=\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]，b=\left[\begin{array}{c}{J}_m\\ J_r\end{array}\right]$$

使用迭代法计算参数更新值$\xi =[{\xi }_m,{\xi }_r{]}^{\mathrm{T}}$时，（牛顿法为例），计算公式如下：
$$\Delta \xi =-H^{-1}b$$

设${\xi }_m$为待marg的变量，利用消元法消去上式中的${\xi }_m$，则可以得到：
$$H_p=H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}$$$$b_p=b_r-H_{rm}{H}_{mm}^{-1}{b}_m$$

在上述的问题例子中marg掉变量${\xi }_1$，使用上述的marg公式的示意图如下：

SLAM中的Hessian矩阵通常比较稀疏，这是由于通常通常一帧的观测对应很多的观测点。Hessian矩阵中元素的0或非0表征者各个变量(元素)之间的联系与否。

看到经过marg边缘化之后，原先稀疏的Hessian矩阵变得稠密，从而表明。marg边缘化会使得原先没有联系的变量发生联系。

当有新的变量${\xi }_7$进入系统，与变量${\xi }_2$发生观测联系，如下示意图：

将新的观测与变量加入Hessian矩阵。首先将marg后得到的Hessian矩阵$H_p$进行扩容，而后加入新的变量及约束构成的Hessian矩阵$H_7$，如下示意图。

边缘化的实际步骤

在非线性优化中，边缘化需处理线性化点固定问题，具体步骤如下：

1. 构建增量方程：
   在当前线性化点（如 ${\xi }_0$）处计算残差的雅可比矩阵 $J$ 和 Hessian 矩阵 $H=J^{T}J$。

2. 选择边缘化变量：
   确定要移除的变量 ${\xi }_m$（如窗口中最旧的位姿 $T_1$ 及其关联的路标点）。

3. 舒尔补操作：
   对 $H$ 和 $b$ 进行分块，计算先验信息 $H_{\text{prior}}$ 和 $b_{\text{prior}}$。

4. 构造先验残差：
   将先验信息表示为虚拟残差项 $r_{\text{p}}$，其对应的优化问题为：
   $$\underset{\Delta {\xi }_r}{\min }{\Vert r_{\text{p}}+J_{\text{p}}(\xi -{\xi }_p)\Vert }^2$$

5. 信息矩阵叠加：
   边缘化后的先验信息作为新的约束加入后续优化中：

• 后续优化问题的 Hessian 矩阵为：
  $$H_{\text{total}}=H_{\text{new}}+H_{\text{p}}$$

• 误差向量叠加：
  $$b_{\text{total}}=b_{\text{new}}+b_{\text{p}}$$

marg先验约束公式

先验约束公式1

marg后得到的先验信息约束方程可以等价为如下公式：
$$e_p=||r_p+J_p(\xi -{\xi }_p)||\text{\hspace{1em}(2)}$$
注意：这里的$J_p$及$r_p$等均是由$H_p$及$b_p$中反构造出的常参数，通常在边缘化迭代中不需要计算，这里的公式是用于说明边缘化的数学约束意义。
$H_p=J_p^T{J}_p$，${\xi }_p$为在新的数据进入之前，优化计算得到的变量值。$r_p$代指加入新数据之前，迭代优化最后剩余的残差。继而可得其对应的迭代公式：$$b_p=J_p^{\mathrm{T}}(r_p+J_p(\xi -{\xi }_p))，H_p=J_p^T{J}_p，\Delta {\xi }_p=-H_p^{-1}{b}_p$$
marg后的第一次迭代中，$\xi ={\xi }_p,b_p=J_p^{\mathrm{T}}{r}_p$。

通常在marg后会加入其他信息到hessian矩阵及$b$中，综合求解新的变量更新值$\Delta \xi$。注意这里$\Delta \xi$表示综合新的信息算出的更新值，而$\Delta {\xi }_p$为使用先验约束算出的变量更新量。

在后续迭代时，变量更新公式：$\xi \leftarrow \xi +\Delta \xi$。

在变量marg之后，marg得到的稠密先验矩阵$H_p$随着变量的变化不再能更新，而其偏置量$b_p$可以使用如下公式进行更新：
将$b_p^{k+1}$在${\xi }_p$处求偏导：
$$\begin{gathered} b_p^{k+1}={b_p^k+\frac{\partial b_p^k}{\partial \xi }|}_{{\xi }_p}\delta \xi \\ =b_p+{\frac{\partial J_p^{\mathrm{T}}(r_p+J_p(\xi -{\xi }_p))}{\partial \xi }|}_{{\xi }_p}\delta \xi \\ =b_p+\frac{\partial J_p^{\mathrm{T}}{r}_p+\partial H_p(\xi -{\xi }_p)}{\partial \xi }\delta \xi \\ =b_p+H_p\delta \xi \\ =b_p+H_p({\xi }^{k+1}-{\xi }_p) \end{gathered}$$

也可以使用推导得到偏置的更新公式。
$$b_p^{k+1}=J_p^{\mathrm{T}}(r_p+J_p({\xi }^k+\Delta \xi -{\xi }_p))=b_p+H_p({\xi }^{k+1}-{\xi }_p)$$
进一步的先验更新公式：
$$\begin{gathered} \Delta {\xi }_p^{k+1}=-H_p^{-1}(b_p+H_p({\xi }^{k+1}-{\xi }_p)) \\ =-H_p^{-1}{b}_p-({\xi }^{k+1}-{\xi }_p)) \end{gathered}$$

先验约束公式2

上述约束可以进一步转化为：
$$e_p^{\prime }=||\xi -({\xi }_p-H_p^{-1}{b}_p)||\text{\hspace{1em}(3)}$$
其中，$b_p=J_p^{\mathrm{T}}{r}_p$。此时，该约束方程对应的雅可比矩阵等等如下：
$$J_p^{\prime }=I,H_p^{\prime }=I,b_p^{\prime }=J_p^{\prime \mathrm{T}}[\xi -({\xi }_p-H_p^{-1}{b}_p)]=\xi -({\xi }_p-H_p^{-1}{b}_p)$$
同样地，marg后第一次迭代，$\xi ={\xi }_p,b_p^{\prime }=H_p^{-1}{b}_p$
$$\Delta {\xi }_p=-I\cdot b_p^{\prime }=-H_p^{-1}{b}_p$$

后续随着变量$\xi$的更新，Hessian阵$H_p^{\prime }$为单位矩阵，不参与优化。偏置$b_p^{\prime }$的更新公式如下：
$$\Delta b_p^{\prime }=\Delta \xi$$

进一步的先验更新公式：
$$\begin{gathered} \Delta {\xi }_p^{k+1^{\prime }}=-I({\xi }^{k+1}-({\xi }_p-H_p^{-1}{b}_p)) \\ =-H_p^{-1}{b}_p-({\xi }^{k+1}-{\xi }_p)) \end{gathered}$$

从而我们得到同样的变量更新结果。从而可知这两个先验约束方程（2）和（3）等价。

由（3）我们可以很清楚地看出，marg后的先验约束，即约束变量维持$\xi =({\xi }_p+H_p^{-1}{b}_p)$，即将marg之前算得的变量值，作为一种约束。在加入后续新的观测时，约束变量在marg之前算出的结果附近。

marg的问题及FEJ

边缘化带来的两个挑战：

• Hessian矩阵变稠密
  首先。marg会将Hessian矩阵变的稠密，而信息矩阵的稀疏性，使得使用回代法求解变量更新值时会带来一定的便利，如方法SAM。

• 变量更新的线性化点不一致的问题
  marg后得到的先验信息矩阵包含了已经被marg掉的变量节点与窗口现有节点之间的约束关系，当窗口现有节点更新时，原本先验信息矩阵与现有节点相关的约束也应该进行更新。
  而，使用marg在迭代优化算法中，marg后得到Hessian矩阵在新的迭代过程中不能再被优化，即图中的紫蓝色部分固定。而新的观测对应信息矩阵，红色部分，会在后续的迭代过程中进一步被优化。从而出现部分变量线性化点不一致的情况，从而使得解空间发生变化。

如本例中将变量${\xi }_1$边缘化，而窗口中仍存在与${\xi }_1$相关的其他变量（本例中为${\xi }_2,{\xi }_3,{\xi }_5,{\xi }_5$），如下图所示。

• 红色为被 marg 变量以及测量约束。
• 绿色为跟 marg 变量有关的保留变量。
• 蓝色为和 marg 变量无关联的变量。

当${\xi }_2,{\xi }_3,{\xi }_5,{\xi }_5$这些变量被进一步优化时，图中原本它们与${\xi }_1$构成的约束（图中红线）对应的数据对应的线性化点也应该进行更新，但是由于边缘化后，Hessian矩阵的更新变的非常复杂。而新的变量${\xi }_7$带来的与${\xi }_2$构建的约束能够随着${\xi }_2$的更新而更新。从而使得先验对于${\xi }_2$的求导位置与新的约束对于${\xi }_2$的求导位置不一致。从而使得${\xi }_2$的解偏离真正的解。

这点也可以，从约束方程（3）中进行具体理解，即先验约束即使在得到新的约束方程后，会约束变量保持先验得到的变量结果，而所有变量的估计是不可能不存在误差的，因而先验得到的变量值也存在误差，进而进一步影响最后的变量估计，这个误差是随着时间累积的，即使有更多的观测约束，也消弭不了。最终滑动窗口+边缘化会表现出随着时间累积而误差累积的情况。

解决方案：FEJ（First Estimate Jacobians）强制所有涉及 ${\xi }_m$ 的雅可比在首次线性化点 ${\xi }_0$ 处计算。但此方法需要${\xi }_0$本身接近于真实值（或者与后续迭代过程中的实际变量值（本该的线性化点）差距不大），否则会影响迭代速度，甚至解算失误的问题。从而该方法也会由此引入新累积误差。

实例分析：VINS-Mono中的滑动窗口策略

• 窗口结构：包含10个关键帧及其关联的路标点、IMU预积分。
• 边缘化规则：
  • 当新帧加入时，若窗口已满，边缘化最旧帧及其观测的路标点。
  • 若最旧帧为关键帧，保留其观测的路标点；否则直接丢弃。
• FEJ应用：所有被边缘化的路标点的雅可比矩阵固定在首次观测时的位姿处计算。

边缘化的代码实现（伪代码）

// 滑动窗口管理
void SlidingWindow::addNewFrame(Frame& new_frame) {
    window.push_back(new_frame);
    if (window.size() > WINDOW_SIZE) {
        Frame old_frame = window.front();
        window.pop_front();
        marginalize(old_frame); // 执行边缘化
    }
}

// 边缘化操作
void marginalize(Frame& old_frame) {
    // 1. 构建Hessian矩阵和误差向量
    MatrixXd H;
    VectorXd b;
    buildHessian(old_frame, H, b);

    // 2. 分块：保留变量ξ_r 和边缘化变量ξ_m
    MatrixXd H_rr = H.block(r_indices, r_indices);
    MatrixXd H_rm = H.block(r_indices, m_indices);
    MatrixXd H_mm = H.block(m_indices, m_indices);
    VectorXd b_r = b.segment(r_indices);
    VectorXd b_m = b.segment(m_indices);

    // 3. 舒尔补计算先验信息
    MatrixXd H_prior = H_rr - H_rm * H_mm.inverse() * H_rm.transpose();
    VectorXd b_prior = b_r - H_rm * H_mm.inverse() * b_m;

    // 4. 将先验信息添加到后续优化问题
    optimizer.addPriorConstraint(H_prior, b_prior);
}

• 滑动窗口：通过动态管理状态变量控制计算复杂度。
• 边缘化：将移除变量的影响转化为先验约束，避免信息丢失。
• 舒尔补：数学工具实现信息矩阵的压缩。
• FEJ：解决线性化点不一致，减少误差累积。

参考

《手写VIO 贺一家，高翔，崔华坤》