50_因子图与非线性最小二乘

博士前数学路线图·第五批·子专题 B：因子图与非线性最小二乘——MAP估计、Gauss-Newton/LM、Schur补与边缘化

---

前置自测 ⭐

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	贝叶斯公式与 MAP：写出后验 \(p(x\mid z)\propto p(z\mid x)p(x)\)，解释 MAP 估计为什么等价于最大化后验。高斯先验 \(p(x)=\mathcal N(\bar x,P_0)\) 取负对数后得到什么形式的代价项？	5-A（贝叶斯滤波）、概率论基础
2	高斯分布与二次型：对多元高斯 \(\mathcal N(\mu,\Sigma)\)，写出概率密度函数。解释信息矩阵 \(\Omega=\Sigma^{-1}\) 与协方差矩阵的对偶关系。	概率与统计基础
3	线性最小二乘：对超定系统 \(Ax=b\)，写出正规方程 \(A^\top A x=A^\top b\)。解释为什么 \(A^\top A\) 半正定，以及什么条件下它正定。	线性代数
4	矩阵分解：什么是 Cholesky 分解？它要求矩阵满足什么条件？\(LDL^\top\) 分解与 \(LL^\top\) 的区别是什么？	线性代数 / 数值方法
5	IEKF 与单步 GN 的等价性：5-A4 证明了 IEKF 等价于带先验的单步 Gauss-Newton。写出这个等价关系的核心方程。如果把"单步"推广到"对所有时间步联合优化"，直觉上会得到什么？	5-A（IEKF）

---

底线陈述（BLUF）：SLAM/VIO 的状态估计在高斯噪声假设下等价于**因子图上的加权非线性最小二乘**；MAP 估计 = WNLS = Gauss-Newton 迭代。把观测图写成因子图 → 堆叠残差与 Jacobian → 形成正规方程 \(H\delta x=-g\) → 利用"位姿-路标"箭头稀疏结构做 Schur 补（Reduced Camera System），是 BA/VIO 加速 100×–1000× 的数学基石。工程上 GTSAM（因子图 + Bayes 树）、Ceres（残差容器 + AutoDiff）、g2o（超图 + 块求解器）三家分治：GTSAM 强于增量与 IMU 预积分，Ceres 强于通用 NLLS 与 AutoDiff，g2o 强于代码轻量与历史积淀。本子专题建立从概率（§B.1–B.3）→ 算法（§B.4–B.8）→ 稀疏数值（§B.9–B.14）→ 工程库（§B.15–§B.17）→ SLAM 典型因子（§B.18–B.21）→ 汇总（§B.22–§B.28）的完整链路，衔接 5-A（IEKF=单步 GN）并铺垫 5-C（iSAM2/Bayes 树）、5-E（SDP 松弛）、5-F（鲁棒核）。

从递推滤波到批量优化：范式切换说明。 A 系列的所有 Kalman 族方法共享同一个"预测-更新"递推范式：维护当前时刻的状态均值与协方差，每收到一帧新量测就前推一步。这种递推范式天然因果、计算量恒定，但有三个根本局限：(1) 一次线性化不可撤销——过去的线性化点一旦选定就无法修正，长期累积导致一致性退化；(2) 只维护最新时刻的边际分布，丢掉了历史状态之间的条件独立结构；(3) 回环闭合需要更新所有相关状态的交叉协方差，复杂度随地图规模二次增长。因子图范式彻底换了一条路：把**所有时间步的状态**视为联合优化变量，把每个运动/观测模型视为一个"因子"（约束），通过求解加权非线性最小二乘一次性获得全局 MAP 估计。代价是从 \(O(n)\) 在线递推变为 \(O(N)\)（甚至更高）批量求解，但稀疏结构（Schur 补、COLAMD 排序、增量 QR/Cholesky）把实际复杂度压到远低于朴素上界，而且可以对过去的线性化点反复修正（重线性化）。A4 已经证明的"IEKF = 单步 Gauss-Newton"正是连接两个范式的桥梁——把单步 GN 推广到全部时间步的联合 GN，就从滤波器走到了因子图。

---

Part 1 · 因子图的概率语义

§B.1 从贝叶斯网络到因子图 ⭐⭐

动机。SLAM 的联合后验

\[ p(x_{0:T},\,l_{1:M}\mid z_{1:K},\,u_{0:T-1}) \;\propto\; p(x_0)\,\prod_{k=1}^{T} p(x_k\mid x_{k-1},u_{k-1})\,\prod_{k=1}^{K} p(z_k\mid x_{i_k},l_{j_k}) \]

是天然分解（factorized）的——每个运动/观测模型只涉及少数变量。因子图（factor graph, FG） 就是这种局部分解的最忠实可视化：圆节点代表变量，方节点代表因子（一个高斯势函数），边连接因子与其作用变量。SLAM 中每条 IMU 约束、每个重投影残差、每次回环匹配都是一个方节点。

贝叶斯网络 (Bayesian Network, BN) 是**有向**图，表达 \(p(x_i\mid \mathrm{pa}(x_i))\) 的条件独立结构；但 MAP/ML 时方向性无实际用；而且同一个条件分布可以画出多种等价 BN。**因子图**抽离方向性后只保留"哪个因子连哪些变量"，这是优化器真正需要的信息。BN→FG 的转换规则极简：每条件分布 \(p(x_i\mid \mathrm{pa}(x_i))\) 变成一个连接 \(\{x_i\}\cup \mathrm{pa}(x_i)\) 的因子。

二部性（bipartite）。因子图是二部图：变量节点之间、因子节点之间都没有直接边。这让"变量消元"（§B.3、§B.10）在图上有清晰的局部操作语义——消一个变量就是**合并它所有邻接的因子为一个新因子**。

直觉。把因子图想成"一堆弹簧":每个因子是一根弹簧，刚度 = 信息矩阵，自然长度 = 测量值；MAP 就是求所有弹簧总弹性能最小的构型。后续所有数值方法都是在求这个能量面上的局部极小点。

教材对照：Dellaert & Kaess, Factor Graphs for Robot Perception (FnT 2017) §2；Barfoot State Estimation for Robotics 2ed §9.1。

---

§B.2 MAP = 非线性最小二乘（高斯噪声下） ⭐⭐

推导。取负对数，MAP 问题化为

\[ \hat x \;=\; \arg\max_x p(x\mid z) \;=\; \arg\min_x \Big[-\log p(x) - \sum_k \log p(z_k\mid x)\Big]. \]

对每个因子做**高斯假设** \(p(z_k\mid x) = \mathcal N(h_k(x),\Sigma_k)\)，则

\[ -\log p(z_k\mid x) \;=\; \tfrac{1}{2}\,\underbrace{(h_k(x)-z_k)^\top \Sigma_k^{-1}(h_k(x)-z_k)}_{\|r_k(x)\|_{\Sigma_k^{-1}}^2} \;+\; \text{const}. \]

于是 MAP = 加权非线性最小二乘 (WNLS)：

\[ \boxed{\;\hat x \;=\; \arg\min_x\; \tfrac12\sum_k \|r_k(x)\|_{\Sigma_k^{-1}}^2,\qquad r_k(x) = h_k(x)-z_k.\;} \]

残差符号约定（工程大坑！）。GTSAM 与大多数教材用 \(r = h(x)-z\)（预测减测量）；少数文献用 \(r = z-h(x)\)。两种约定仅差一个负号，但影响 Jacobian 符号与鲁棒核解释。本文统一用 \(r = h(x)-z\)。

概念陷阱：残差符号反向导致迭代发散。 如果项目中一部分因子用 \(r=h-z\)，另一部分用 \(r=z-h\)，Jacobian 正负号不一致，GN/LM 的增量方向会指向代价上升的方向。现象是优化器第一步就把代价翻倍，然后 LM 疯狂加大阻尼 \(\lambda\) 直到步长为零。排查方式：对每个 CostFunction 用数值差分检查解析 Jacobian 的符号。正确做法：全项目统一约定，写在代码仓库的 CONTRIBUTING.md 里。

信息矩阵 vs 协方差。二次型 \(\|r\|_{\Sigma^{-1}}^2 = r^\top \Sigma^{-1} r\) 中的 \(\Omega := \Sigma^{-1}\) 叫**信息矩阵 / 精度矩阵 (information / precision matrix)。两者对偶：协方差描述不确定性的大小，信息矩阵描述确定性的强度；**SLAM 中信息矩阵在 smoothing 下稀疏，而协方差在 filtering 下稠密——这是选 smoothing 的数学理由。（Dellaert–Kaess §3）

先验 = 伪观测。\(p(x_0) = \mathcal N(\bar x_0, \Sigma_0)\) 对应残差 \(r_0 = x_0 \boxminus \bar x_0\)（见 §B.18），先验与观测在代价函数中地位完全等价——这就是为什么 GTSAM 里 PriorFactor 和 BetweenFactor 长得像同一个 API。

教材对照：Barfoot 2ed §4.1.1–4.1.2；Thrun Probabilistic Robotics §11.2；Triggs et al. BA — A Modern Synthesis §3。

---

§B.3 因子图 ↔ 贝叶斯网络 ↔ 马尔可夫随机场 ⭐⭐

图模型	图类型	表达对象	SLAM 中的用途
Bayesian Network (BN)	有向无环图	条件独立结构 \(p(x_i\mid\mathrm{pa})\)	建模生成过程（先运动再观测）
Markov Random Field (MRF)	无向图	团势函数 \(\phi_c(\mathbf X_c)\)（Gibbs 分布）	信念传播、图割
Factor Graph (FG)	二部无向图	单个因子 \(\phi_k\) 与其作用变量	非线性最小二乘 / 变量消元 / iSAM2

等价性。三种模型在"正密度下"可以相互转换：BN → MRF 需要 moralization（把每个节点的父节点两两相连并去向），BN/MRF → FG 只需把每个团/条件分布视为一个因子。FG 是最细粒度的——同一个 MRF 团可对应多个因子（如视觉 BA 中同一 landmark 被多个相机观测产生多个因子）。

Moralization 保证 BN 中隐含的\"解释消除 (explaining away)\"结构在 MRF 中正确保留。Triangulation（弦化）是将 MRF 变为 chordal graph 的过程；消元顺序决定弦化结果，弦化后的最大团即 clique tree / junction tree 的节点，也就是 iSAM2 中 Bayes 树团的雏形。

消元（elimination）的三图诠释（K&F 2009 Ch.9）：

• BN 视角：按顺序把 \(p(x_1,\dots,x_n)\) 化为 \(p(x_n\mid x_{n-1},\dots)\,p(x_{n-1}\mid\dots)\cdots\)——这就是**链式分解**。
• MRF 视角：sum-product——对被消变量求和，产生新的团势函数。
• FG 视角：合并邻接因子 → 形成新因子 → 删除被消变量节点。图操作最直观。

对线性高斯 SLAM，消元一个变量等价于对 Hessian 做一次 Schur 补（§B.10）；在 FG 上看就是\"剪掉一个变量节点后剩余因子的新边\"。

教材对照：Koller & Friedman PGM (2009) Ch.4, 9；Barfoot §9.1.2。

---

Part 2 · 非线性最小二乘求解器

§B.4 Gauss-Newton 方法 ⭐⭐

残差堆叠。令 \(r(x) := [r_1(x)^\top,\dots,r_K(x)^\top]^\top \in \mathbb R^m\)，总代价

\[ F(x) = \tfrac12 \|r(x)\|_\Omega^2 = \tfrac12\,r(x)^\top \Omega\, r(x),\quad \Omega = \mathrm{blkdiag}(\Omega_1,\dots,\Omega_K). \]

一阶 Taylor 展开 \(r(x+\delta) \approx r(x) + J\delta\)，其中 \(J = \partial r/\partial x\)。代入并对 \(\delta\) 求极值得 Gauss-Newton 正规方程 (normal equation)：

\[ \boxed{\;(J^\top \Omega\, J)\,\delta x = -J^\top \Omega\, r(x).\;}\qquad H:=J^\top\Omega J,\ g:=J^\top\Omega r. \]

近似 Hessian。对加权残差，真 Hessian 为 $$ \nabla^2 F = J^\top \Omega J + \sum_k (\Omega r)_k\,\nabla^2 r_k $$ 若先把残差白化为 \(e=\Omega^{1/2}r\)，则可写成 \(\nabla^2 F=J_e^\top J_e+\sum_k e_k\nabla^2 e_k\)。GN 扔掉后一项。一般非线性最小二乘中，GN 只能保证局部线性收敛；当残差接近 0、Jacobian 满秩且模型条件良好时，二阶项消失或足够小，GN 才会接近牛顿法并可能达到二次收敛（Nocedal-Wright Thm 10.1）。

为什么 SLAM 偏爱 GN。(a) 典型工作点处残差 \(r_k\) 是像素级/厘米级小量；(b) \(J^\top J\) 保证半正定，不需评估二阶导数；(c) 仅需一阶 Jacobian，AutoDiff（§B.16）即可。

思维陷阱：以为 GN 在 SLAM 中总是二次收敛。 GN 接近二次收敛需要三个条件同时满足：\(J\) 列满秩、\(J\) Lipschitz 连续、残差 \(r(x^\star)\) 充分小。在坏初值或大回环后残差很大时，GN 可能不收敛甚至发散。此时必须切换到 LM（阻尼保守策略）或 Dogleg（信赖域）。判断经验：若优化第一步代价上升或 \(\delta\) 范数超过当前状态尺度的 10%，说明 GN 的线性模型已不可靠。

与牛顿法对比：牛顿法需计算 \(\nabla^2 r_k\)，每项都是三阶张量，SLAM 中工程代价极高；GN 用"\(J^\top J\)"替代，损失一点精度换来巨大的实现简化。

伪代码：

GN(x0):
  x ← x0
  while not converged:
    r, J ← evaluate(x)
    solve (Jᵀ Ω J) δ = -Jᵀ Ω r       // 稀疏 Cholesky / QR
    x ← x ⊞ δ                           // 流形 retraction
  return x

教材对照：Nocedal-Wright §10.2；Barfoot 2ed §4.1.3；Dellaert-Kaess §4.

---

§B.5 Levenberg-Marquardt 方法 ⭐⭐

动机。GN 在残差大或 \(J^\top J\) 条件数差时发散；梯度下降鲁棒但慢。LM 做二者的**阻尼插值**：

\[ \boxed{\;(J^\top \Omega J + \lambda D)\,\delta x = -J^\top \Omega\, r.\;} \]

• \(\lambda \to 0\)：退化为 GN（大步长，快收敛）。
• \(\lambda \to \infty\)：\(\delta x \approx -\lambda^{-1} D^{-1} g\)，退化为缩放的**最速下降**（小步长，保守）。

阻尼矩阵 \(D\)：原始 Levenberg 取 \(D=I\)；Marquardt 变体取 \(D=\mathrm{diag}(J^\top\Omega J)\)（若已白化残差，则等价写成 \(\mathrm{diag}(J^\top J)\)）。它按各维曲率自适应缩放，应对各向异性更鲁棒。Ceres、g2o、GTSAM 默认都用 Marquardt 版。

Nielsen 阻尼策略（Madsen-Nielsen-Tingleff 2004 §3.2，现代实现几乎一统）：

• 初值 \(\mu_0 = \tau\cdot\max_i (J_0^\top\Omega J_0)_{ii}\)，\(\tau\in[10^{-6},10^{-3}]\)。若使用白化残差，\(\Omega\) 已被吸收到 \(r,J\) 中。
• 增益比 \(\rho = \dfrac{F(x)-F(x+\delta)}{L(0)-L(\delta)}\)，其中 \(L(\delta)=\tfrac12\|r+J\delta\|_\Omega^2\)；白化写法下为 \(\tfrac12\|r+J\delta\|^2\)，分母解析简化为 \(\tfrac12\,\delta^\top(\mu\delta - J^\top r)\)。
• \(\rho>0\)（接受）：\(\mu \leftarrow \mu\cdot \max(\tfrac13,\,1-(2\rho-1)^3),\;\nu\leftarrow 2\)。
• \(\rho\le 0\)（拒绝）：\(\mu \leftarrow \mu\nu,\;\nu\leftarrow 2\nu\)（2, 4, 8, 16, ... 指数上升）。

信赖域视角。在白化残差写法下，求解 \((J^\top J+\lambda I)\delta = -J^\top r\) 精确等价于

\[ \min_\delta \|J\delta + r\|^2\quad\text{s.t.}\quad \|\delta\|\le \Delta(\lambda), \]

即 LM 是**隐式信赖域方法**——\(\lambda\) 大 ↔ \(\Delta\) 小。Nocedal-Wright §10.3 从这个角度证明全局收敛到驻点。

教材对照：Nocedal-Wright §10.3；Madsen-Nielsen-Tingleff 2004 §3.2；Ceres trust_region_minimizer.cc；g2o optimization_algorithm_levenberg.cpp。

---

§B.6 Dogleg 方法 ⭐⭐⭐

思想（Powell's dog leg，Nocedal-Wright §4.1；Madsen §3.3）：在**最速下降（Cauchy 点）**与**高斯-牛顿步**两个候选步之间走一条\"狗腿\"折线，用信赖域半径 \(\Delta\) 选点。

令

\[ h_{sd} = -J^\top r,\quad \alpha = \frac{\|J^\top r\|^2}{\|J\,J^\top r\|^2},\quad h_\alpha = \alpha h_{sd},\quad h_{gn} = -(J^\top J)^{-1}J^\top r. \]

Dogleg 步：

\[ h_{dl}=\begin{cases} h_{gn}, & \|h_{gn}\|\le \Delta\\ \Delta\,h_{sd}/\|h_{sd}\|, & \alpha\|h_{sd}\|\ge \Delta\\ h_\alpha + \beta(h_{gn}-h_\alpha), & \text{否则，}\beta \text{由 }\|h_{dl}\|=\Delta \text{ 确定} \end{cases} \]

相对 LM 的优势：Dogleg 每次迭代仅解一次 \((J^\top J)h_{gn}\)；若步被拒绝只需缩 \(\Delta\)、沿同一折线取不同点，无需重新解方程。LM 每次拒绝都要重解 \((J^\top J+\lambda I)\delta\)（因 \(\lambda\) 变了）。当线性求解 >> 残差评估时 Dogleg 更划算。

实现：Ceres TrustRegionStrategyType::DOGLEG（TRADITIONAL_DOGLEG / SUBSPACE_DOGLEG）；g2o OptimizationAlgorithmDogleg；GTSAM DoglegOptimizer。

教材：Nocedal-Wright §4.1；Madsen-Nielsen-Tingleff §3.3。

---

§B.7 线搜索 vs 信赖域 ⭐⭐⭐

线搜索 (line search)：先定方向 \(p_k\)，再定步长 \(\alpha_k\)，令 \(x_{k+1} = x_k + \alpha_k p_k\)。

• Armijo 条件（充分下降）：\(f(x_k+\alpha p_k)\le f(x_k)+c_1\alpha\,\nabla f_k^\top p_k\)，\(c_1\in(0,1)\)，典型 \(c_1=10^{-4}\)。
• （强）Wolfe 曲率条件：\(|\nabla f(x_k+\alpha p_k)^\top p_k|\le c_2|\nabla f_k^\top p_k|\)，\(c_1<c_2<1\)。
• Backtracking：\(\alpha \leftarrow 1\)；若 Armijo 不满足则 \(\alpha \leftarrow \rho\alpha\)（\(\rho=0.5\)）。

信赖域 (trust region)：先定半径 \(\Delta_k\)，再在球 \(\|\delta\|\le \Delta_k\) 内最小化二次模型 \(m_k(\delta)\)；由 \(\rho_k\) 调整 \(\Delta_k\)：\(\rho_k>0.75\) 扩，\(<0.25\) 缩。

SLAM 选择。LM/Dogleg 本质都是**信赖域**；而纯线搜索 GN 在残差大或 Jacobian 病态时不鲁棒。Ceres 默认 TRUST_REGION + LEVENBERG_MARQUARDT；也提供 LINE_SEARCH minimizer（搭配 LBFGS 用于通用非线性规划，SLAM 中少用）。

视角	线搜索	信赖域
第一步	确定方向	确定半径
第二步	确定步长	在半径内求方向+步长
失败处理	缩 \(\alpha\)	缩 \(\Delta\)（模型不准）
SLAM 代表	—	LM、Dogleg

教材：Nocedal-Wright Ch.3, Ch.4。

---

§B.8 收敛性分析 ⭐⭐⭐

GN 局部收敛 (NW Thm 10.1)：若 \(J(x^\star)\) 列满秩、\(J\) 在 \(x^\star\) 邻域 Lipschitz 连续、残差 \(r(x^\star)\) 充分小，则 GN 迭代 \(x_k\to x^\star\) 线性至二次**收敛，收敛率与 \(\|r(x^\star)\|\) 成正比。**解释：残差小时 \(\sum r_k\nabla^2 r_k\) 项可忽略，GN \(\approx\) Newton \(\approx\) 二次收敛；残差大时 GN 退化为线性。

LM 全局收敛：\(\mu\) 足够大时 LM 步长满足 Cauchy 下降量条件，配合 Nielsen 策略全局收敛到驻点（可能局部极小）。

SLAM 实际含义：

• 好初值（前端里程计、EPnP、视觉对齐、IMU pre-integration）→ 残差小 → GN/LM 通常进入正确 basin，快速收敛到低代价局部解；在常规里程计/VIO 数据上这个局部解往往就是工程上需要的解，但不能把它当作全局最优保证。
• 坏初值（大尺度 SfM、回环后首次优化）→ 残差大，常陷**局部极小**（pose graph 有 \(2^n\) 量级极小点，见 Carlone 2014）。需要 certifiable 方法：SE-Sync、SDP 松弛（→ 5-E）。

经验数字：在 KITTI 里程计、EuRoC VIO、BAL 大规模 BA 等数据集上，GN/LM 典型 5–15 次迭代即收敛到像素级精度。iSAM2 通常每帧 1–3 次 wildfire 步即可。

教材：Nocedal-Wright §10.2 Thm 10.1；Carlone et al. Planar Pose Graph SLAM IJRR 2014。

---

Part 3 · 稀疏性、Schur 补与边缘化

§B.9 SLAM 中 Hessian 的稀疏结构 ⭐⭐

把状态分为位姿 \(x_p\in\mathbb R^{6m}\)（\(m\) 相机）与路标 \(x_l\in\mathbb R^{3n}\)（\(n\) 点），线性化系统呈**箭头结构 (arrowhead)**：

\[ H = \begin{bmatrix} H_{pp} & H_{pl} \\ H_{pl}^\top & H_{ll} \end{bmatrix},\qquad \begin{cases} H_{pp} & 位姿块，稀疏（只有有共视的相机对才耦合）\\ H_{ll} & \textbf{块对角}（每路标 3 \times 3，相互独立）\\ H_{pl} & 稀疏（相机-路标可见性二分图） \end{cases} \]

关键事实：\(H_{ll}\) 是**块对角**——每个 3D 点的信息仅来自观测它的相机，路标之间没有直接因子。这是 Schur 加速的根本。

邻接图 ↔ Hessian 稀疏模式。因子图中两个变量被同一个因子连接 \(\iff\) \(H\) 的对应分块非零。由此**因子图结构**直接决定**Hessian fill-pattern**。画出因子图即可预测稀疏模式。

Pose-Graph 特例（无路标）：\(H = H_{pp}\) 块三对角（里程计）+ 少量回环跨块 → 高度稀疏 → 可直接稀疏 Cholesky。

教材：Barfoot 2ed §9.2；Triggs et al. 2000 §6。

---

§B.10 Schur 补在 BA 中的加速 ⭐⭐

消元路标。写出正规方程分块形式：

\[ \begin{bmatrix} H_{pp} & H_{pl} \\ H_{pl}^\top & H_{ll} \end{bmatrix} \begin{bmatrix} \delta x_p \\ \delta x_l \end{bmatrix} = \begin{bmatrix} -g_p \\ -g_l \end{bmatrix} \]

从第二行解出 \(\delta x_l = H_{ll}^{-1}(-g_l - H_{pl}^\top \delta x_p)\)，代入第一行：

\[ \boxed{\;\underbrace{(H_{pp} - H_{pl}\,H_{ll}^{-1}\,H_{pl}^\top)}_{S,\ \text{reduced camera system}}\;\delta x_p = -g_p + H_{pl}\,H_{ll}^{-1} g_l.\;} \]

然后回代求 \(\delta x_l\)。

复杂度。\(H_{ll}\) 块对角，每块 \(3 \times 3\) 求逆 \(O(n)\)；形成 \(S\) 的代价 \(O(n\cdot m_j^2)\)（\(m_j\) 为第 \(j\) 点被观测的相机数）；最终只需解 \(6m\) 维稠密（或稀疏）方程。总复杂度 \(O(m^3 + nm^2)\)，相比不 Schur 的 \(O((6m+3n)^3)\) 可降低数个数量级。

典型数字。BA 中 10000 个路标 + 100 个相机：不 Schur 维度 \(3\times 10000 + 6\times 100 = 30600\)；Schur 后仅需解 \(6\times 100 = 600\) 维。矩阵求解规模差 \((30600/600)^3 \approx 1.3\times 10^5\) 倍。

编程陷阱：Ceres 中 Schur 消元顺序反了。 在 ParameterBlockOrdering 中把 camera 放 group 0（先消）而非 point，会先消相机留下稠密 camera-camera block，复杂度退化到不做 Schur 的水平甚至更差。正确做法：3D point 放 group 0（先消），camera 放 group 1（后解）。g2o 中等价操作是对 landmark 顶点 setMarginalized(true)。

概率解释。Schur 补 = 边缘化：

\[ p(x_p\mid z) = \int p(x_p,x_l\mid z)\,dx_l. \]

对高斯分布，边缘化联合信息矩阵对应的块 \(\Lambda_{pp\mid l} = \Lambda_{pp} - \Lambda_{pl}\Lambda_{ll}^{-1}\Lambda_{lp}\) 正是 \(S\)（信息矩阵形式）。\"消元 = 边缘化 = Schur 补\" 三位一体。

工程实现：

• GTSAM：消元序把路标先消（VariableIndex + COLAMD），gtsam::HessianFactor::schur 自动做。
• g2o：对路标顶点 setMarginalized(true) + 使用 BlockSolver_6_3 自动启用 Schur。
• Ceres：Solver::Options::linear_solver_type = SPARSE_SCHUR 或 DENSE_SCHUR；通过 ParameterBlockOrdering 把 3D 点放 group 0、相机放 group 1，group 0 先被消元。

教材：Triggs et al. BA — A Modern Synthesis §6；Barfoot §9.3；Dellaert-Kaess §4。

---

§B.11 变量排序与 Fill-in ⭐⭐⭐

Fill-in。对稀疏 SPD 矩阵 \(A\) 做 Cholesky \(A=LL^\top\)，\(L\) 可能比 \(A\) 稠密得多——额外的非零元叫 fill-in。消元顺序不同，fill-in 量差几个数量级。最优排序 = NP-hard，实际用启发式。

算法	原理	适用场景
AMD (Approximate Minimum Degree, Amestoy-Davis-Duff)	每步选度数近似最小的节点消元	对称矩阵，通用
COLAMD (Column AMD, Davis et al.)	针对 \(A^\top A\) 的列度数，无需显式构造	最小二乘正规方程 / BA
METIS / Nested Dissection (George 1973)	递归图分割，分离集后序消元	平面/网格结构 3D SLAM，\(O(n^{3/2})\) fill

SLAM 实践：Pose graph 选 AMD/COLAMD；BA 在 Schur 消路标**之后**再对 \(S\) 做 COLAMD（两级排序）；超大规模室外地图选 METIS/nested dissection。

Ceres 内部在 SPARSE_SCHUR 中自动跑 COLAMD；手动指定见 ParameterBlockOrdering。g2o 的 LinearSolverCholmod 通过 CHOLMOD 默认启用 AMD。GTSAM 在 Ordering::COLAMD(graph) 或 Ordering::METIS(graph)。

教材：Davis Direct Methods for Sparse Linear Systems (SIAM 2006) Ch.7；Koller-Friedman Ch.9（induced width）。

---

§B.12 边缘化作为信息压缩 ⭐⭐⭐

为什么不能\"直接丢弃\"旧变量。滑窗 VIO（VINS-Mono、OKVIS、ORB-SLAM3 VI 模式）需要维持有限窗口（如 10 KF）以控制计算量。简单丢弃最旧 KF 意味着**抛弃所有历史约束**，系统退化为局部 VO，尺度/重力/偏移信息流失。

边缘化流程（VINS-Mono marginalization_factor.cpp）：

1. 累加所有与被 marg 变量 \(x_m\) 相关的残差，线性化得 \(A = \sum J^\top\Omega J,\ b=\sum J^\top\Omega r\)。
2. 分块 \(A = \begin{bmatrix}\Lambda_{mm}&\Lambda_{mr}\\\Lambda_{rm}&\Lambda_{rr}\end{bmatrix}\)。
3. Schur 补：\(\Lambda_p = \Lambda_{rr} - \Lambda_{rm}\Lambda_{mm}^{-1}\Lambda_{mr}\)，\(b_p = b_r - \Lambda_{rm}\Lambda_{mm}^{-1}b_m\)。
4. 特征分解 \(\Lambda_p = V\mathrm{diag}(s)V^\top\)（过滤 \(s<10^{-8}\) 的零空间），得 \(J_p = \mathrm{diag}(\sqrt s)V^\top\)。
5. 下次优化时添加**线性先验因子** \(\|r_p + J_p\,dx\|^2\)，其中 \(dx = x - x_0\)，\(x_0\) 是 marg 时刻的线性化点。

信息保留最大化。在 KL 散度意义下，Schur 补是\"信息损失最小的\"维度约减——它保留了边缘分布的全部一阶（均值）和二阶（协方差）信息。

FEJ（First-Estimates Jacobian）的必要性。边缘化产生的先验\"锁死\"了线性化点 \(x_0\)。若后续其他因子仍按**最新估计** \(\hat x\) 线性化，而先验按 \(x_0\) 线性化，两者在不可观子空间（yaw、global position 4 DoF）的切平面方向不一致，系统会生成**伪信息**——沿不可观方向的协方差虚假收缩，导致 over-confident、一致性丢失。FEJ 的修正：所有涉及被保留变量的因子都在 \(x_0\) 处线性化；或如 VINS-Mono 折中——仅 marg prior 做 FEJ。相关理论：Huang-Mourikis-Roumeliotis A First-Estimates Jacobian EKF IJRR 2010；Chen-Yang-Geneva-Huang FEJ2 ICRA 2022。

教材：Sibley-Mei-Reid-Newman Vast-Scale Outdoor Navigation IJRR 2010；Leutenegger OKVIS IJRR 2015；Qin-Li-Shen VINS-Mono T-RO 2018 §VI-D。

---

Part 4 · 稀疏直接法与迭代法

§B.13 稀疏直接法：Cholesky 分解 ⭐⭐

Cholesky 对 SPD 矩阵 \(H\) 分解 \(H = LL^\top\)，\(L\) 为下三角。求解 \(H\delta = g\) 两步：\(Ly = g\)（前代）→ \(L^\top\delta = y\)（回代），每步 \(O(\mathrm{nnz}(L))\)。

LDLᵀ 变体：\(H = LDL^\top\)，\(D\) 对角，免开方，数值更稳，半正定也适用。SLAM 正规方程（\(J^\top J\) 在 Jacobian 不满秩时半正定）推荐 LDLᵀ。

Simplicial vs Supernodal（Davis Direct Methods Ch.4–6；CHOLMOD）：

	Simplicial	Supernodal
实现	逐列处理，elimination tree 导向	合并同稀疏结构的列为超节点（稠密块）
BLAS 级别	1 / 2	3（dgemm/dtrsm），充分利用缓存
flops/nnz(L)	低 → 慢	高 → 快（门槛 \(\approx 40\)）
更新/downdate	支持	不支持
适用矩阵	极稀疏、需动态增删行	大规模、稠密 fill

Multifrontal 是 CHOLMOD 中第三种实现：在 elimination tree 上后序汇总\"frontal matrix + update matrix\"，并行性最好，是超大 BA 的首选。

库映射：

• SuiteSparse::CHOLMOD：supernodal/simplicial 自动切换（阈值 Common.supernodal_switch=40）。
• Eigen::SimplicialLLT<SparseMatrix<double>>：纯 Eigen，简式 LL^T。
• Eigen::SimplicialLDLT：SLAM 推荐。
• Eigen::SparseLU：非对称，LU。
• Eigen::SparseQR：稀疏 QR，iSAM1 使用。
• Eigen::CholmodDecomposition：链接 libcholmod，自动在 simplicial/supernodal 间切换，大规模 SLAM 比纯 Eigen 快 2–10×。

---

§B.14 迭代法简介：PCG 与预条件 ⭐⭐⭐

何时选迭代法：当 \(H\) 太大（百万维 BA、NeRF 优化、百亿参数）以至 Cholesky 的 fill 内存溢出；或只需不精确解（inexact Newton）。

PCG (Preconditioned Conjugate Gradient)。求解 SPD 系统 \(Hx=b\)：

r0 = b - H x0;  z0 = M⁻¹ r0;  p0 = z0
for k = 0, 1, ...:
  α = (rᵀz)/(pᵀ H p)
  x ← x + α p
  r_new = r - α H p
  if ||r_new|| < tol: stop
  z_new = M⁻¹ r_new
  β = (r_newᵀ z_new)/(rᵀz)
  p ← z_new + β p

能量范数误差界：\(\|x_k-x^\star\|_H \le 2\Big(\dfrac{\sqrt\kappa-1}{\sqrt\kappa+1}\Big)^k \|x_0-x^\star\|_H\)，\(\kappa=\kappa(M^{-1}H)\)。好预条件 = 压缩 \(\kappa\)。

预条件器 \(M\)：

• Jacobi：\(M = \mathrm{diag}(H)\)，最简，\(O(n)\)。
• Block-Jacobi：按变量分块对角，\(6 \times 6\) for pose + \(3 \times 3\) for landmark。
• Schur-Jacobi：Ceres SCHUR_JACOBI——用 \(S\) 的块对角预条件 CG on \(S\)。
• Incomplete Cholesky IC(0)：\(\tilde L\tilde L^\top \approx H\)，\(\tilde L\) 稀疏模式限制为 \(H\) 的下三角。
• Cluster-Jacobi / Cluster-Tridiagonal（Ceres）：对相机做聚类，簇内块对角或三对角。

SLAM 中的 PCG：Ceres ITERATIVE_SCHUR——CG on \(S\) + Schur-Jacobi 预条件，Agarwal et al. Bundle Adjustment in the Large ECCV 2010 的成果，用于百万级 3D 点重建（Photo Tourism）。g2o LinearSolverPCG。

隐式 Schur：不显式构造 \(S = H_{pp} - H_{pl}H_{ll}^{-1}H_{pl}^\top\)，只需计算 \(S\cdot v\) 的矩阵-向量积，内存省 10–100×。

教材：Saad Iterative Methods for Sparse Linear Systems 2ed (2003) §9–10；Agarwal et al. ECCV 2010。

---

Part 5 · GTSAM 与 Ceres 的工程对照

§B.15 GTSAM 的因子图抽象 ⭐⭐

哲学：变量、因子、图，一切 API 围绕因子图语义。

核心类（头文件路径）：

类	头文件	作用
NonlinearFactorGraph	gtsam/nonlinear/NonlinearFactorGraph.h	因子容器
Values	gtsam/nonlinear/Values.h	Key → Manifold 值
Key	gtsam/inference/Key.h	uint64_t 索引
Symbol	gtsam/inference/Symbol.h	(char,uint64_t) → Key
PriorFactor<T>	gtsam/slam/PriorFactor.h	单变量先验
BetweenFactor<T>	gtsam/slam/BetweenFactor.h	两位姿相对约束
BearingRangeFactor	gtsam/sam/BearingRangeFactor.h	极坐标观测
GPSFactor	gtsam/navigation/GPSFactor.h	Pose3 平移先验
ImuFactor / CombinedImuFactor	gtsam/navigation/ImuFactor.h, CombinedImuFactor.h	IMU 预积分因子
GenericProjectionFactor	gtsam/slam/ProjectionFactor.h	重投影残差
SmartProjectionPoseFactor<CAL>	gtsam/slam/SmartProjectionPoseFactor.h	打包同一路标的多观测，隐式三角化
ISAM2	gtsam/nonlinear/ISAM2.h	增量优化

NoiseModel（gtsam/linear/NoiseModel.h）：

auto n1 = noiseModel::Gaussian::Covariance(Σ);       // 由协方差
auto n2 = noiseModel::Gaussian::Information(Λ);      // 由信息矩阵
auto n3 = noiseModel::Diagonal::Sigmas(Vector3(.1,.1,.02));
auto n4 = noiseModel::Isotropic::Sigma(2, 1.0);
auto huber = noiseModel::mEstimator::Huber::Create(1.345);
auto robust = noiseModel::Robust::Create(huber, n4);

优化器：

LevenbergMarquardtParams p;
p.setVerbosityLM("SUMMARY");
p.relativeErrorTol = 1e-5;
p.maxIterations = 100;
Values result = LevenbergMarquardtOptimizer(graph, initial, p).optimize();

Marginals marginals(graph, result);
auto cov = marginals.marginalCovariance(X(5));

流形约定（关键陷阱）：

类型	切向量维度	顺序
Rot3	3	\(\omega\)（SO(3)，右乘扰动 \(R\mathrm{Exp}(\omega)\)）
Pose3	6	\([\omega;\rho]\)（旋转在前，平移在后！）
NavState	9	\([\omega;\rho;\nu]\)（R, t, v）

Ceres 一般约定 \([\rho;\omega]\)——与 GTSAM 相反，跨库移植时必须显式转换。

最小示例（2D Pose Graph）：

using symbol_shorthand::X;
NonlinearFactorGraph graph;
graph.addPrior(X(1), Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector3(.3,.3,.1)));
auto odo = noiseModel::Diagonal::Sigmas(Vector3(.2,.2,.1));
graph.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(2,0,0), odo);
graph.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(2,0,M_PI_2), odo);
// ...
Values initial; initial.insert(X(1), Pose2(0.5,0,0.2)); /* ... */
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

---

§B.16 Ceres Solver 的残差抽象 ⭐⭐

哲学：通用非线性最小二乘容器，不预设图结构；自动微分是一等公民。

核心 API：

ceres::Problem problem;
problem.AddParameterBlock(camera, 6);
problem.SetManifold(camera, new ceres::ProductManifold<SO3Manifold, EuclideanManifold<3>>(...));

auto* cost = new ceres::AutoDiffCostFunction<ReprojectionFunctor,
                 /*residual*/2, /*camera*/6, /*point*/3>(new ReprojectionFunctor(u, v, K));
problem.AddResidualBlock(cost, new ceres::HuberLoss(1.0), camera, point);

ceres::Solver::Options opt;
opt.linear_solver_type = ceres::SPARSE_SCHUR;
opt.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
opt.preconditioner_type = ceres::SCHUR_JACOBI;
opt.max_num_iterations = 50;
opt.num_threads = 8;
ceres::Solver::Summary summary;
ceres::Solve(opt, &problem, &summary);

CostFunction 家族：

• SizedCostFunction<NumRes, N0, N1, ...>：手写 Evaluate + 解析 Jacobian。
• AutoDiffCostFunction<Functor, NumRes, N0, N1, ...>：模板 functor，operator() 用 T（Jet<double,N>）；Ceres 用前向模式自动微分。
• NumericDiffCostFunction<Functor, CENTRAL/FORWARD/RIDDERS, ...>：数值差分。
• DynamicAutoDiffCostFunction：运行时维度（如 IMU 预积分长度不定）。

LossFunction（ceres/loss_function.h）。作用于 \(s = \|r\|^2\)：

类	\(\rho(s)\)	用途
TrivialLoss	\(s\)	标准 L2
HuberLoss(a)	\(s\le a^2: s\)；否则 \(2a\sqrt s - a^2\)	一般鲁棒首选
SoftLOneLoss(a)	\(2a^2(\sqrt{1+s/a^2}-1)\)	平滑 L1
CauchyLoss(a)	\(a^2\log(1+s/a^2)\)	强抑 outlier
ArctanLoss(a)	\(a^2\arctan(s/a^2)\)	饱和型
TukeyLoss(a)	redescending	大残差权重归零

Manifold（Ceres 2.0+，ceres/manifold.h；2.2 已移除旧 LocalParameterization）：

class Manifold {
  virtual int AmbientSize() const = 0;
  virtual int TangentSize() const = 0;
  virtual bool Plus(const double* x, const double* delta, double* x_plus) const = 0;
  virtual bool PlusJacobian(const double* x, double* J) const = 0;   // Ambient×Tangent
  virtual bool Minus(const double* y, const double* x, double* delta) const = 0;
  virtual bool MinusJacobian(const double* x, double* J) const = 0;  // Tangent×Ambient
};

内建：EuclideanManifold<N>、SubsetManifold、QuaternionManifold（Ceres 序 w,x,y,z）、EigenQuaternionManifold（x,y,z,w，与 Eigen::Quaterniond 内存布局匹配）、SphereManifold<N>、LineManifold<N>、ProductManifold<M1,M2,...>。

Solver::Options 关键字段：

字段	取值	备注
linear_solver_type	DENSE_QR, DENSE_NORMAL_CHOLESKY, SPARSE_NORMAL_CHOLESKY, DENSE_SCHUR, SPARSE_SCHUR, ITERATIVE_SCHUR, CGNR	BA 首选 SPARSE_SCHUR
trust_region_strategy_type	LEVENBERG_MARQUARDT, DOGLEG	默认 LM
dogleg_type	TRADITIONAL_DOGLEG, SUBSPACE_DOGLEG	—
preconditioner_type	IDENTITY, JACOBI, SCHUR_JACOBI, CLUSTER_JACOBI, CLUSTER_TRIDIAGONAL, SCHUR_POWER_SERIES_EXPANSION	用于 ITERATIVE_SCHUR/CGNR
sparse_linear_algebra_library_type	SUITE_SPARSE, EIGEN_SPARSE, ACCELERATE_SPARSE, CUDA_SPARSE	推荐 SuiteSparse
linear_solver_ordering	ParameterBlockOrdering*	手动 Schur
max_num_iterations	int	默认 50
function_tolerance, gradient_tolerance, parameter_tolerance	double	终止条件
num_threads	int	并行

手动 Schur 消元：

auto* ordering = new ceres::ParameterBlockOrdering;
for (auto* p : points)  ordering->AddElementToGroup(p, 0);   // 先消
for (auto* c : cameras) ordering->AddElementToGroup(c, 1);   // 后解
options.linear_solver_ordering.reset(ordering);
options.linear_solver_type = ceres::SPARSE_SCHUR;

教程：ceres-solver.org/tutorial.html；nnls_modeling.html；nnls_solving.html。

---

§B.17 GTSAM vs Ceres vs g2o 对比 ⭐⭐

维度	GTSAM	Ceres Solver	g2o
抽象	因子图（因子 = 概率密度）	残差块容器（参数+CostFunction）	HyperGraph（可超边）
设计哲学	数学优先：可做消元/边缘化/Bayes 树	通用 NLLS，Google 搜索等非 SLAM 也用	工程轻量，SLAM 专用
流形	原生，Pose3/Rot3/NavState 一等公民	Ceres 2.0 Manifold API（2.2 移除旧 LocalParameterization）	需手写 oplusImpl
AutoDiff	通过 Expression 有限支持；主要靠 NoiseModelFactorN 手写	一等公民（AutoDiffCostFunction + Jet<T,N>）	无（数值差分或手写）
增量优化	ISAM2（Bayes 树）	无	批处理为主，有部分 incremental
IMU 预积分	原生 ImuFactor/CombinedImuFactor（Forster T-RO 2017）	需自写（VINS-Mono 做法）	需自写
鲁棒核	noiseModel::Robust（Huber/Cauchy/Tukey/GemanMcClure/DCS/L2WithDeadZone）	最丰富：Huber/SoftLOne/Cauchy/Arctan/Tukey/Tolerant + Composed/Scaled	RobustKernelHuber/Cauchy/Tukey（较少）
边缘化	Marginals + LinearContainerFactor	需自写（VINS-Mono MarginalizationFactor）	Marginalized 顶点（Schur 内置）
回环后重优化	iSAM2 自动局部重排+重线性化	全局重优化	initializeOptimization 全量
并行	多线程（omp）	num_threads	手动
Python 绑定	gtsam PyPI 包	pyceres（第三方）	g2o-python（第三方）
代表用户	Kimera, LIO-SAM, GTSLAM, Forster VIO	COLMAP, Google Street View, TensorFlow	ORB-SLAM 系列（最重要）, LSD-SLAM, DSO
源码体量	大（~200k LOC）	中（~80k）	小（~20k）
文档质量	好（tutorials + doxygen）	优秀（ceres-solver.org）	一般（主要靠读代码）

选型建议：

• 工业级 VIO / VO：GTSAM（iSAM2 + ImuFactor）。
• SfM / 大规模 BA：Ceres（SPARSE_SCHUR / ITERATIVE_SCHUR + Schur-Jacobi）。
• Legacy ORB-SLAM 生态 / 教学：g2o（代码短易读，便于改造）。
• 研究原型：先 GTSAM Python 快速验证，再 Ceres C++ 上线。

---

Part 6 · SLAM 中的典型因子

§B.18 Prior Factor ⭐

\[ r(x) = x \boxminus \bar x_0\qquad (\Sigma = \Sigma_0). \]

流形减法 \(\boxminus\)：\(\mathbb R^n\) 中就是减号；\(SO(3)\) 中 \(r = \mathrm{Log}(\bar R^\top R)\)；\(SE(3)\) 中 \(r = \mathrm{Log}(\bar T^{-1} T)\)（或右扰动版 Pose3::localCoordinates）。

用途：锚定 gauge（BA 消不可观 7-DoF）；IMU bias 先验；重力方向先验（VIO 初始化后）；GPS 观测的平移先验（GPSFactor）。

陷阱：先验方差极小会导致 Hessian 病态；过大等于无先验。经验值：gauge fixing 的 pose prior 用 \(\sigma \approx 10^{-6}\) m/rad。

---

§B.19 Between Factor（里程计/回环约束） ⭐⭐

SE(3) 上的残差（Barfoot §9.1.3；GTSAM BetweenFactor<Pose3>）：

\[ r(T_i, T_j) = \mathrm{Log}\big(\Delta T_{ij}^{\text{meas},\,-1}\cdot T_i^{-1} T_j\big)^\vee \in \mathbb R^6. \]

其中 \(\Delta T_{ij}^{\text{meas}}\) 来自里程计或回环匹配。

Jacobian（右扰动版）。定义扰动 \(T_i \leftarrow T_i\,\mathrm{Exp}(\delta\xi_i)\)，\(T_j \leftarrow T_j\,\mathrm{Exp}(\delta\xi_j)\)：

\[ \frac{\partial r}{\partial \delta\xi_i} = -J_r^{-1}(r)\cdot \mathrm{Ad}_{T_j^{-1} T_i},\qquad \frac{\partial r}{\partial \delta\xi_j} = J_r^{-1}(r). \]

\(J_r\) 是 SE(3) 右雅可比，\(\mathrm{Ad}\) 是伴随。左扰动版本符号不同，工程中必须与流形 retraction 约定一致（GTSAM 默认右乘 retraction）。

2D 退化：\(r = \mathrm{Log}_{\mathrm{SE}(2)}(\Delta T_{ij}^{-1}\cdot T_i^{-1} T_j) \in \mathbb R^3\)。

**回环因子**同 between factor，仅信息矩阵更小（匹配不确定性更大）+ 通常套鲁棒核 Huber/Cauchy/DCS 抑异常。

---

§B.20 IMU 预积分因子（Forster T-RO 2017） ⭐⭐⭐

预积分量定义（Forster et al. arXiv:1512.02363 Eq. 35-37）：

\[ \Delta R_{ij} \triangleq R_i^\top R_j = \prod_{k=i}^{j-1}\mathrm{Exp}\big((\tilde\omega_k - b_k^g - \eta_k^{gd})\,\Delta t\big) \]

\[ \Delta v_{ij} \triangleq R_i^\top(v_j - v_i - g\Delta t_{ij}) = \sum_{k=i}^{j-1}\Delta R_{ik}(\tilde a_k - b_k^a - \eta_k^{ad})\Delta t \]

\[ \Delta p_{ij} \triangleq R_i^\top\big(p_j - p_i - v_i\Delta t_{ij} - \tfrac12 g\Delta t_{ij}^2\big) = \sum_{k=i}^{j-1}\big[\Delta v_{ik}\Delta t + \tfrac12 \Delta R_{ik}(\tilde a_k - b_k^a - \eta_k^{ad})\Delta t^2\big] \]

15 维残差（Eq. 45），状态 \(x_i = (R_i, p_i, v_i, b_i^g, b_i^a)\)，残差 \(r\in\mathbb R^{15}\)：\([r_{\Delta R}; r_{\Delta v}; r_{\Delta p}; r_{\Delta b^g}; r_{\Delta b^a}]\)。

Bias 一阶修正（Eq. 40–44，关键工程 trick）。优化中 bias 更新 \(\hat b = \bar b + \delta b\) 时**无需重积分**：

\[ \Delta\tilde R_{ij}(\hat b^g) \approx \Delta\tilde R_{ij}(\bar b^g)\cdot \mathrm{Exp}\!\Big(\frac{\partial \Delta\bar R_{ij}}{\partial b^g}\,\delta b^g\Big) \]

\[ \Delta\tilde v_{ij}(\hat b) \approx \Delta\tilde v_{ij}(\bar b) + \frac{\partial\Delta\bar v}{\partial b^g}\delta b^g + \frac{\partial\Delta\bar v}{\partial b^a}\delta b^a,\qquad \text{同理 }\Delta\tilde p. \]

\(\partial\Delta\bar R/\partial b^g\) 等解析 Jacobian 在积分过程中一并累积。

协方差递推（Eq. 62-63）：切空间噪声 \(\eta_{ij}^{\Delta} = [\delta\phi;\delta v;\delta p]\)，\(\Sigma_{ij+1} = A_j\Sigma_{ij}A_j^\top + B_j\Sigma^{gd}B_j^\top + C_j\Sigma^{ad}C_j^\top\)。

与 ESKF 的等价性。ESKF 预积分（5-A3）做误差状态线性传播；Forster 预积分做流形上的\"压缩状态\"。数学等价但 Forster 显式分离 bias 依赖，更便于优化框架使用。

工程映射：

• GTSAM：PreintegratedImuMeasurements.integrateMeasurement(acc, gyro, dt) → ImuFactor(Xi, Vi, Xj, Vj, Bi, preint)（5-way 因子，不约束 bias 演化）；CombinedImuFactor 为 6-way 版，内含 bias random walk。
• VINS-Mono：vins_estimator/src/factor/integration_base.h（mid-point 积分）+ factor/imu_factor.h（Ceres SizedCostFunction<15,7,9,7,9>）。

---

§B.21 视觉重投影因子 ⭐⭐

重投影残差（归一化相机模型，已知内参 \(K\)）：

\[ r(T_{wc}, p_w) = \pi\!\big(T_{wc}^{-1}\,p_w\big) - z_{\mathrm{pixel}},\qquad \pi([X,Y,Z]^\top)= \begin{bmatrix} f_x X/Z+c_x\\ f_y Y/Z+c_y \end{bmatrix}. \]

Jacobian 分项：\(\partial r/\partial T_{wc}\)（对位姿流形）、\(\partial r/\partial p_w\)（对 3D 点，\(2\times 3\)）。

逆深度参数化（Civera et al. T-RO 2008；VINS-Mono 默认）。在首观测帧以 \((u_i,v_i,\lambda)\) 表示路标（\(\lambda = 1/Z\)），好处：

• 初始化时 \(\lambda = 0\) 对应无穷远点，避免三角化失败时的数值爆炸；
• 深度测量噪声在 \(1/Z\) 域更接近高斯；
• 优化变量维度少 1（无需存全局 XYZ）。

Smart Factor（GTSAM SmartProjectionPoseFactor<Cal3_S2>）。同一路标的 \(n\) 次观测打包成**一个**因子，内部 Schur 消去该 landmark：

• 变量只剩相机位姿；
• 因子内部 triangulateSafe 做隐式三角化；
• 每次 error() 调用重新计算 optimal landmark；
• 适合 VIO——路标只用于约束相机，不作为最终输出。

重投影因子 vs Smart Factor 选择：

	GenericProjectionFactor	SmartProjectionPoseFactor
变量	位姿 + 3D 点	仅位姿
每观测	1 个因子	同路标观测打包 1 个因子
噪声模型	任意（支持鲁棒）	必须 isotropic
输出路标位置	有	无（可选事后恢复）
适用	全 SLAM / SfM 需要稠密地图	VIO / pose-only

Schur 消路标：即使用 GenericProjectionFactor，Ceres/g2o/GTSAM 后端都会通过 Schur 把 \(H_{ll}\) 消掉，加速 100×+。

---

Part 7 · 汇总表格

§B.22 非线性最小二乘求解器对比

方法	更新方程	收敛性	每步代价	全局收敛	SLAM 库
Gauss-Newton	\((J^\top\Omega J)\delta = -J^\top\Omega r\)	局部超线性/二次（小残差）	1 次线性求解	✗（初值差发散）	教学；Ceres LINE_SEARCH+GN
Levenberg-Marquardt	\((J^\top\Omega J + \lambda D)\delta = -J^\top\Omega r\)	全局到驻点 + 局部 GN 速率	每拒一次重解	✓（信赖域）	Ceres/g2o/GTSAM 默认
Dogleg	\(h_{dl}\) 折线插值 \(h_{sd}, h_{gn}\)	同 LM	每步 1 次线性求解	✓	Ceres DOGLEG、g2o、GTSAM
Trust-Region (generic)	在 \(\|\delta\|\le\Delta\) 解二次模型	全局	1 子问题	✓	Ceres 通用框架
PCG	迭代求解 \(H\delta=-g\)	\(O(\sqrt\kappa)\) 步	每步矩阵-向量积	—	Ceres ITERATIVE_SCHUR、g2o PCG
IEKF = 单步 GN	单次 GN + 先验约束	等价 MAP	1 次	—	5-A3 桥梁
BA = 全步 GN/LM	全变量联合优化	同上	依 Schur	—	COLMAP、Ceres BAL

---

§B.23 稀疏线性求解方法对比

方法	类型	适用问题	典型 SLAM 规模	库
Dense QR / Cholesky	直接	稠密小问题	\(\le\) 数百变量	Eigen, LAPACK
Sparse Simplicial LL^T / LDL^T	直接	中等稀疏 SPD	1k–100k	Eigen::SimplicialLLT/LDLT
Sparse Supernodal / Multifrontal	直接	大稀疏 SPD	10k–1M	CHOLMOD
Sparse LU	直接	非对称	1k–100k	Eigen::SparseLU, UMFPACK
Sparse QR	直接	最小二乘 / iSAM1	1k–100k	Eigen::SparseQR, SuiteSparseQR
PCG + Jacobi	迭代	超大 SPD、条件数适中	1M+	Ceres CGNR
PCG + Schur-Jacobi	迭代	超大 BA	1M+ 3D 点	Ceres ITERATIVE_SCHUR
PCG + Incomplete Cholesky	迭代	超大差条件 SPD	1M+	g2o PCG

选型经验：BA < 100 相机 → DENSE_SCHUR；100–10000 相机 → SPARSE_SCHUR；> 10000 → ITERATIVE_SCHUR + SCHUR_JACOBI；pose-graph → SPARSE_NORMAL_CHOLESKY。

---

§B.24 GTSAM vs Ceres vs g2o 对比表

见 §B.17。

---

§B.25 SLAM 典型因子类型表

因子	残差	维度	GTSAM 类	g2o 类	Ceres 实现
Prior	\(x\boxminus \bar x\)	dim(x)	PriorFactor<T>	setFixed / 软先验边	自写
Between（SE(3)）	\(\mathrm{Log}(\Delta T^{-1} T_i^{-1} T_j)\)	6	BetweenFactor<Pose3>	EdgeSE3	自写
IMU 预积分	15 维 \([r_R;r_v;r_p;r_{bg};r_{ba}]\)	15	ImuFactor/CombinedImuFactor	自写	VINS imu_factor.h
Visual Reprojection	\(\pi(T^{-1}p) - z\)	2	GenericProjectionFactor<Pose3,Point3,Cal3_S2>	EdgeSE3ProjectXYZ	自写（AutoDiff）
Smart Projection	同上但消路标	2n-3	SmartProjectionPoseFactor	无	无
Bearing-Range	\((\theta - \hat\theta; r - \hat r)\)	2	BearingRangeFactor	EdgeSE2PointXYBearing	自写
GPS / UWB	\(p - z_{\mathrm{GPS}}\)	3	GPSFactor	自写	自写
LiDAR ICP (point-to-plane)	\(n^\top(Tp_s - q_t)\)	1	自写（LIO-SAM）	自写	Problem::AddResidual
Plane Factor	SE(3) 作用平面参数	3	自写	EdgeSE3Plane（types_slam3d_addons）	自写
Marginalization Prior	\(r_p + J_p\,dx\)（线性）	变	LinearContainerFactor	—	VINS MarginalizationFactor

---

§B.26 核心教材/论文表

资源	重点章节	用途	链接
Barfoot State Estimation for Robotics 2ed (2024)	§4.1.1–4.1.3, §9	MAP/GN、因子图、Schur	asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf
Nocedal-Wright Numerical Optimization 2ed (2006)	Ch.3, 4, §10.2–10.3	Line search / trust region / GN / LM	—
Dellaert-Kaess Factor Graphs for Robot Perception FnT 2017	§2–5	因子图、消元、Bayes 树	—
Kschischang, Frey, Loeliger "Factor Graphs and the Sum-Product Algorithm" IEEE T-IT 47(2):498–519, 2001	全	因子图的原始定义论文，sum-product 消息传递 ⭐⭐⭐	doi:10.1109/18.910572
Triggs et al. Bundle Adjustment — A Modern Synthesis 2000	§3, §6	BA 百科全书，Schur	Springer LNCS 1883
Dellaert Hands-on GTSAM Tutorial 2012	全	GTSAM 入门	gtsam.org/tutorials/intro.html
Forster et al. On-Manifold Preintegration T-RO 2017	全	IMU 因子	arXiv:1512.02363
Kaess et al. iSAM2 IJRR 2012	全	增量 + Bayes 树	—
Kümmerle et al. g2o ICRA 2011	全	超图框架	—
Qin-Li-Shen VINS-Mono T-RO 2018	§VI	滑窗 + Marg + FEJ	arXiv:1708.03852
Davis Direct Methods for Sparse Linear Systems SIAM 2006	Ch.4–7	Cholesky / AMD / COLAMD	—
Madsen-Nielsen-Tingleff Methods for NLS IMM DTU 2004	§3.2–3.3	Nielsen LM、Dogleg	www2.imm.dtu.dk/pubdb/edoc/imm3215.pdf
Saad Iterative Methods 2ed (2003)	Ch.9–10	PCG、预条件	www-users.cse.umn.edu/~saad/IterMethBook_2ndEd.pdf
Thrun-Burgard-Fox Probabilistic Robotics	Ch.11–12	GraphSLAM / SEIF	—
Koller-Friedman PGM (2009)	Ch.4, 9	MRF / VE / moralization	MIT Press
高翔 视觉 SLAM 十四讲 2ed	Ch.6, 9, 10, 11	中文入门	github.com/gaoxiang12/slambook2
崔华坤 VINS-Mono 公式推导与代码解析	全	中文 VINS	CSDN

---

§B.27 C++/Python 库映射表

库	核心类/头文件	安装	Python
GTSAM	NonlinearFactorGraph, Values, ISAM2, ImuFactor	apt install libgtsam-dev / 源码	pip install gtsam
Ceres	Problem, AutoDiffCostFunction, Manifold, Solver::Options	apt install libceres-dev	pyceres（第三方）
g2o	SparseOptimizer, VertexSE3Expmap, EdgeSE3ProjectXYZ, BlockSolver_6_3	源码编译	g2o-python
Eigen	SparseMatrix, SimplicialLDLT, CholmodDecomposition	header-only	—
SuiteSparse	cholmod_factorize, cholmod_solve, AMD/COLAMD	apt install libsuitesparse-dev	scikit-sparse
Sophus	SE3d, SO3d, exp/log	header-only	sophuspy
OpenGV	absolute/relative pose solvers	CMake	pyopengv
COLMAP	SfM pipeline, built on Ceres	apt install colmap	pycolmap

最小依赖栈（VIO 研究）：Eigen + Sophus + GTSAM/Ceres + OpenCV。

---

§B.28 常见陷阱（12 条）

1. 残差符号反向。\(r = h(x) - z\) 还是 \(r = z - h(x)\) 决定 Jacobian 与增量方向相反，混用会导致迭代发散。约定一次，全项目贯彻。

2. 流形链式法则漏项。对 SE(3) 残差 \(r = \mathrm{Log}(\Delta T^{-1} T_i^{-1} T_j)\) 求 \(\partial r/\partial T_i\)，必须包含 \(\mathrm{Ad}\) 项与 \(J_r^{-1}\)（右雅可比逆），否则数值与解析 Jacobian 差异巨大。

3. Schur 消元顺序导致 fill-in 爆炸。在 Ceres 中把 camera 放 group 0 而非 point，会先消相机留下稠密 camera-camera block；正确做法是**把 point 放 group 0**（或 g2o setMarginalized(true) on landmark）。

4. 边缘化先验未 FEJ。VINS 类滑窗 VIO 若对所有因子都用最新估计线性化，不可观方向（global yaw、XYZ）会虚假\"获得信息\"，系统 over-confident，长时间运行发散。修复：prior 因子 FEJ，残差按 \(x_0\) 线性化。

5. BA 初值过差 LM 陷局部最优。Pose-graph 有 \(2^n\) 量级局部极小（Carlone 2014）。应先用 EPnP/DLT/VIO 给好初值，或用 certifiable SDP 松弛（SE-Sync）做全局验证。

6. 协方差 vs 信息矩阵混淆。GTSAM noiseModel::Gaussian::Covariance(Σ) 传协方差，Information(Λ) 传信息矩阵——传错导致不确定性反演。在配置文件里**一律写 sigma 或 variance**，内部转换。

7. Pose3 切向量顺序不一致。GTSAM 用 \([\omega;\rho]\)（旋转在前），Ceres/Eigen 约定常是 \([\rho;\omega]\)。跨库桥接协方差/Jacobian 时必须做 \(6 \times 6\) permutation。

8. IMU 预积分 bias 修正漏写。优化迭代中 bias 更新后必须调用 biasCorrectedDelta(bias)（GTSAM）或对应函数；否则预积分量仍停留在旧 bias，残差失真。

9. g2o setFixed(true) vs GTSAM PriorFactor 误用。setFixed 是数学上把变量当常量（0 自由度），GTSAM PriorFactor 是软约束（可被其他因子推动）。Gauge fixing 用前者；锚定不确定性用后者。

10. LinearSolverType 选错。在 pose-only 图（无路标）用 SPARSE_SCHUR 导致退化；在 BA 用 SPARSE_NORMAL_CHOLESKY 不启用 Schur 会慢 100×。规则：有路标且量大 → SPARSE_SCHUR；否则 → SPARSE_NORMAL_CHOLESKY。

11. AutoDiffCostFunction 维度模板参数错。AutoDiffCostFunction<F, 2, 6, 3> 必须与 Functor operator()(const T* p, const T* x, T* r) 的 dims 一致；否则编译不报错但运行时 Jacobian 错乱。

12. Ceres 2.2 移除 LocalParameterization。旧代码 problem.SetParameterization(x, new EigenQuaternionParameterization()) 会编译失败，需改为 problem.SetManifold(x, new EigenQuaternionManifold())。

13. Huber 阈值单位。GTSAM Huber::Create(k) 中 \(k\) 是 whitened 残差范数空间（即 \(k\) 个 sigma）；Ceres 的 LossFunction 接收 \(s=\|r\|^2\)，但 HuberLoss(a) 的参数 \(a\) 仍是残差范数尺度，转折点在 \(s=a^2\)。搞错会让鲁棒核几乎失效或过度抑制。

14. ISAM2::update 遗漏新变量的初值。ISAM2 要求 new factors 涉及的 new keys 必须同时出现在第二参数 newTheta 里，否则 \"ValuesKeyDoesNotExist\"。

---

§B.29 关键公式速查表

名	公式
MAP	\(\hat x = \arg\min \tfrac12 \sum \|r_k\|_{\Sigma_k^{-1}}^2\)
GN 正规方程	\((J^\top \Omega J)\,\delta = -J^\top\Omega r\)
LM 阻尼	\((J^\top\Omega J + \lambda D)\,\delta = -J^\top\Omega r\)
增益比	\(\rho = [F(x)-F(x+\delta)]/[L(0)-L(\delta)]\)
Schur 补	\(S = H_{pp} - H_{pl} H_{ll}^{-1} H_{pl}^\top\)
边缘化	\(\Lambda_p = \Lambda_{rr} - \Lambda_{rm}\Lambda_{mm}^{-1}\Lambda_{mr}\)
FEJ 残差	\(r(x) = r_p + J_p(x - x_0)\)
SE(3) Between 残差	\(r = \mathrm{Log}(\Delta T^{-1} T_i^{-1} T_j)^\vee\)
重投影残差	\(r = \pi(T^{-1} p_w) - z_{\mathrm{pix}}\)
IMU 预积分 \(\Delta R\)	\(\Delta R_{ij} = \prod \mathrm{Exp}((\tilde\omega - b^g)\Delta t)\)
Bias 一阶修正	\(\Delta \tilde R(\hat b) \approx \Delta\tilde R(\bar b)\,\mathrm{Exp}(J_b\,\delta b)\)
PCG 收敛率	\(\|x_k-x^\star\|_H \le 2(\tfrac{\sqrt\kappa-1}{\sqrt\kappa+1})^k\|x_0-x^\star\|_H\)
Armijo	\(f(x+\alpha p)\le f(x)+c_1\alpha\nabla f^\top p\)
Cauchy point	\(p^C = -\tau\,\dfrac{\Delta}{\|g\|}g\)
Dogleg	折线 \(\{h_{sd}\to h_\alpha \to h_{gn}\}\)

---

Part 8 · 学习资源与自测

学习视频与中文资源

• Cyrill Stachniss BA 系列（YouTube）：
• The Basics about Bundle Adjustment https://www.youtube.com/watch?v=sobyKHwgB0Y
• The Numerics of Bundle Adjustment https://www.youtube.com/watch?v=LKDLcKrWOIU
• Robot Mapping Playlist: https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_
• Dellaert GTSAM Tutorial：gtsam.org/tutorials/intro.html；MIT 2015 讲座 youtube.com/watch?v=hCxiAkOu07c。
• 高翔 slambook2：github.com/gaoxiang12/slambook2。Ch.6（NLS/Ceres/g2o 拟合）、Ch.9（BA 数据集实战）、Ch.10（g2o pose graph）、Ch.11（Sphere 数据集）。
• 崔华坤 VINS-Mono 推导：blog.csdn.net/qq_41839222/article/details/85793998；CSDN pdf 下载。
• 深蓝学院：视觉 SLAM 理论与实践（高翔主讲），shenlanxueyuan.com。
• 泡泡机器人：B 站公开课合集，含 g2o、ICP、LSD-SLAM 等专题。
• VINS-Mono 源码导读：github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/vins_estimator/src/factor/marginalization_factor.cpp。

---

§B.30 学习时间预算

档位 3（博士入学）：25–30h

• 因子图概念 + §B.1–B.3：3h
• Gauss-Newton + LM + Dogleg 推导 + 习题：5h
• Schur 补 + 边缘化 + FEJ：5h
• GTSAM 2D pose graph 代码实战：4h
• Ceres BAL 数据集 BA：4h
• g2o ORB-SLAM 风格示例：2h
• 阅读 Forster T-RO 2017 + Qin VINS-Mono：4h
• 自测题全做：3h

档位 4 进阶：+10–15h

• 读 Dellaert-Kaess FnT 2017 全 §1–5：5h
• iSAM2 论文深读 + GTSAM ISAM2 代码：3h
• 实现 VIO 最小管线（IMU factor + smart projection + marginalization）：5h
• Ceres ITERATIVE_SCHUR + PCG 大规模 BA：2h

---

§B.31 自测题（10 道）

1. （基础推导） 证明：高斯噪声下 MAP 估计等价于加权非线性最小二乘。写出从 \(p(x|z)\propto p(x)\prod p(z_k|x)\) 到 \(\hat x = \arg\min \tfrac12\sum\|r_k\|_{\Sigma_k^{-1}}^2\) 的完整推导，标明所有假设。

2. （GN 推导） 从一阶 Taylor 展开 \(r(x+\delta)\approx r(x)+J\delta\) 出发，推导 Gauss-Newton 正规方程 \((J^\top\Omega J)\delta = -J^\top\Omega r\)，并说明为何 \(H = J^\top\Omega J\) 相比真 Hessian 忽略了哪一项，在什么条件下该项可忽略。

3. （Schur 加速） 对 BA 问题 \(m\) 相机 + \(n\) 路标（\(n\gg m\)），写出 Hessian 的块结构。证明通过 Schur 消元路标后，需要求解的线性系统维度从 \(6m+3n\) 降至 \(6m\)，并推导总计算复杂度从 \(O((m+n)^3)\) 降至 \(O(m^3 + nm^2)\)。

4. （GTSAM 实战） 用 GTSAM 构建一个 5 节点 + 1 回环的 2D pose graph（节点 1→2→3→4→5→2），里程计 sigma \([0.2,0.2,0.1]\)，锚定节点 1，用 LM 优化并输出协方差矩阵。给出完整 C++ 或 Python 代码。

5. （Ceres 实战） 用 AutoDiffCostFunction 写一个 BA 残差 functor，变量：相机（6 维 [rx,ry,rz,tx,ty,tz]，旋转用 angle-axis）、3D 点（3 维）、像素观测（2 维）。然后配置 SPARSE_SCHUR + SCHUR_JACOBI 并对 BAL problem-49-7776-pre.txt 数据集优化，对比 DENSE_SCHUR 和 SPARSE_SCHUR 速度。

6. （方法对比） 在同一 SLAM 数据集（如 sphere2500 或 Parking Garage）上比较 GN、LM、Dogleg 的收敛曲线（横轴迭代次数，纵轴 cost）。给出 3 种 solver 在**好初值**和**坏初值**下的成功率统计。

7. （SE(3) Jacobian） 推导 BetweenFactor<Pose3> 的残差 \(r = \mathrm{Log}(\Delta T^{-1} T_i^{-1} T_j)^\vee\) 在 GTSAM 右扰动约定下的 Jacobian \(\partial r/\partial \delta\xi_i\) 和 \(\partial r/\partial \delta\xi_j\)，写出完整含 \(J_r^{-1}(r)\) 与 \(\mathrm{Ad}_{T}\) 的表达式。

8. （IMU 预积分） 从 Forster T-RO 2017 Eq. 35-37 出发，推导预积分量 \(\Delta R, \Delta v, \Delta p\) 对 bias 的一阶修正公式（Eq. 40-44）。解释为什么优化迭代中每次 bias 更新无需重积分，但重积分仍在某些情况下必要（提示：bias 变化超过什么阈值？）。

9. （边缘化 FEJ） 写一段伪代码实现 VINS-Mono 风格的 marginalization 因子：(a) 累加 \(A, b\)；(b) Schur 消去旧帧；(c) 特征分解恢复 \(J_p, r_p\)；(d) 下次优化中用 \(x_0\)（线性化点）评估 \(r_p + J_p(x-x_0)\)。解释为什么 IMU 和视觉因子**不**做 FEJ、仅 prior 做 FEJ 的折中理由。

10. （概率诠释） 证明：对线性高斯因子图，消元一个变量（sum-product 消元）等价于对联合信息矩阵做 Schur 补。用 2 变量情形 \(p(x_1,x_2) = \mathcal N(\mu,\Sigma)\) 的例子验证，然后推广到多变量。

---

Part 9 · 范式切换与数值核心的逐步推导

§B.32 从递推滤波到全局平滑：同一个后验的两种组织方式 ⭐⭐⭐

前面的章节已经说明：Kalman 系列方法维护的是当前时刻的边际分布。

现在把这个结论写得更精确。

给定状态链 \(x_0,x_1,\ldots,x_T\)、控制 \(u_0,\ldots,u_{T-1}\)、量测 \(z_1,\ldots,z_T\)，完整后验为

\[ p(x_{0:T}\mid z_{1:T},u_{0:T-1}) \propto p(x_0)\prod_{k=1}^{T}p(x_k\mid x_{k-1},u_{k-1}) \prod_{k=1}^{T}p(z_k\mid x_k). \]

这个式子同时支持两种算法。

**滤波**选择只保留最后一个变量的边际：

\[ p(x_k\mid z_{1:k}) = \int p(x_{0:k}\mid z_{1:k})\,dx_{0:k-1}. \]

**平滑**选择保留整条轨迹的联合分布：

\[ p(x_{0:T}\mid z_{1:T}) \quad\text{直接作为优化对象。} \]

两者不是两个不同问题，而是同一个后验的两种压缩方式。

视角	保留对象	丢失对象	计算好处	代价
滤波	\(p(x_k\mid z_{1:k})\)	历史状态之间的显式关联	每步固定大小	过去线性化不可修正
固定滞后平滑	\(p(x_{k-L:k}\mid z_{1:k})\)	窗口外状态	计算有界	需边缘化先验
全局平滑	\(p(x_{0:k}\mid z_{1:k})\)	无	可回溯修正历史	需稀疏线性代数
增量平滑	Bayes 树上的局部条件分布	无	只更新受影响子树	数据结构复杂

为什么滤波会失去结构？

关键在积分。

当从 \(p(x_{0:k})\) 得到 \(p(x_k)\) 时，所有历史变量都被积分掉。

在线性高斯情形，积分后的结果仍是高斯，所以滤波器只需保存均值 \(\mu_k\) 与协方差 \(P_k\)。

但协方差 \(P_k\) 是稠密的。

稠密的原因不是实现问题，而是概率问题：历史中的每一次观测都通过积分折叠进当前状态，信息被混合在一个矩阵中。

平滑的策略相反。

它不急着积分掉历史变量，而是把每个局部约束保留为一个因子。

因此信息矩阵

\[ \Lambda = J^\top \Omega J \]

仍然继承因子图的稀疏性。

这就是一句话的范式切换：

本质洞察：滤波把历史信息压缩进当前状态的协方差；平滑把历史信息保留为图上的局部因子。前者牺牲结构换取固定维度，后者保留结构换取可重线性化与可回环修正。

用线性系统看这个差别最清楚。

考虑一维常速度链：

\[ x_k = x_{k-1}+w_k,\qquad z_k=x_k+v_k. \]

批量 MAP 的负对数后验为

\[ F(x_{0:T}) = \frac12\|x_0-\bar x_0\|_{P_0^{-1}}^2 +\frac12\sum_{k=1}^{T}\|x_k-x_{k-1}\|_{Q^{-1}}^2 +\frac12\sum_{k=1}^{T}\|z_k-x_k\|_{R^{-1}}^2. \]

把所有 \(x_k\) 堆叠成 \(x=[x_0,\ldots,x_T]^\top\)，Hessian 是块三对角。

三对角来自马尔可夫性：每个运动因子只连接相邻状态。

如果执行 Kalman 滤波，前一步的所有历史因子被折叠为一个预测高斯：

\[ p(x_k\mid z_{1:k-1}) = \mathcal N(\mu_{k|k-1},P_{k|k-1}). \]

更新时只解一个当前状态的小问题：

\[ \min_{x_k} \frac12\|x_k-\mu_{k|k-1}\|_{P_{k|k-1}^{-1}}^2 +\frac12\|z_k-Hx_k\|_{R^{-1}}^2. \]

这看似更便宜，但代价是 \(x_{0:k-1}\) 已经不在优化变量中。

一旦第 \(T\) 时刻发生回环观测 \(z_{T,0}\)，平滑可直接添加因子 \(f(x_T,x_0)\) 并重优化。

滤波必须通过当前状态的协方差把回环影响传回所有历史状态；若历史状态不再保留，就只能引入额外地图变量、稠密交叉协方差或单独的图优化后端。

这解释了现代 SLAM 的常见工程分层：

层	常用方法	作用
前端短期跟踪	EKF/ESKF/光流/ICP	给出好初值与局部约束
后端平滑	因子图 GN/LM/iSAM2	融合长期约束与回环
全局验证	SE-Sync/SDP/GNC	处理坏初值、外点和证书

反事实很重要。

如果只用滤波，不做平滑，回环闭合只能作为当前状态的量测处理。

这会让过去轨迹保持原漂移形状，地图几何出现撕裂。

如果只用全局批量平滑，不做增量结构，每帧都全量分解，实时系统会在关键帧数达到几千后失去帧率。

因此实际系统常把两者组合：ESKF 提供 1 kHz 传播，局部 VIO/scan matching 提供高频约束，因子图以 10-30 Hz 或关键帧频率做平滑。

常见误区

误区	后果	正确理解
平滑一定比滤波慢很多	忽略稀疏性后得出错误结论	平滑的朴素形式慢，稀疏平滑可实时
滤波丢掉历史就一定不准	忽略实时因果需求	短期惯性传播中滤波仍是最合适工具
iSAM2 是滤波器	混淆边际递推与联合 MAP	iSAM2 是增量平滑，维护的是 Bayes 树
回环只需一个强先验	可能把错误回环硬注入	回环要配鲁棒核、PCM/GNC 或证书

练习

1. 对线性链系统写出 \(T=4\) 时的批量 Hessian，标出所有非零块，解释为何它是块三对角。
2. 把同一系统用 Kalman 滤波递推写出 \(P_{k|k-1}\) 与 \(P_{k|k}\)，说明历史信息如何被压缩。
3. 在草稿纸上构造一个 \(x_0\) 与 \(x_5\) 的回环因子，观察批量 Hessian 多出哪个非零块。

---

§B.33 白化残差：为什么不是直接最小化原始误差 ⭐⭐⭐

MAP 推导中最容易被略过的一步是白化。

这一节把它展开。

单个量测模型为

\[ z = h(x)+\epsilon,\qquad \epsilon\sim\mathcal N(0,\Sigma). \]

残差采用本文约定

\[ r(x)=h(x)-z. \]

似然为

\[ p(z\mid x) = \frac{1}{\sqrt{(2\pi)^m|\Sigma|}} \exp\left(-\frac12 r(x)^\top\Sigma^{-1}r(x)\right). \]

取负对数后，与 \(x\) 有关的部分是

\[ \frac12 r^\top\Sigma^{-1}r. \]

如果 \(\Sigma=\sigma^2I\)，白化残差是 \(e=r/\sigma\)；等价地，平方代价是把欧氏平方误差除以 \(\sigma^2\)。

如果 \(\Sigma\) 非对角，残差各分量相关，不能分别除以标准差。

这时需要把协方差写成平方根形式。

常用做法是对信息矩阵做 Cholesky：

\[ \Omega=\Sigma^{-1}=L^\top L. \]

符号注意：这里的 \(L\) 是**上三角矩阵**（即标准下三角 Cholesky 因子的转置）。标准 Cholesky 分解写作 \(\Omega=\tilde L\tilde L^\top\)（\(\tilde L\) 下三角），令 \(L=\tilde L^\top\) 即得本文写法 \(\Omega=L^\top L\)。GTSAM 和 SLAM 文献普遍采用此约定，因为上三角形式 \(R\) 与 QR 分解、Bayes 树中的 square-root information factor 记号自然一致（参见 5-C §C.5、§C.27）。使用时注意区分：\(L\) 上三角，\(\tilde L\) 下三角，二者互为转置。

于是

\[ r^\top\Sigma^{-1}r = r^\top L^\top Lr = \|Lr\|^2. \]

定义白化残差

\[ \boxed{e(x)=Lr(x)} \]

代价就变为普通欧氏最小二乘：

\[ \frac12\|e(x)\|^2. \]

白化不是数值技巧，而是概率坐标变换。

它把一个椭球形的高斯噪声变成单位球形噪声。

原始空间	白化空间
噪声协方差为 \(\Sigma\)	噪声协方差为 \(I\)
等概率面是椭球	等概率面是球
残差单位混合米、像素、弧度	残差单位是 sigma
阈值难以统一	\(\chi^2\) 阈值可统一

这一步直接影响鲁棒核。

Huber 阈值 \(k=1.345\) 的含义不是 \(1.345\) 米，也不是 \(1.345\) 像素。

它的标准含义是 \(1.345\) 个白化标准差。

若一个像素观测的噪声是 \(\sigma=0.5\) px，原始残差 \(1.0\) px 的白化值是 \(2.0\)。

若一个 GPS 观测噪声是 \(\sigma=2\) m，原始残差 \(1.0\) m 的白化值是 \(0.5\)。

同样的原始数值，在概率意义上完全不同。

这解释了为什么多传感器融合必须先白化。

否则高单位量纲的传感器会支配优化。

从白化到 Jacobian

线性化原始残差：

\[ r(x\oplus\delta)\approx r(x)+J\delta. \]

白化后为

\[ e(x\oplus\delta) = Lr(x\oplus\delta) \approx Lr(x)+LJ\delta. \]

因此白化 Jacobian 是

\[ \boxed{J_e=LJ.} \]

正规方程可写成

\[ J_e^\top J_e\delta = -J_e^\top e. \]

展开后就是

\[ J^\top L^\top L J\delta = -J^\top L^\top L r \quad\Longleftrightarrow\quad J^\top\Omega J\delta = -J^\top\Omega r. \]

所以"加权最小二乘"与"白化后普通最小二乘"完全等价。

工程上更推荐白化形式，因为求解器只需处理统一的残差与 Jacobian。

白化矩阵的选择

白化矩阵不唯一。

只要 \(L^\top L=\Omega\)，就有相同二次型。

常见选择如下。

方式	形式	优点	风险
信息 Cholesky	\(\Omega=L^\top L\)	与正规方程直接一致	\(\Omega\) 若近奇异会失败
协方差 Cholesky	\(\Sigma=CC^\top,\ e=C^{-1}r\)	传感器常给 \(\Sigma\)	需要三角求解而非显式逆
对角 sigma	\(e_i=r_i/\sigma_i\)	最快	忽略相关性
特征分解	\(\Sigma=U\Lambda U^\top\)	可处理半正定与零空间	成本高，需阈值截断

不要显式计算 \(\Sigma^{-1}\)。

正确方式是对 \(\Sigma\) 做 Cholesky，然后解三角系统：

\[ C y=r,\qquad e=y. \]

这比显式求逆更稳定，也更接近数值线性代数的标准做法。

与 \(\chi^2\) 检验的关系

若残差模型正确，白化残差满足

\[ e\sim\mathcal N(0,I_m). \]

于是平方马氏距离

\[ s=e^\top e=r^\top\Sigma^{-1}r \]

服从自由度为 \(m\) 的 \(\chi^2_m\) 分布。

这给出统一的门限：

\[ s\le \chi^2_{m,\alpha}. \]

例如 \(m=2\) 的像素观测，\(\alpha=0.95\) 时阈值约为 \(5.991\)。

这不是经验调参，而是概率假设下的显著性检验。

后续 F 章中的 TLS、GNC、PCM 阈值都应尽量落到这一白化坐标系中。

本质洞察：白化把"传感器单位"转换为"统计置信度单位"。没有白化，优化器比较的是米、像素和弧度的裸数值；白化之后，优化器比较的是每个残差相对于自身噪声模型有多异常。

常见误区

误区	现象	修正
直接把像素误差和 IMU 误差相加	某一类因子支配 Hessian	每个因子按协方差白化
手写 \(\Sigma^{-1}\)	小噪声方向数值爆炸	用 Cholesky 三角求解
鲁棒阈值按原始单位设	Huber/DCS 行为不可解释	阈值按白化残差或 \(\chi^2\) 设
半正定协方差不处理零空间	Cholesky 失败	特征截断或添加物理可观先验

练习

1. 给定 \(\Sigma=\begin{bmatrix}4&1\\1&1\end{bmatrix}\)，手算一个可用的白化矩阵，并验证 \(r^\top\Sigma^{-1}r=\|Lr\|^2\)。
2. 对 2D 像素残差，查 \(\chi^2_{2,0.95}\) 与 \(\chi^2_{2,0.99}\)，解释为何 99% gate 更宽松。
3. 解释 Ceres 中如果 CostFunction 返回未白化残差，HuberLoss(1.0) 的物理含义为什么不确定。

---

§B.34 从 MAP 到正规方程：在流形上每一步都发生了什么 ⭐⭐⭐

很多资料写出

\[ H\delta=-g \]

就停止了。

对机器人状态估计来说，还必须回答一个问题：\(x+\delta\) 在 \(SO(3)\)、\(SE(3)\) 上没有意义时怎么办？

答案是使用 retraction。

在流形 \(\mathcal M\) 上，增量 \(\delta\) 活在切空间 \(T_x\mathcal M\)。

更新写作

\[ x^+ = x\oplus \delta. \]

对 \(SE(3)\)，常见右扰动约定为

\[ T^+ = T\operatorname{Exp}(\delta\xi). \]

残差线性化应写成

\[ r(x\oplus\delta) \approx r(x)+J\delta, \qquad J=\left.\frac{\partial r(x\oplus\delta)}{\partial\delta}\right|_{\delta=0}. \]

注意 \(J\) 不是对矩阵元素的普通导数，而是对切空间扰动的导数。

把白化残差 \(e=Lr\) 代入局部二次模型：

\[ m(\delta) = \frac12\|e+J_e\delta\|^2. \]

展开：

\[ m(\delta) = \frac12(e+J_e\delta)^\top(e+J_e\delta). \]

逐项展开：

\[ m(\delta) = \frac12e^\top e +\frac12e^\top J_e\delta +\frac12\delta^\top J_e^\top e +\frac12\delta^\top J_e^\top J_e\delta. \]

因为 \(e^\top J_e\delta\) 是标量，等于其转置 \(\delta^\top J_e^\top e\)，所以

\[ m(\delta) = \frac12e^\top e +\delta^\top J_e^\top e +\frac12\delta^\top J_e^\top J_e\delta. \]

对 \(\delta\) 求梯度：

\[ \nabla_\delta m = J_e^\top e + J_e^\top J_e\delta. \]

令梯度为零：

\[ J_e^\top J_e\delta=-J_e^\top e. \]

因此

\[ \boxed{H=J_e^\top J_e,\qquad g=J_e^\top e,\qquad H\delta=-g.} \]

这就是正规方程。

如果有多个因子，每个因子贡献一个局部 Hessian：

\[ H=\sum_k J_{e,k}^\top J_{e,k},\qquad g=\sum_k J_{e,k}^\top e_k. \]

因此装配 Hessian 的过程就是"把每个因子的局部信息散射到全局矩阵"。

两个变量因子 \(r(x_i,x_j)\) 会贡献四个块：

\[ \begin{aligned} H_{ii} &\mathrel{+}= J_i^\top J_i,\\ H_{ij} &\mathrel{+}= J_i^\top J_j,\\ H_{ji} &\mathrel{+}= J_j^\top J_i,\\ H_{jj} &\mathrel{+}= J_j^\top J_j. \end{aligned} \]

这就是"同一个因子连接的变量在 Hessian 中出现非零块"的代数来源。

Gauge 自由度从哪里来

若没有先验，纯相对位姿图满足

\[ r(T_i,T_j)=\operatorname{Log}(\tilde T_{ij}^{-1}T_i^{-1}T_j) \]

对任意全局变换 \(G\in SE(3)\)，同时左乘所有位姿

\[ T_i' = GT_i \]

不改变相对量：

\[ (GT_i)^{-1}(GT_j)=T_i^{-1}T_j. \]

因此代价函数沿 6 维 gauge 方向完全不变。

Hessian 至少有 6 个零特征值。

这不是数值退化，而是物理不可观。

解决方式有三类：

方法	做法	适用
固定首帧	把 \(T_0\) 设为常量	g2o 常用，简单
加强先验	添加小方差 PriorFactor	GTSAM 常用，可保留协方差语义
零空间投影	显式处理 gauge nullspace	理论分析、协方差评估

若错误地用极小方差先验锚定所有不可观方向，Hessian 条件数会恶化。

若完全不锚定，Cholesky 会报告矩阵非正定。

工程上常用一个适度强的首帧 prior，而不是把每个状态都加硬约束。

与 LM 的关系

当 \(H\) 近奇异或线性模型不可靠时，LM 解

\[ (H+\lambda D)\delta=-g. \]

这有两个效果：

1. 若 \(H\) 有小特征值，\(\lambda D\) 抬高曲率，线性系统更稳定。
2. 若二次模型不可信，\(\lambda\) 变大，步长变小，接近梯度下降。

但 LM 不能修复 gauge 不可观。

它只能让线性系统可逆，不能给不可观方向创造真实信息。

若优化后协方差在 gauge 方向看起来很小，说明先验或阻尼被误读成物理信息。

练习

1. 对一个二元因子 \(r=x_i-x_j-z\)，手算它对 \(H_{ii},H_{ij},H_{jj}\) 的贡献。
2. 证明纯相对 SE(3) 位姿图存在 6 维 gauge 自由度。
3. 比较 setFixed(true) 与软 PriorFactor 对协方差解释的影响。

---

§B.35 Schur 补的条件高斯解释：为什么消元不是近似 ⭐⭐⭐⭐

Schur 补常被当成代数技巧。

在线性高斯情形，它其实是精确概率运算。

考虑二次代价

\[ F(x,y)=\frac12 \begin{bmatrix}x\\y\end{bmatrix}^\top \begin{bmatrix}A&B\\B^\top&C\end{bmatrix} \begin{bmatrix}x\\y\end{bmatrix} - \begin{bmatrix}a\\b\end{bmatrix}^\top \begin{bmatrix}x\\y\end{bmatrix}. \]

假设要消去 \(y\)。

先对 \(y\) 求最优性条件：

\[ \frac{\partial F}{\partial y}=B^\top x+Cy-b=0. \]

若 \(C\succ0\)，

\[ y^\star(x)=C^{-1}(b-B^\top x). \]

代回 \(F\)。

只保留与 \(x\) 有关的二次项：

\[ \frac12x^\top Ax -a^\top x -\frac12(b-B^\top x)^\top C^{-1}(b-B^\top x) +\text{const}. \]

展开最后一项：

\[ -\frac12 b^\top C^{-1}b +x^\top B C^{-1}b -\frac12x^\top B C^{-1}B^\top x. \]

因此消元后的 \(x\) 子问题为

\[ \frac12x^\top(A-BC^{-1}B^\top)x - (a-BC^{-1}b)^\top x +\text{const}. \]

得到 Schur 系统：

\[ \boxed{ (A-BC^{-1}B^\top)x = a-BC^{-1}b. } \]

这与线性方程组分块消元完全一致。

概率上，联合高斯的信息矩阵为

\[ \Lambda= \begin{bmatrix}A&B\\B^\top&C\end{bmatrix}. \]

对 \(y\) 边缘化后，\(x\) 的信息矩阵是

\[ \Lambda_x=A-BC^{-1}B^\top. \]

这不是近似。

近似只发生在非线性问题的线性化那一步；一旦线性化固定，Schur 消元是精确的高斯边缘化。

条件化与边缘化的区别

若把 \(y\) 固定为某个值 \(\bar y\)，则 \(x\) 的 Hessian 仍是 \(A\)。

若把 \(y\) 边缘化掉，则 \(x\) 的 Hessian 变成 \(A-BC^{-1}B^\top\)。

两者不一样。

操作	数学含义	Hessian 变化	SLAM 对应
固定变量	条件分布 \(p(x\mid y=\bar y)\)	保留 \(A\)	gauge fixing、外参固定
消元变量	边缘分布 \(p(x)\)	Schur 补	BA 消路标、滑窗 marg
删除变量与因子	丢弃信息	信息减少	不推荐直接丢旧关键帧

这解释了滑窗 VIO 中 "marginalization prior" 的必要性。

窗口外状态不是被固定，也不是被直接删除，而是通过 Schur 补把它们携带的信息转移到窗口内变量上。

BA 中 Schur 补为何特别强

BA 的变量分为相机 \(c\) 与路标 \(p\)。

路标之间没有直接因子，所以

\[ H_{pp}=\operatorname{blkdiag}(H_{p_1p_1},\ldots,H_{p_np_n}). \]

消路标时需要 \(H_{pp}^{-1}\)。

由于 \(H_{pp}\) 是块对角，这个逆只需要逐个 \(3\times3\) 或 \(1\times1\) 块求解。

对第 \(j\) 个路标，若它被 \(m_j\) 个相机观测，它只会在这些相机之间产生填充。

换句话说，Schur 补把 "相机-路标-相机" 的二跳关系变成 "相机-相机" 的直接关系。

这正是 reduced camera system 的图意义。

Schur 补也会制造稠密先验

边缘化不是免费的。

若被消变量连接了很多保留变量，Schur 补会让这些保留变量两两相连。

例如滑窗 VIO 中边缘化最旧关键帧，如果它连接窗口内所有路标和 IMU 状态，得到的 prior 可能近似稠密。

这就是滑窗系统常见的工程权衡：

选择	好处	代价
窗口大	线性化误差小，信息更完整	Schur prior 大，求解慢
窗口小	实时性好	边缘化更频繁，FEJ 更重要
丢弃弱特征	控制 fill-in	损失视觉约束
只保留关键帧	降低变量数	前端选择质量变关键

本质洞察：Schur 补是在问"如果被消变量总是取其条件最优值，剩余变量看到的等效代价是什么"。它保留了线性高斯意义下的全部边缘信息，但可能把原本局部的图变成稠密约束。

练习

1. 对 \(2\times2\) 标量块 \(\begin{bmatrix}a&b\\b&c\end{bmatrix}\)，手推消去 \(y\) 后的 \(x\) 信息量 \(a-b^2/c\)。
2. 解释为什么 \(a-b^2/c\le a\)，并把它理解为"边缘化增加不确定性"。
3. 画一个路标被 4 个相机观测的 BA 子图，消去路标后画出相机之间的填充边。

---

§B.36 稀疏 Cholesky：从标量消元到 Bayes 树的数值骨架 ⭐⭐⭐⭐

对 SPD 矩阵 \(H\) 做 Cholesky：

\[ H=R^\top R. \]

这看起来是矩阵分解。

但对稀疏 SLAM 来说，它更像图消元。

考虑标量变量 \(x_1,\ldots,x_n\)。

若先消 \(x_1\)，把 Hessian 写成

\[ H= \begin{bmatrix} h_{11} & h_{1S}\\ h_{S1} & H_{SS} \end{bmatrix}. \]

消去 \(x_1\) 后，剩余变量的等效 Hessian 为

\[ H_{SS}'=H_{SS}-h_{S1}h_{11}^{-1}h_{1S}. \]

若 \(x_1\) 的邻居集合是 \(N(1)\)，则 \(h_{S1}h_{11}^{-1}h_{1S}\) 只在 \(N(1)\times N(1)\) 上非零。

因此消去一个变量会把它的所有邻居连成团。

这些新边就是 fill-in。

稀疏 Cholesky 的核心任务不是"怎么分解矩阵"，而是"用什么顺序消元使填充最少"。

消元顺序的反事实

考虑一条链：

\[ x_1-x_2-x_3-\cdots-x_n. \]

若从一端开始消元，每次变量的邻居最多一个未消变量，不产生额外填充。

若先消中间变量 \(x_{n/2}\)，它的两个邻居会被连起来，链被改造成带填充的图。

二维网格更明显。

随意排序会让 Cholesky 逐渐变成稠密矩阵。

嵌套剖分会先消局部块，把分隔集留到最后，使 fill-in 受控。

图结构	好排序	典型复杂度	说明
一维链	从端点到端点	\(O(n)\)	Kalman/RTS 的本质
二维网格	Nested Dissection	\(O(n^{3/2})\)	2D pose graph 近似
三维网格	Nested Dissection	\(O(n^2)\)	3D mapping 更难
BA 二分图	landmark first + camera COLAMD	依相机图	Schur 后再排序

从列 Cholesky 到超节点

逐列 Cholesky 的公式为

\[ R_{ii}=\sqrt{H_{ii}-\sum_{k<i}R_{ki}^2}, \]

\[ R_{ij}=\frac{1}{R_{ii}}\left(H_{ij}-\sum_{k<i}R_{ki}R_{kj}\right),\qquad j>i. \]

若连续多列具有相同的非零结构，逐列处理会重复访存。

超节点方法把这些列合并为一个小稠密块，用 BLAS-3 计算。

这就是 CHOLMOD supernodal 模式比 Eigen simplicial 模式快的原因。

速度来自缓存与稠密矩阵乘法，而不是不同数学。

Multifrontal 的消息传递解释

Multifrontal Cholesky 为每个消元团构造一个 frontal matrix。

一个团消完后，把剩余 separator 上的 Schur 补作为 update matrix 发送给父团。

这与因子图消元完全同构：

Multifrontal 术语	因子图/Bayes 树术语
frontal variables	frontal variables \(F_k\)
update variables	separator \(S_k\)
update matrix	separator 上的新因子
assembly tree	Bayes tree / clique tree
extend-add	收集邻接因子并装配

这也是 C 章 Bayes 树能够增量更新的数值基础。

Bayes 树不是稀疏 Cholesky 之外的另一个概念，而是稀疏 Cholesky 的概率组织形式。

正定性检查

Cholesky 失败通常有四类原因。

失败原因	现象	检查方式
Gauge 未锚定	第一个接近零的 pivot 对应全局位姿	加首帧 prior 或固定变量
观测退化	某些深度/尺度方向无信息	看最小特征值方向
鲁棒核权重过低	重要因子被降权为 0	打印每类因子权重
数值尺度差	pivot 跨越很多数量级	白化、变量尺度归一

LDLᵀ 比 LLᵀ 更适合诊断。

因为 \(D\) 的对角块直接暴露正、零、负曲率。

若出现负 pivot，说明线性系统不是 SPD。

对 GN 的 \(J^\top J\) 来说，负 pivot 通常来自数值误差、错误的鲁棒二阶修正或手写 Hessian 符号错误。

工程边界

稀疏直接法适合中大规模 SLAM，但不是无限扩展。

当 fill-in 使内存超过预算时，应考虑：

1. Schur 后用 PCG 隐式乘 \(S v\)；
2. 用滑窗限制变量数；
3. 分层地图，把局部子图先压缩成相对位姿因子；
4. 用分布式/多机器人 Bayes 树交换 separator 信息；
5. 对离线超大 BA 使用 cluster-Jacobi 或 multigrid 预条件。

这不是数学优劣，而是内存层级决定的工程边界。

练习

1. 对 5 节点链图分别用顺序 \(1,2,3,4,5\) 与 \(3,2,4,1,5\) 消元，画出 fill-in。
2. 解释为什么 BA 必须先消路标，再对相机系统排序。
3. 在一个 GTSAM pose graph 中切换 Ordering::COLAMD 与 Ordering::METIS，统计 nnz(R) 的差异。

---

与其他子任务的桥梁

• ← 5-A（贝叶斯滤波与 IEKF）：5-A4 建立 IEKF = 单步 GN、RTS = 块三对角回代、BA = 全步 GN；本节将滤波视角正式转为优化视角，因子图是同一估计问题的另一种数据结构。

• → 5-C（iSAM2 与 Bayes 树）：本节的变量消元 + Schur 补是 iSAM2 的基石；Bayes 树是\"消元序 + 弦图 + 最大团\"的组合产物。本节结尾处的 §B.11–B.12 已为 Bayes 树的局部更新与 fluid relinearization 搭好台子。

• → 5-E（SDP 松弛与全局最优 SLAM）：§B.8 点名\"坏初值陷入局部极小\"，5-E 用 Lagrangian 对偶 + SDP 松弛（Carlone IROS 2015、Rosen SE-Sync IJRR 2019）给出**可验证全局最优**的解法。

• → 5-F（鲁棒估计）：本节噪声假设严格高斯，但 SLAM 中 outliers（错回环、动态物体、遮挡）极为常见。5-F 讲 M-estimator、switchable constraints、DC-SAM、graduated non-convexity——所有这些都是\"替换 §B.2 里的二次代价为鲁棒代价\"的技术。

---

§B.29b 因子图的历史演进与未来方向 ⭐⭐⭐

从 EKF-SLAM 到因子图：范式转换的动因

SLAM 后端的历史演进清晰地展示了"为什么因子图取代了滤波"：

年代	方法	复杂度	关键局限
1986-2000	EKF-SLAM	\(O(n^2)\) 每步	协方差稠密增长，不支持回环修正历史
2002-2006	FastSLAM (PF+EKF)	\(O(Mn\log n)\)	粒子退化，大地图不收敛
2006-2012	SAM/iSAM/iSAM2	\(O(n)\)-\(O(n^{1.5})\)	需要良好初值（局部最优）
2015-present	因子图 + 鲁棒核	同上 + outlier 处理	计算量仍随地图增大
2019-present	Certifiable SLAM (SDP)	多项式时间	全局最优但目前仅限小/中规模

每一代解决了上一代的什么问题？ - EKF → SAM：解决了"不能修正历史估计"和"协方差稠密增长" - SAM → iSAM2：解决了"每次回环都要全局重算" - iSAM2 → Certifiable：解决了"坏初值陷入局部极小"

因子图的现代扩展（2024-2025）

方向	代表工作	核心思想
可微因子图	Theseus (Meta, 2022), jaxfg	把因子图嵌入自动微分框架，端到端训练
神经因子	NeRF-SLAM, Gaussian-SLAM	用神经网络参数化地图表征，作为因子图中的"特殊因子"
分布式因子图	DC-SLAM, Mr. Bayes	多机器人各自维护局部因子图，通过 separator 交换信息
实时大规模	SuperNoVA (2025)	算法-硬件协同设计，FPGA 加速稀疏 Cholesky
连续时间	Gaussian Process priors	用 GP 替代离散运动因子，支持任意时刻查询

本质洞察：因子图不仅是一种算法，更是一种**思维范式**——把复杂的联合概率问题分解为局部因子的组合。这种分解思想远超 SLAM：它已经被应用于机械臂轨迹优化（GPMP2）、接触力估计、甚至蛋白质结构预测。掌握因子图思维，就掌握了一种通用的"把大问题拆成小块"的方法论。

从因子图到 Bayes 树：为什么需要更进一步

本章建立了因子图+GN/LM 的完整框架。但实时 SLAM 还有一个关键需求没有满足：增量更新。每来一帧新观测，如果重新对整张图做 GN，计算量随地图规模线性增长——最终会超过实时预算。

iSAM2 的解决方案是把因子图的变量消元过程组织成一棵**Bayes 树**（clique tree）。新因子到达时，只需要更新受影响的局部子树（"fluid relinearization"），而非重做全图。这将在下一章 5-C 详细展开。

从本章到 5-C 的桥梁关系： - 本章的 Schur 补 = Bayes 树中"消元一个团"的操作 - 本章的 COLAMD 排序 = Bayes 树的拓扑结构 - 本章的 Cholesky 因子 \(R\) = Bayes 树的根到叶方向的条件分布链

理解了这些对应，Bayes 树就不是一个新概念，而是本章所有工具的**有组织的重新包装**。

---

结语

因子图把 SLAM 的估计问题**从\"滤波更新的递推序列\"重写为\"一张图上的能量最小化\"**，这不是换语法，而是换本体论。好处在四个层面：

概率层面，MAP ↔ 因子图 ↔ Markov 场 ↔ 贝叶斯网，四种等价视角让我们可以随意挑最方便的那个用。算法层面，Gauss-Newton / LM / Dogleg 构成信赖域方法的标准工具箱；其中 LM 因 Nielsen 阻尼策略与隐式信赖域框架，几乎是工业首选。数值层面，Schur 补把 BA 的线性系统降维 2 个数量级，让百万点重建与实时 VIO 成为可能；边缘化则是 Schur 补在滑窗优化中的在线版。工程层面，GTSAM 用因子图 + Bayes 树做到了增量与预积分原生支持，Ceres 用 AutoDiff + Manifold 做到了通用性与易用性，g2o 用 HyperGraph 为 ORB-SLAM 时代提供了轻量选择。

读懂这一节之后，再读 ORB-SLAM3 的 Optimizer.cc、VINS-Mono 的 estimator.cpp、LIO-SAM 的 mapOptimization.cpp，应像读母语——你能立刻指出每段代码对应因子图上哪个节点或哪次 Schur。这就是 5-B 的**可迁移能力**：不只是懂一个算法，而是掌握了整个\"因子图 + 稀疏线性代数 + 信赖域 + 流形\"的思维范式，往下可接 iSAM2、certifiable SLAM、鲁棒优化，往上可接大规模 SfM、NeRF 联合优化、具身智能中的可微物理仿真。

本章常见误解汇总

误解	正确理解
因子图是一种全新的估计理论	因子图是联合后验分解结构的**图表示**，其数学本质仍是 MAP 估计 = 加权非线性最小二乘；新的是表示方式和由此带来的稀疏求解策略，而非概率基础
Gauss-Newton 收敛意味着找到了全局最优	GN 是局部方法，只保证收敛到一阶驻点（梯度为零）；非凸问题（如 SLAM 有回环时）可能存在多个局部极小，初值远离真值时可能收敛到错误解
LM 和 GN 是两种完全不同的求解器	LM 在 GN 的 Hessian 近似上加阻尼 \(\lambda I\)（或 \(\lambda\,\mathrm{diag}(H)\)），当 \(\lambda\to 0\) 时退化为 GN、\(\lambda\to\infty\) 时退化为梯度下降；LM 是 GN 的鲁棒包装而非独立方法
Schur 补只是一种数学技巧，没有物理含义	Schur 补等价于在概率层面**边缘化**一组变量；BA 中消去路标的 Schur 补本质是将路标积分掉后只看相机位姿的边缘分布，这是 SLAM 加速 100x-1000x 的数学基石
边缘化（marginalization）不会丢失信息	精确边缘化不丢信息，但工程中固定线性化点的边缘化（如滑窗中的 Schur 补）会冻结被消变量的线性化点，后续状态变化时无法修正，长期累积导致一致性退化
GTSAM 比 Ceres 更好（或反过来）	两者设计目标不同：GTSAM 以因子图为核心抽象，原生支持增量求解（iSAM2/Bayes 树）和 IMU 预积分；Ceres 是通用 NLLS 求解器，AutoDiff 和 Manifold API 更灵活；选择取决于应用场景而非笼统的"好坏"
因子图中所有因子的权重应该相同	每个因子的权重由其噪声模型 \(\Sigma_k^{-1}\) 决定（白化残差）；不同传感器的量级和精度差异极大，不白化会导致高噪声传感器主导优化结果
因子图优化太慢不能用于实时系统	利用稀疏结构（COLAMD 排序 + 稀疏 Cholesky）、增量求解（iSAM2/Bayes 树只更新受影响的变量）和滑窗策略，因子图优化已在 VINS-Mono、LIO-SAM 等系统中实现实时运行

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关节
优化后某传感器主导全部结果	不同单位残差未白化	1.检查每个因子是否乘 \(\Sigma^{-1/2}\) 2.打印各类因子 \(\|r_k\|\) 量级 3.对齐单位	§B.2
BA Hessian 奇异	gauge 未固定或 landmark 观测不足	1.加首帧 prior 2.检查观测图连通性 3.看 LDL 最小 pivot	§B.9
LM 一直加大阻尼	residual/Jacobian 符号或维度错	1.用数值差分检查 Jacobian 2.打印每步 \(\rho\) 值 3.检查残差维度	§B.5
Schur 补后系统变稠密	消元顺序错误（先消相机留路标）	1.把 landmark 放 group 0 2.检查 COLAMD 输出 3.画 Hessian 稀疏模式	§B.10
边缘化后精度反而下降	FEJ 未生效或特征值过滤阈值不当	1.对比有/无先验的 NEES 2.检查线性化点冻结 3.调整伪逆阈值	§B.14
IMU 预积分残差异常大	bias 修正未更新或预积分周期重叠	1.检查 correctDelta(bias) 调用 2.验证时间区间无重叠 3.打印 \(\Delta b\)	§B.19

⚠️ 陷阱一：残差没有白化就直接套鲁棒核。 Huber 的 \(k=1.345\) 是在**白化残差空间**（单位为 \(\sigma\)）中的阈值。如果残差还是原始物理单位（米、像素、弧度），需要先除以 \(\sigma\) 再判断是否超过 Huber 阈值。否则：像素残差（量级 \(\sim\) 数十）几乎总超 1.345，所有观测被当作 outlier；米制残差（量级 \(\sim 0.01\)）几乎从不超阈值，鲁棒核形同虚设。

⚠️ 陷阱二：把重投影残差写成 3 维齐次量。 实际像素残差通常是二维 \((u-\hat u, v-\hat v)\)，对应的 Jacobian 维度是 \(2\times 6\)（对位姿）或 \(2\times 3\)（对路标）。如果错误使用三维齐次坐标做残差，(1) 多了一个无物理意义的自由度，(2) Jacobian 维度错误导致正规方程结构破坏。

⚠️ 陷阱三：以为 GN 小残差必然二次收敛。 GN 的”接近二次收敛”需要三个条件同时满足：(1) 残差在解处接近零（小残差问题），(2) Jacobian 在解处满秩，(3) 残差函数足够光滑。当 \(\|r(x^\star)\|\) 不为零（大残差问题），GN 退化为线性收敛或甚至不收敛。此时应切换到完整 Newton 法或使用 LM 阻尼。

⚠️ 陷阱四：GTSAM ISAM2::update 遗漏新变量初值。 调用 isam.update(newFactors, newValues) 时，newFactors 中引用的新 key 必须同时出现在 newValues 中。如果漏掉，GTSAM 抛出 ValuesKeyDoesNotExist 异常。这在动态添加路标时特别容易发生——新路标的 factor 加了但初始化 value 忘了。

本质洞察：因子图的核心不是”把所有误差相加”，而是把概率假设、稀疏结构和数值线性代数绑在一起；任何一个因子的单位或维度错，都会污染整张图。调试因子图的第一原则是”逐因子验证”——先让每个因子独立通过数值 Jacobian 检查，再组合。

---

本章小结

知识点	核心内容	解决什么问题	工程落地
因子图定义	二部图：变量节点+因子节点	可视化 MAP 分解结构	GTSAM FactorGraph
MAP = WNLS	负对数后验 = 加权最小二乘	概率→优化的等价	所有后端优化器
Gauss-Newton	\((J^\top\Omega J)\delta=-J^\top\Omega r\)	线性化后求增量	教学基础
Levenberg-Marquardt	加阻尼 \(\lambda D\)	GN 不收敛时的全局策略	Ceres/g2o/GTSAM 默认
Dogleg	折线插值 GN 步和梯度步	每步只解一次线性系统	GTSAM DoglegOptimizer
Schur 补	消路标得相机系统	BA 加速 100-1000x	Ceres SPARSE_SCHUR
边缘化 = 在线 Schur	消旧变量得等价先验	滑窗保持窗外信息	VINS MarginalizationFactor
稀疏 Cholesky	COLAMD 排序 + fill-in 最小化	大规模线性求解	CHOLMOD, Eigen
IMU 预积分因子	关键帧间 IMU 约束	避免重积分	GTSAM ImuFactor
重投影因子	\(\pi(T^{-1}p)-z\)	视觉 BA 核心	Ceres AutoDiff
Smart Factor	消路标的投影因子	VIO pose-only 优化	GTSAM SmartProjectionFactor
残差符号约定	\(r=h(x)-z\) 全局统一	避免 Jacobian 方向错误	项目规范

---

累积项目：本章新增模块

项目：从零构建因子图优化器

本章在前四章的基础上，从滤波范式跨越到优化范式，新增以下模块：

模块	功能	依赖
factor_graph.py	因子图数据结构（变量节点、因子节点、残差计算）	NumPy
gauss_newton.py	Gauss-Newton 求解器（组装 J、r、正规方程、增量）	factor_graph
levenberg_marquardt.py	LM 求解器（Nielsen 阻尼、接受/拒绝判断）	gauss_newton
pose_graph_2d.py	2D pose graph SLAM（SE(2) Between 因子）	factor_graph + LM
schur_complement.py	Schur 补消元（消路标得相机系统）	factor_graph

本章新增任务：

1. 在 gauss_newton.py 中实现：给定残差函数列表和初值，用 GN 迭代求解。验证对二次函数一步收敛。
2. 在 pose_graph_2d.py 中构造一个 10 节点回环 pose graph，用 LM 求解。对比有/无回环因子的结果，验证回环闭合消除累积漂移。
3. 在 schur_complement.py 中对一个 3 相机+5 路标的小 BA 问题，实现 Schur 消元。验证消元前后解的一致性，并比较求解时间。

---

延伸阅读

类型	资源	难度	重点
教材	Barfoot State Estimation for Robotics 2ed (2024) §4, §9	⭐⭐	MAP/GN/因子图
教材	Nocedal-Wright Numerical Optimization 2ed (2006) Ch.3-4, §10	⭐⭐⭐	信赖域/LM/线搜索
综述	Dellaert-Kaess Factor Graphs for Robot Perception FnT 2017	⭐⭐⭐	因子图/消元/Bayes 树
论文	Triggs et al. Bundle Adjustment — A Modern Synthesis 2000	⭐⭐⭐	BA 百科全书
论文	Kaess et al. iSAM2 IJRR 2012	⭐⭐⭐	增量+Bayes 树
论文	Forster et al. On-Manifold Preintegration T-RO 2017	⭐⭐⭐	IMU 因子
论文	Kschischang et al. Factor Graphs and Sum-Product IEEE T-IT 2001	⭐⭐⭐	因子图原始定义
论文	Madsen et al. Methods for NLS DTU 2004	⭐⭐	Nielsen LM/Dogleg
教材	Davis Direct Methods for Sparse Linear Systems SIAM 2006	⭐⭐⭐⭐	稀疏 Cholesky
代码	GTSAM tutorials (gtsam.org)	⭐⭐	因子图入门
代码	Ceres Solver (ceres-solver.org)	⭐⭐	AutoDiff + Manifold
代码	高翔 slambook2 Ch.6, 9-11	⭐⭐	中文 NLS/BA 实战

---

练习

1. 对一个二维 GPS 因子，分别在原始残差和白化残差下写出 Hessian 贡献，说明 \(\sigma\) 如何改变权重。
2. 手推 pinhole projection 的 \(2\times 6\) Jacobian（对 \(SE(3)\) 位姿），并说明为什么不能直接拿三维齐次像素做残差。
3. 构造一个三节点 pose graph，分别固定和不固定首节点，观察 Hessian 秩的变化。解释 gauge freedom 的含义。 4.（跨章综合题）结合 A4 的 IEKF=单步 GN 结论，对同一个 5 步 VIO 问题，分别用 (a) 逐步 IEKF 和 (b) 5 步联合 GN（因子图 BA）求解。比较两者的轨迹精度差异，验证”BA 精度 >= IEKF”。解释精度差异来源于”是否允许重新线性化历史状态”。

5.（⭐⭐⭐）实现 Nielsen LM 策略：对一个简单的曲线拟合问题 \(y=a\exp(-bx)+c\)，用 LM 从不同初值出发求解。绘制 \(\lambda\) 随迭代次数的变化曲线，观察”接受步 \(\lambda\) 减小，拒绝步 \(\lambda\) 增大”的行为。与纯 GN（\(\lambda=0\)）对比，展示 LM 在初值差时的优势。

6.（⭐⭐⭐⭐）阅读 GTSAM 源码中 EliminateQR 函数（gtsam/linear/GaussianFactorGraph.cpp），追踪一个 3 变量因子图的消元过程。画出消元前后的因子图变化，标注每步产生的 fill-in。与本章 §B.10 的理论描述对照。

7.（⭐⭐⭐）对一个 2D pose graph（圆形轨迹+回环），分别用 Euler 初值（前向积分）和带噪声的 ground truth 作为初值，跑 LM 优化。比较两种初值下的收敛速度和最终解。讨论：坏初值如何导致 LM 陷入局部极小？ORB-SLAM3 如何通过 PnP+RANSAC 给出好初值？

8.（⭐⭐⭐⭐）推导 Ceres 的 AutoDiffCostFunction 在内部是如何用双数（dual number）计算 Jacobian 的。对一个简单的 2D 重投影因子，手动实现双数版本的 operator()，验证与数值差分的 Jacobian 一致。讨论 AutoDiff 相比手写解析 Jacobian 的优缺点（精度、速度、维护成本）。

---

§B.31 读者自检清单 ⭐

编号	自检问题	合格标准
1	因子图的二部性	能说出”变量-变量不直连、因子-因子不直连”
2	MAP = WNLS	能从负对数后验推导出加权最小二乘形式
3	GN 正规方程	能写出 \(H\delta=-g\) 并解释 \(H=J^\top\Omega J\)
4	LM 与 GN 的区别	能说出”加阻尼 \(\lambda D\)，接受/拒绝步机制”
5	Schur 补的物理意义	能说出”消路标 = 对相机系统求边际信息”
6	边缘化 = 在线 Schur	能解释滑窗如何通过 Schur 补保留窗外信息
7	COLAMD 的作用	能说出”减少 fill-in = 减少计算量”
8	因子图 vs 滤波	能说出三个根本优势：重线性化、条件独立结构、回环修正
9	鲁棒核的作用	能解释”核函数降低大残差权重以减少外点对解的拉扯”
10	AutoDiff 原理	能说出”前向模式用 dual number 传播导数，无截断误差”
11	信息矩阵稀疏性	能解释”只有直接相连的变量之间有非零块”
12	Gauge freedom	能说出”绝对位姿不可观时需固定一帧或加先验消除零空间”

延伸阅读：因子图在现代 SLAM 系统中的具体应用

以下列出因子图方法在主流开源系统中的典型使用方式，帮助建立从理论到工程的桥梁：

系统	因子图后端	特色因子类型	关键设计决策
ORB-SLAM3	g2o (LM)	重投影因子 + 本质图闭环因子	仅在闭环时做全局 BA，日常用 local BA
VINS-Mono	Ceres / 自研	IMU 预积分因子 + 视觉重投影	滑窗边缘化 + 先验因子保留历史信息
LIO-SAM	GTSAM (iSAM2)	IMU + LiDAR scan-match + GPS	利用 iSAM2 增量更新避免每帧全局优化
Kimera	GTSAM + RPGO	语义因子 + 鲁棒 PCM 后端	在线 iSAM2 + 离线鲁棒验证双层架构

理解这些系统的架构选择，有助于把本章的理论知识转化为可落地的工程方案。

关键工程经验总结：

• 因子图的威力在于其模块化——新增传感器只需新增一种因子类型，无需改动整体求解框架
• 稀疏性是大规模因子图可解的根本原因：信息矩阵的非零元素数量与因子数量线性相关，而非与变量数量的平方成正比
• 初值质量决定了非线性优化能否收敛到全局最优——这也是为什么 SLAM 前端（特征匹配、PnP、scan matching）的精度直接影响后端表现
• 实际系统中，因子图的构建速度往往不是瓶颈，真正的瓶颈是数据关联（哪些观测对应同一路标）和外点检测
• 调试因子图系统时，首先检查单个因子的残差和 Jacobian 正确性（用数值差分验证），再检查全图的收敛行为

---