4. 图优化基础

在视觉SLAM中，回环检测给出“这两帧可能是同一地点”的约束，而真正把位姿轨迹修正到全局一致的，是后端图优化。图优化并不是一个孤立算法，它把SLAM问题统一表达为“变量节点 + 约束边”的概率推断问题，再转化为最小二乘，最后用非线性优化求解。理解因子图、贝叶斯网络、最小二乘、稀疏性与信息矩阵之间的关系，是后续写回环优化器的前提。

一、从贝叶斯网络到因子图：把概率写成乘积

SLAM的完整状态可记为 X = {x_1, x_2, ..., x_n, l_1, ..., l_m}，其中 x_i 是机器人位姿，l_j 是路标。观测为 Z，控制输入为 U。贝叶斯网络用有向图描述条件依赖：位姿之间由运动模型连接，位姿与路标之间由观测模型连接。其联合概率可分解为：

P(X | Z, U) ∝ P(x_0) · Π P(x_i | x_{i-1}, u_i) · Π P(z_k | x_{i_k}, l_{j_k})

因子图是贝叶斯网络的无向二部图表达：圆形节点表示待优化变量，方形因子表示概率约束。每个因子只与其涉及的变量相连，整个后验概率就是所有因子函数的乘积。相比贝叶斯网络，因子图更强调“约束从哪来、连到哪些变量”，非常适合回环这种任意两帧之间的额外约束。

二、最大后验 → 最小二乘

假设各因子服从高斯分布，因子 f_k 的误差为 e_k(X)，信息矩阵为 Ω_k，则负对数后验正比于：

X* = argmin Σ e_k(X)ᵀ Ω_k e_k(X)

这就是非线性最小二乘的标准形式。信息矩阵 Ω 是协方差的逆，表示该约束的“可信程度”：Ω 越大，该因子的误差被惩罚得越重，优化时越倾向于满足它。回环因子的 Ω 通常根据匹配内点数、重投影误差或相似度评分设定。

三、非线性优化：GN 与 LM 的迭代流程

对误差函数在当前估计 X 处一阶泰勒展开：e(X+ΔX) ≈ e(X) + JΔX，其中 J 为雅可比矩阵。代入目标函数并对 ΔX 求导，得到正规方程：

(Jᵀ Ω J) ΔX = -Jᵀ Ω e
H ΔX = b

其中 H = Jᵀ Ω J 称为信息矩阵或 Hessian 近似，b = -Jᵀ Ω e。高斯-牛顿法直接求解该方程；列文伯格-马夸尔特法引入阻尼项 (H + λI)ΔX = b，在梯度下降与高斯-牛顿之间自适应切换，更适合初值较差的回环场景。

1. 构建因子图，确定变量节点与因子节点。
2. 给定初始位姿（通常来自前端里程计）。
3. 计算每个因子的误差 e_k 与雅可比 J_k。
4. 组装稀疏 H 与 b。
5. 求解 H ΔX = b，更新 X ← X ⊞ ΔX（位姿需用流形更新）。
6. 判断收敛：误差下降量、梯度范数或迭代次数满足阈值则停止。

四、稀疏性与信息矩阵：为什么能实时求解

SLAM 因子图天然稀疏：一个位姿只与相邻位姿、少量路标和偶尔的回环相连。因此 H 是稀疏矩阵，非零块集中在对角线附近及回环对应的位置。稀疏性是后端能实时运行的根本原因。

概念	数学对象	在SLAM中的含义	工程影响
因子	误差函数 e_k	里程计、观测、回环约束	决定图的结构
信息矩阵	Ω_k	约束置信度，协方差之逆	影响权重与收敛方向
雅可比	J_k	误差对变量的偏导	决定 H 的稀疏结构
Hessian	H = Jᵀ Ω J	全局信息矩阵	稀疏、对称、半正定
边缘化	Schur 补	消去路标，保留位姿	降低求解规模

实际求解时常用 Cholesky 分解、QR 分解或预条件共轭梯度。变量排序（如 AMD、COLAMD）会显著影响分解产生的填充量。回环会引入远距离连接，使 H 出现更多非零块，因此回环越多，单次优化越慢，但全局一致性越好。

五、案例：一次回环如何改变 H 的结构

假设轨迹有 5 个位姿 x₁…x₅，里程计因子连接相邻位姿，回环因子连接 x₁ 与 x₅。无回环时，H 是三对角块矩阵；加入回环后，H(1,5) 与 H(5,1) 出现非零块，矩阵不再是带状。求解器需要处理这些额外非零元，但正是它们把累计漂移“闭合”回起点。

无回环 H 结构（块三对角）：
■ ■ . . .
■ ■ ■ . .
. ■ ■ ■ .
. . ■ ■ ■
. . . ■ ■

加入 x₁-x₅ 回环后：
■ ■ . . ■
■ ■ ■ . .
. ■ ■ ■ .
. . ■ ■ ■
■ . . ■ ■

六、常见误区与复盘

• 把信息矩阵当成普通权重：它必须与残差量纲匹配，旋转与平移的信息矩阵通常需要分别设置。
• 忽略流形更新：旋转不能直接相加，需用四元数或 SO(3) 的右乘扰动更新。
• 回环因子信息矩阵过大：错误回环会被当成强约束，导致轨迹被拉断。
• 只关注求解器，不关注图结构：变量排序、边缘化策略和因子组织方式往往比换求解器更影响性能。
• 忘记鲁棒核：回环外点较多时，应使用 Huber、Cauchy 等鲁棒核函数降低异常因子影响。