边缘化(Marginalization)

一个实时SLAM估计器不可能把过去所有的位姿和地图点都作为活跃变量保留下来——那样问题规模会无限增长。边缘化是移除旧变量的、在概率意义上正确的方式:不是简单地删除它们(这会丢弃它们所携带的信息),而是将它们从联合分布中积分消去,在剩余变量上留下一个先验(prior),用来概括被移除变量所贡献的信息。

代数原理:Schur补

将线性化后的系统划分为要保留的变量( $\mathbf{k}$ )和要边缘化的变量( $\mathbf{m}$ ):

\begin{bmatrix} H_{kk} & H_{km} \\ H_{mk} & H_{mm} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{x}_k \\ \Delta\mathbf{x}_m \end{bmatrix} = \begin{bmatrix} \mathbf{b}_k \\ \mathbf{b}_m \end{bmatrix}

消去 $\mathbf{m}$ 得到缩减后的系统:

H_{\text{prior}} = H_{kk} - H_{km} H_{mm}^{-1} H_{mk}, \qquad \mathbf{b}_{\text{prior}} = \mathbf{b}_k - H_{km} H_{mm}^{-1} \mathbf{b}_m

$(H_{\text{prior}}, \mathbf{b}_{\text{prior}})$ 随后在所有后续优化中,作为一个稠密的先验因子作用于被保留的变量。

这一结果从何而来?对于用信息形式表示的联合高斯分布(信息矩阵 $H$ 、信息向量 $\mathbf{b}$ ),将其中一部分变量积分消去,得到的仍是一个高斯分布,其信息矩阵恰好就是上面的Schur补。因此,边缘化在代数上与求解稀疏系统时的一步变量消元完全相同——唯一的区别在于意图:消元是临时的(该变量在回代时还会出现),而边缘化是永久性的。

代价所在:填充(fill-in)

信息被保留下来了,但稀疏性却没有——边缘化会使被移除变量所连接的那些变量之间的联系变得稠密。举一个小例子:在一个窗口 $x_0, x_1, x_2, x_3$ 中,若地图点观测将 $x_0$ 与若干同样被 $x_1$ 、 $x_2$ 观测到的地图点相连,那么边缘化 $x_0$ 及其独占的地图点,会产生一个单一的、耦合 $x_1$ 和 $x_2$ (以及 $x_0$ 所触及的其他任何变量)的稠密先验——而这些变量对之间原本并没有直接的边。这正是为什么系统会谨慎地选择要边缘化的变量(旧的关键帧及其独占的地图点),而不是随意选取;也正是为什么一些系统会故意在边缘化之前丢弃若干测量值(类似OKVIS的稀疏化做法)——牺牲一点信息,以避免整个窗口的先验变得过度稠密。

固定滞后平滑(Fixed-lag smoothing)

固定滞后平滑是建立在这一操作之上的估计器架构:维护一个由最新状态组成的滑动窗口作为活跃的非线性变量;当窗口已满时,把最旧的状态边缘化进先验中。这样每帧的计算量就有了上限,同时又保留了(线性化后概括的)全部历史信息——这是从OKVIS以来基于优化的VIO的标准设计。例如VINS-Mono,会根据新进帧是否为关键帧,决定边缘化最旧帧还是次新帧。

隐患:一致性(consistency)

先验是在边缘化发生时所处的估计值处线性化的,之后无法再重新线性化(被边缘化的变量已经不存在了)。如果剩余变量的线性化点随后发生了漂移,而被冻结的先验却原地不动,估计器就会混用来自不同线性化点的雅可比矩阵,从而获得虚假的信息量——这就是所谓的”FEJ问题”。标准的缓解手段包括:

首次估计雅可比(First-Estimate Jacobians, FEJ)——对与先验相连的变量,始终在其首次估计处求雅可比,使所有信息沿一致的方向进入系统。
平方根/Cholesky形式的先验,以获得数值稳定性(如Basalt所采用)。
延迟边缘化(DM-VIO)——推迟冻结的时刻,让图中更多部分保持可重新线性化的状态。
非线性因子恢复(Basalt)——用一组恢复出的非线性因子来近似替代被冻结的线性先验。

实践清单

绝不重复使用已经被吸收进先验中的测量值——那会造成信息的重复计数。
应将仅被即将离开的关键帧观测到的地图点与该关键帧一起边缘化;仍然可见的地图点则应改为丢弃其旧观测,或通过稀疏化处理。
关注先验的条件数:病态的 $H_{mm}$ 会使 $H_{mm}^{-1}$ 爆炸;平方根形式和谨慎的变量缩放能有所帮助。
如果类似NEES的一致性检验只在长时间运行后才失败,边缘化/FEJ流程是首要的嫌疑对象。

与另一种后端风格作对比:增量平滑(iSAM2)保留全部变量并巧妙地进行更新,以问题规模的(尽管被高效维护的)持续增长为代价,避免了冻结先验的问题。固定滞后+边缘化换来的是严格有界的计算代价——这对嵌入式VIO而言是正确的取舍。

对SLAM的意义

边缘化是使实时、内存受限的视觉惯性里程计成为可能的核心机制;滑动窗口+Schur补先验+FEJ的模式正是OKVIS、VINS-Mono、Basalt和DM-VIO的字面架构。它也是理论在实践中最容易出问题的地方:处理不当的边缘化是估计器过度自信、结果不一致的经典成因,这正是该概念与一致性、可观测性紧密关联的原因。