3. 概率图模型基础

SLAM问题的本质是状态估计：我们需要从一系列带噪声的传感器观测中，推断机器人轨迹和地图特征的最优值。概率图模型（Probabilistic Graphical Model, PGM）为这类问题提供了统一的数学语言和结构化表示框架。在SLAM中，我们面对的变量维度极高（例如包含数万个位姿和路标点），直接操作联合概率分布几乎不可能。概率图模型通过将联合分布分解为局部函数的乘积，使得建模、推理和学习变得可行。本章将系统梳理贝叶斯网络、马尔可夫随机场与因子图三种核心表示，重点阐述因子图在SLAM中的核心地位——它不仅是g2o、GTSAM等主流优化库的底层数据结构，也是连接概率推理与非线性优化的桥梁。

上图展示了三种概率图模型的核心差异：贝叶斯网络用有向无环图表达因果关系，马尔可夫随机场用无向图表达变量间的相互约束，而因子图则通过显式的因子节点将全局函数分解为局部函数的乘积。在SLAM中，因子图因其天然的稀疏性和对非线性优化的友好性，成为最主流的表示方式。下面我们逐一深入剖析。

3.1 贝叶斯网络：有向图与条件独立

核心原理

贝叶斯网络（Bayesian Network, BN）是一个有向无环图（DAG），节点表示随机变量，有向边表示条件依赖关系。其联合概率分布可分解为：

P(X1, X2, ..., Xn) = ∏ P(Xi | Parents(Xi))

其中Parents(Xi)是Xi的父节点集合。这种分解直接利用了条件独立性质：给定父节点，每个变量与其非后代节点条件独立。在SLAM早期研究中，贝叶斯网络常用于描述运动模型和观测模型的因果链，例如机器人位姿x_k由x_k-1和控制量u_k决定，观测z_k由x_k和路标l_j决定。

判断方法与适用场景

• 适用条件：当变量间存在明确的因果方向（如时间先后、物理驱动关系）时，BN是自然选择。
• 推理方式：通过贝叶斯公式进行因果推理（从因到果）和诊断推理（从果到因）。
• 局限性：SLAM中大量约束是双向的（如两个位姿间的相对测量），用有向边表达会引入环，导致条件独立性质难以利用。

案例拆解：视觉SLAM中的运动模型

考虑一个简化的视觉里程计系统。设x_k为第k帧的相机位姿，u_k为IMU测量的相对运动。贝叶斯网络可表示为：

x1 → x2 → x3 → ... → xK
↓     ↓     ↓
z1    z2    z3    (观测)

这里x_k只依赖x_k-1和u_k，形成马尔可夫链。但实际SLAM中，同一路标被多帧观测，会产生“x1→l→x3”这样的V型结构，导致条件独立关系复杂化。当闭环检测引入x_K与x₁之间的边时，DAG中出现环，BN无法直接处理。

3.2 马尔可夫随机场：无向图与势能函数

核心原理

马尔可夫随机场（Markov Random Field, MRF）使用无向图表示变量间的相互依赖。联合分布通过定义在团（clique）上的势能函数（potential function）的乘积来分解：

P(X) = (1/Z) ∏_c ψ_c(X_c)

其中Z是配分函数（归一化常数），ψ_c是团c上的非负势能函数。MRF的核心优势在于能够自然地表达“对称”的约束关系——例如两个位姿之间的相对测量，不区分谁是因谁是果。

与贝叶斯网络的对比

案例拆解：位姿图优化

位姿图（Pose Graph）是MRF在SLAM中的典型应用。节点为机器人位姿，边为相对测量约束（如激光匹配结果）。势能函数定义为：

ψ_ij(x_i, x_j) = exp(-0.5 * ||e_ij(x_i, x_j)||²_Σ)

其中e_ij是预测测量与实际测量的残差，Σ是信息矩阵。整个位姿图的联合分布为所有边势能的乘积。然而，直接计算配分函数Z在高维空间中不可行，因此SLAM通常不直接使用MRF进行概率推理，而是将其转化为最小二乘问题求解。

3.3 因子图：统一表示与优化桥梁

核心原理

因子图（Factor Graph）是一种二分图，包含两类节点：变量节点（圆圈表示）和因子节点（方块表示）。全局函数分解为：

f(X1, X2, ..., Xn) = ∏_i f_i(X_i)

其中每个因子f_i连接其涉及的变量子集X_i。因子图的关键优势在于：

• 统一性：BN和MRF都可以转换为因子图，但因子图能表达更一般的分解。
• 稀疏性：每个因子只连接少量变量，对应SLAM中约束的局部性。
• 优化友好：因子图直接对应非线性最小二乘的稀疏雅可比矩阵结构。

因子节点与变量节点的角色

案例拆解：完整SLAM因子图构建

考虑一个单目视觉SLAM系统，包含3个关键帧位姿（x1, x2, x3）和2个路标点（l1, l2）。因子图包含：

变量节点: x1, x2, x3, l1, l2
因子节点:
  f_prior(x1)      - 先验因子，固定初始位姿
  f_odom(x1, x2)   - 里程计因子，来自IMU预积分
  f_odom(x2, x3)   - 里程计因子
  f_obs(x1, l1)    - 观测因子，x1看到l1
  f_obs(x2, l1)    - 观测因子，x2看到l1
  f_obs(x2, l2)    - 观测因子，x2看到l2
  f_obs(x3, l2)    - 观测因子，x3看到l2
  f_loop(x3, x1)   - 闭环因子，检测到回环

每个因子对应一个残差项。例如观测因子f_obs(x_i, l_j)的残差为：

e_ij = z_ij - h(x_i, l_j)

其中z_ij是实际像素观测，h是相机投影模型。所有因子的残差平方和构成总代价函数，通过Levenberg-Marquardt或Gauss-Newton法求解。

3.4 图结构表示与SLAM系统设计

核心原理

因子图的图结构直接决定了优化问题的稀疏模式。在SLAM中，变量节点数量可达数万，但每个因子只连接少量节点，因此雅可比矩阵是高度稀疏的。这种稀疏性使得我们能够利用Cholesky分解或QR分解的高效稀疏版本进行求解。

图结构设计原则

1. 最小化环：虽然因子图允许环存在，但环会增加推理复杂度。在增量平滑（iSAM）中，环会导致重新线性化范围扩大。
2. 因子类型划分：将运动因子、观测因子、闭环因子分开管理，便于鲁棒核函数（如Huber）的独立设置。
3. 变量排序：使用COLAMD等算法对变量节点重排序，减少填充（fill-in），提升稀疏分解效率。

案例拆解：因子图在视觉惯性导航中的应用

以VINS-Mono系统为例，其因子图包含：

• IMU预积分因子：连接连续两帧位姿、速度和偏置，残差包含旋转、速度和位置误差。
• 视觉重投影因子：连接位姿和路标，残差为像素坐标误差。
• 边缘化因子：将旧帧的信息转换为先验因子，保持滑动窗口大小。
• 闭环因子：通过DBoW2检测回环，添加相对位姿约束。

系统初始化时，先通过SFM恢复初始位姿和路标，然后构建因子图并执行非线性优化。每加入新帧，执行增量更新（iSAM2），仅重新计算受影响的部分。

总结复盘