边缘化与 Schur 补：丢掉关键帧不等于丢掉约束

一套滑窗 VIO 在办公室数据上跑得很顺，窗口长度从 10 帧改到 15 帧后 ATE 还降了一点；换到车库长跑，半小时后回到起点却出现十几厘米的横向跳变。前端没有大面积跟丢，IMU 预积分残差也没有爆，最后查到的是边缘化先验：旧关键帧被移出窗口时，和它相连的视觉、IMU、外参约束并没有按同一套线性化契约压回剩余状态。系统表面上只是“丢掉一帧”，实际是在改写整张因子图的历史信息。

边缘化最容易被讲成内存管理技巧：窗口满了，删掉最老帧，留下一个先验因子。这个说法会掩盖两个工程事实。第一，删除变量不是删除测量；只要测量曾经同时约束被删变量与保留变量，它的信息就必须转移到保留变量上。第二，转移后的先验是线性化对象，不能像普通视觉因子那样随意重新解释。后端后续每一次 relinearize、重排状态、重开窗口，都要尊重这个先验形成时的坐标、残差方向和雅可比点。

1. 从整张图看滑窗

把滑窗里的状态写成 $x = x_{0}, \dots, x_{k}$ ，其中每个 $x_{i}$ 可能包含位姿、速度、IMU bias，也可能再挂外参、时间偏移和逆深度路标。因子图优化通常在当前线性化点 $x ˉ$ 附近求增量 $δ x$ ，把非线性残差一阶展开为

r (x) ≃ r (x ˉ) + J δ x .

加权最小二乘的正规方程是

H δ x = b, H = J^{T} W J, b = - J^{T} W r (x ˉ) .

当新关键帧进入窗口时，最老关键帧 $x_{m}$ 不能无限保留；但它参加过的因子并不只约束它自己。相邻 IMU 预积分因子连接 $x_{m}$ 与 $x_{m + 1}$ ，视觉重投影可能连接 $x_{m}$ 、路标和多个仍在窗口内的位姿，回环或 GNSS 因子还可能给它一个全局参考。若直接从状态向量中删掉 $x_{m}$ ，这些交叉信息会被截断，剩余状态的协方差会虚假变大；若把旧因子留着却不再优化 $x_{m}$ ，又会形成一个悬空变量。边缘化的目标，是在删变量前把它对剩余变量的影响条件化进去。

2. Schur 补到底保留了什么

按“保留变量” $x_{r}$ 与“待边缘化变量” $x_{m}$ 分块，线性化后的二次型可以写成

\frac{1}{2}\begin{bmatrix}\delta x_r\ \delta x_m\end{bmatrix}^{\mathsf T}\begin{bmatrix}H_{rr} & H_{rm}\H_{mr} & H_{mm}\end{bmatrix}\begin{bmatrix}\delta x_r\ \delta x_m\end{bmatrix}-\begin{bmatrix}b_r\ b_m\end{bmatrix}^{\mathsf T}\begin{bmatrix}\delta x_r\ \delta x_m\end{bmatrix}.

对固定的 $δ x_{r}$ ，最优的 $δ x_{m}$ 满足

H_{m r} δ x_{r} + H_{mm} δ x_{m} = b_{m},

因此

δ x_{m}^{⋆} = H_{mm}^{- 1} (b_{m} - H_{m r} δ x_{r}) .

把它代回二次型，就得到只含 $δ x_{r}$ 的先验：

H_{p} = H_{r r} - H_{r m} H_{mm}^{- 1} H_{m r}, \qquadb_{p} = b_{r} - H_{r m} H_{mm}^{- 1} b_{m} .

这就是 Schur 补。它不是近似地“记住一个均值”，而是在当前线性化点附近，把待删变量已经能解释掉的自由度消去，保留剩余变量之间新的耦合。原来 $x_{i}$ 与 $x_{j}$ 可能没有直接因子，只是都通过旧帧或旧路标相连；边缘化后， $H_{p}$ 会让它们在先验里直接相关。工程上看到先验矩阵变稠密，不是实现坏了，而是历史路径被压缩后的自然结果。

从概率角度看，若线性化后的系统近似为高斯信息形式 $Ω$ ，边缘化就是对联合分布 $p (x_{r}, x_{m})$ 积分掉 $x_{m}$ ，得到 $p (x_{r})$ 。对高斯分布，这个积分的结果仍是高斯，信息矩阵正是 Schur 补。也因此，边缘化不是“条件在旧变量等于某个值上”，而是把旧变量所有与剩余变量一致的可能性都吸收进 $x_{r}$ 的边缘分布。把这点想清楚，很多“固定旧帧姿态再优化”的捷径就会暴露问题：固定是条件化，边缘化是积分，二者给出的不确定性不同。

3. 一个三节点例子

假设一维轨迹有三个位姿 $x_{0}, x_{1}, x_{2}$ ，里程计只提供相对约束 $x_{1} - x_{0} = z_{01}$ 、 $x_{2} - x_{1} = z_{12}$ ，另有一个弱先验锚住 $x_{0}$ 。当窗口滑动到只保留 $x_{1}, x_{2}$ 时，若直接删 $x_{0}$ ， $x_{1}$ 的绝对参考随之变弱；若把 $x_{0}$ 固定在上次估计值， $x_{1}$ 会继承一个看似很硬的参考，却没有把 $x_{0}$ 的不确定性传递过来。

Schur 边缘化会产生一个关于 $x_{1}$ 的新先验。它的强度取决于旧锚点和 $z_{01}$ 的权重：旧锚点很弱时，新先验也弱；旧锚点很强、相对约束也强时，新先验才硬。这个结果符合物理直觉。被删的 $x_{0}$ 不是确定的墙钉，它只是曾经参与估计的随机变量。正确先验必须让这种不确定性继续影响 $x_{1}$ ，否则窗口越滑越容易过度自信。

多维 VIO 中同样如此。旧关键帧的 yaw、尺度、bias 与路标深度可能弱可观，Schur 补会把这些弱方向以小特征值形式留在先验里。若实现里为了 Cholesky 方便把小特征值直接 clamp 成固定阈值，系统会把弱可观误当成强约束；若完全丢掉交叉块 $H_{r m}$ ，又会漏掉旧变量对新变量的历史传递。两种错误在短序列上可能只差几毫米，长时间重访同一区域时会变成轨迹不闭合。

4. 先验因子不是普通因子

边缘化之后，后端通常把 $H_{p}, b_{p}$ 转成一个先验残差。常见做法是对信息矩阵做特征分解或 Cholesky，构造

∣ A δ x_{r} - e ∣^{2}, A^{T} A = H_{p}, A^{T} e = b_{p} .

先验残差的变量是“当前状态相对边缘化线性化点的局部增量”。这句话比矩阵公式更重要。若状态在下一轮优化中已经从 $x ˉ_{r}$ 移到 $x^_{r}$ ，先验不能简单改写成关于 $x^_{r}$ 的全新非线性测量；它只能通过定义好的 $⊟$ 或 retraction 计算 $δ x_{r} = x^_{r} ⊟ x ˉ_{r}$ ，再使用当时保存的 $A, e$ 。对 SE(3) 状态，还必须明确扰动在左还是右，切向量顺序是平移在前还是旋转在前。否则同一块 $H_{p}$ 会被套到另一套坐标上，数值仍能收敛，物理含义已经变了。

这也是边缘化代码看起来啰嗦的原因。它不仅存矩阵，还要存 parameter block 地址、维度、局部参数化、线性化点、丢弃变量列表和参与边缘化的因子快照。窗口滑动后，原来的内存地址可能复用给新帧；若只按指针匹配变量，先验会悄悄绑到错误状态上。稳妥实现会用稳定的状态 id 建映射，重排矩阵时显式维护 block 顺序，并在每次 marginalize 后检查先验维度与剩余状态一致。

5. 固定先验：简单但容易变硬

固定先验指把边缘化时得到的 $H_{p}, b_{p}$ 和线性化点永久保存，后续不再根据新估计重新线性化旧测量。它的优点直接：实现清楚，计算量稳定，符合“旧变量已经离开窗口”的事实。VIO 系统里大量工程代码都采用这个思路，因为重新访问所有历史因子会破坏滑窗的复杂度边界。

风险在于非线性。先验只在 $x ˉ_{r}$ 附近是一阶有效的；当保留状态被后续新观测拉得很远，旧先验仍沿着原来的切空间施加约束。小运动、短窗口、IMU 质量好时，这个误差可接受；急转弯、弱纹理、时间同步偏差或外参在线估计时，先验可能成为一块越来越不合时宜的硬板。它不是随机噪声，而是系统性线性化误差，常表现为优化残差不大、协方差很小、轨迹却在全局比较中慢慢弯掉。

固定先验的工程边界应写进代码注释和日志：当状态相对先验线性化点的姿态差、位置差或 bias 差超过阈值时，至少打 warning；若系统支持重定位或闭环大修正，应考虑丢弃局部先验并重建窗口，而不是让一个远离有效域的先验继续统治优化。

6. 相对边缘化：让先验随参考系移动

相对边缘化试图缓解固定先验的坐标僵硬。它不把先验直接锚在某个全局姿态上，而是把被保留状态分成参考变量和相对变量，让先验主要约束相对几何。例如在单目 VIO 中，全局 yaw 和位置本来不可观，先验若在这些自由度上变硬，就会破坏可观性；相对表示可以把历史信息绑在窗口内部的相对位姿、相对速度或局部基准上，减少全局漂移自由度被错误收紧的机会。

工程做法有多种：可以在边缘化前做 nullspace 投影，去掉理论不可观方向；也可以把先验残差写成相对某个锚帧的局部误差，并在锚帧变化时通过伴随和链式法则搬运雅可比。关键不是名字，而是先回答三个问题：哪些自由度在传感器组合下不可观，先验是否给它们增加了新信息，参考变量被再次边缘化时先验如何重表达。若只是在残差里减一个当前参考姿态，却没有同步变换信息矩阵和误差向量，相对边缘化会变成另一种坐标错配。

相对边缘化的代价是实现复杂度和测试面。固定先验只需保证一次 Schur 分块正确；相对方案还要验证参考变换、nullspace 基、雅可比搬运和重排逻辑。对于没有强闭环、主要跑局部里程计的系统，固定先验加 FEJ 往往已经足够；对于要承受地图重定位、大尺度坐标修正或多会话拼接的系统，相对边缘化才更值得投入。

7. FEJ 解决的不是 Schur 公式

First-Estimate Jacobian 常和边缘化一起出现，但它不是 Schur 补的替代品。Schur 补回答“删掉变量后信息矩阵如何分块消元”，FEJ 回答“雅可比应该在哪个估计点计算”。在非线性系统里，同一个视觉测量若在进入先验时用旧线性化点，在窗口内继续优化时又按新估计重新线性化，理论不可观方向可能被不同雅可比基底重复约束。结果是信息矩阵出现虚假的秩，系统对尺度、yaw 或全局平移过度自信。

FEJ 的做法是为相关因子固定首次估计点 $x ˉ^{FEJ}$ ，用

J_{FEJ} = \frac{\partial r}{\partial x}_{x = x ˉ^{FEJ}}

参与线性化，同时残差值仍可按当前状态计算或按实现约定维护。它牺牲一部分局部收敛速度，换来可观性一致。对 VIO 而言，IMU bias、重力方向、单目尺度和路标逆深度之间的耦合很敏感，FEJ 往往比“每轮都用最新点得到更准确雅可比”更稳。这里的稳不是短期 ATE 更漂亮，而是信息矩阵不会凭空生成传感器没有观测到的方向。

与已有 FEJ 简介不同，本篇强调的是分工：先用 Schur 补把被删变量积分掉，再用固定线性化契约维护先验含义；FEJ 是其中一层契约。若 Schur 分块、变量重排或 $H_{mm}$ 求解本身错了，FEJ 救不了；若 Schur 正确但新旧因子雅可比点混乱，系统仍会不一致。

8. 数值实现要先处理零空间

实际 $H_{mm}$ 不一定严格正定。原因可能是被边缘化变量缺少锚定，也可能是 gauge freedom、弱纹理、纯旋转或重复参数化造成的零空间。直接对 $H_{mm}$ 做普通 Cholesky，失败时加一个很大的 $λ I$ ，是在把不可观方向硬变成可观方向。更稳妥的流程是先对待边缘化块做对称化，检查特征值，按阈值分离有效子空间：

cpp

Eigen::SelfAdjointEigenSolver<MatrixXd> es(Hmm);
VectorXd inv = VectorXd::Zero(es.eigenvalues().size());
for (int i = 0; i < inv.size(); ++i) {
  if (es.eigenvalues()[i] > eps) {
    inv[i] = 1.0 / es.eigenvalues()[i];
  }
}
MatrixXd Hmm_pinv =
    es.eigenvectors() * inv.asDiagonal() * es.eigenvectors().transpose();
MatrixXd Hp = Hrr - Hrm * Hmm_pinv * Hmr;
MatrixXd bp = br - Hrm * Hmm_pinv * bm;
Hp = 0.5 * (Hp + Hp.transpose());

这段代码只是说明结构，生产实现还要考虑稀疏块、ordering 和性能。重点是用伪逆表达“这些方向没有足够信息”，而不是用阻尼制造信息。边缘化后的 $H_{p}$ 也应做谱检查：允许接近零的 gauge 方向存在，不允许明显负特征值长期出现。小的负值可能来自浮点误差，可以截断；大的负值通常意味着雅可比符号、残差权重或变量顺序有错。

9. 重开窗口是一种强验收

只看单次优化是否收敛，不足以验收边缘化。一个更有杀伤力的测试是“重开窗口”：在同一段 bag 上运行两套后端。一套正常滑窗并持续边缘化；另一套在若干关键时刻丢弃旧先验，用最近 $N$ 帧和可重新取到的原始测量重建窗口，再从相同状态初始化优化。若边缘化先验正确，两条轨迹在局部可观方向上应连续，重开窗口后不应出现系统性姿态跳变或 bias 突变。

这个测试抓的是先验吸收信息的真实性。若固定先验已经把 yaw 弱方向压得过硬，重开窗口会释放这部分自由度，两条轨迹在转弯后分叉；若某些视觉因子被边缘化后又留在当前窗口，正常滑窗会比重开窗口更自信，残差 chi-square 反而偏小。记录重开前后 $p, R, v, b_{g}, b_{a}$ 的差值，比只看最终 ATE 更容易定位问题发生在哪类状态上。

重开窗口不是要求线上系统真的这样跑，而是离线验收手段。它应覆盖直线、急转、短时遮挡、弱纹理和回到旧区域的片段。每个片段都保存边缘化事件编号、被删 block、先验维度、最小特征值和优化迭代次数。这样一旦轨迹跳变，可以回到具体一次 Schur 操作，而不是在半小时日志里猜。

10. 信息矩阵谱要按物理解释

边缘化先验的谱比单个 ATE 数字更早暴露问题。每次生成 $H_{p}$ 后，至少记录 $λ_{m i n}$ 、 $λ_{m a x}$ 、有效秩、条件数和最小特征向量在状态 block 上的能量分布。若最小方向主要落在全局平移和 yaw 上，在单目或 VIO 无全局参考时是合理的；若某次边缘化后这些方向突然抬高几个数量级，就要怀疑 FEJ、nullspace 或重复因子。若最大特征值持续增长且对应旧 bias 或旧姿态 block，可能是先验被不断叠加却没有正确移除已吸收测量。

谱检查要和残差统计一起看。白化残差的 $χ^{2}$ 长期远小于自由度，常提示信息过强或噪声设得过大；长期远大于自由度，则可能漏信息、模型错或外点未处理。对边缘化而言，最危险的是“残差很好，谱越来越硬”，因为它会诱导团队误以为系统更稳定。真正稳定的系统应在传感器不可观方向保持诚实的不确定性，在可观方向逐步收敛。

11. 和批量优化对账

滑窗边缘化的金标准不是另一套滑窗，而是一段可控长度内的批量优化。选取几十秒数据，把所有原始视觉、IMU 和外参因子保留下来，先跑完整批量问题得到参考解；再按同样初值、同样噪声和同样鲁棒核运行滑窗，让它在相同时间点输出保留状态。若没有闭环大修正，两者在局部可观方向上的差异应随窗口长度增加而减小，而不是在每次边缘化后出现固定符号的偏移。

对账时不要只比较位姿曲线。要把某次边缘化前后的线性系统导出，检查 Schur 后的 $δ x_{r}$ 是否等于完整系统消元得到的保留增量；再比较边缘协方差的对角块与关键交叉块。很多 bug 在位姿上被鲁棒核和 IMU 平滑掩盖，却会在 bias-姿态、路标-位姿交叉协方差里明显错位。若批量解和滑窗解差异集中在某个重投影因子集合，还要回查测量生命周期：特征第一次被观测、被三角化、被判外点、被边缘化和被删除，是否每一步只进入一次信息矩阵。

这里还有一个常被忽略的边界：鲁棒核的权重属于线性化时刻。若一个外点在边缘化时被 Huber 降权，它被压进先验后不应在未来因为残差变小又恢复完整权重；反过来，一个当时未识别的外点一旦进入先验，后续也很难单独剥离。因此边缘化前的外点判定、时间同步筛查和特征质量过滤，比普通窗口内因子更严格。先验是不可逆压缩，压进去的坏信息会陪系统走很久。

可复现性也要纳入验收。每个先验应记录版本号、参与因子数量、线性化状态摘要、白化残差范数和谱摘要；回放同一 bag 时，这些摘要应在浮点容差内稳定。若只是最终轨迹相似，而中间先验的有效秩随机变化，说明 ordering、并行归约或外点集合还没有确定性，后续排障会非常困难。

12. 代码审查时看四个断点

第一，看参与边缘化的因子是否从活动窗口中移除。一个测量不能既被打包进先验，又以原始因子形式继续约束同一批变量。第二，看 block ordering 是否稳定，特别是 IMU pose、speed-bias、外参和路标混排时，矩阵切片很容易错一列。第三，看局部参数化是否一致；四元数在全局有四维，在切空间只有三维，先验必须落在最小增量上。第四，看先验线性化点是否随状态 id 保存，而不是随内存地址漂移。

我会给边缘化模块加一个小型解析测试：构造三个位姿和两个相对因子，手算或用完整批量求解得到保留变量的边缘协方差，再与 Schur 结果比较。随后再加随机数值测试：生成稀疏高斯系统，随机选择待删 block，比较完整求解的 $δ x_{r}$ 与 Schur 后求解的结果。这个测试不需要相机模型，却能抓住符号、转置和重排错误。

13. 收束：删的是变量，不是历史

边缘化的工程契约可以压成三句话。Schur 补负责把被删变量解释过的信息转移到保留变量上；先验因子负责在固定线性化契约下继续表达这份信息；FEJ 或相对边缘化负责避免不可观方向被后续线性化悄悄收紧。三者任何一层混乱，都会把“节省窗口大小”变成“篡改历史约束”。

验收也应对应这三层：用解析与随机线性系统验证 Schur；用重开窗口验证先验吸收是否等价；用信息矩阵谱、有效秩和 $χ^{2}$ 时间序列验证可观性是否诚实。短序列 ATE 只能说明系统没有立刻坏掉，不能证明边缘化正确。真正可靠的滑窗后端，在丢掉关键帧后仍能说清楚每一份历史信息去了哪里、以什么坐标留下、在哪些方向上保持沉默。