从零开始理解因子图模型

1. 概述

因子图是一种概率图模型（Probabilistic Graphical Model），提供了一个优雅而强大的框架，将复杂的多传感器状态估计问题变得清晰、可管理且可求解，是用来表示分解关系的二分图，核心作用是将一个复杂的全局概率分布，分解为多个简单的局部因子的乘积，可以用一个比喻来理解：

• 问题： 侦探要破解一个复杂的案件，需要综合所有线索（测量值）来推断出真相（状态变量：凶手、动机、时间等）。
• 传统方法： 侦探把所有线索和所有可能性都放在一张巨大的桌子上，试图理清它们之间所有的关系。
• 因子图方法： 侦探将案件分解成几个独立的子问题（因子），每个子问题都更容易分析和解决，最后，侦探通过综合所有这些子问题的结论，推断全局真相。
  • 不在场证明因子： 只关心嫌疑人和案发时间的关系。
  • 动机因子： 只关心嫌疑人和受害者的关系。
  • 凶器因子： 只关心嫌疑人和凶器来源的关系。

在SLAM/状态估计的语境下：

• 真相（状态变量）： 机器人过去所有时刻的位姿 X₀, X₁, …, Xₖ。
• 线索（测量值）： IMU读数、里程计、视觉匹配、激光雷达匹配、GPS位置、回环检测结果等。
• 子问题（因子）： IMU因子只约束相邻位姿，激光因子只约束相连的位姿，回环因子只约束形成回环的两个位姿。

2. 因子图的机制与原理

2.1 图的构成

因子图是一种二分图，包含两种节点，一个因子节点只连接到它约束的变量节点上：

• 变量节点： 用圆圈 ○ 表示，代表需要估计的状态变量，例如位姿 $x_i$、速度 $v_i$、偏差 $b_i$ 等。
• 因子节点 ： 用小方块 □ 表示，代表对相关联变量的约束，来源于传感器测量或先验知识。

2.2 数学原理：从贝叶斯网络到因子图

状态估计问题的核心是最大后验概率估计（MAP）：给定所有传感器测量 z，找到最可能的状态 X。

$X^{\ast }=arg\underset{X}{\max }P(X|z)$
根据贝叶斯定理得到：P(X|z) ∝ P(z|X) * P(X)，假设测量值相互独立，这个联合概率可以分解为：

$P(X|z)\propto \prod _{k}P(z_k|X_k)$
其中 $X_k$ 是因子 $z_k$ 所约束的变量的子集。

取负对数后，最大化后验概率 P(X|z) 等价于最小化一个负对数似然函数的总和：

$X^{\ast }=arg\underset{X}{\min }\sum _k\underset{因子f_k(X_k)的残差}{\underbrace{||h_k(X_k)-z_k|{|}_{{\sum }_k}^2}}$​

这里：

• $h_k(X_k)$是测量模型函数，表示从状态 $X_k$ 预测的测量值。
• $z_k$ 是实际的传感器测量值。
• $||...|{|}_{\Sigma }^2$ 是协方差加权平方差
• ${\Sigma }_k$ 是协方差矩阵，代表该传感器测量的不确定性
• 每一项 $||h_k(X_k)-z_k|{|}_{\Sigma }^2$就是一个因子 $f_k$ 的残差。

因子图的功能就是直观地表达这个分解后的求和项，每个因子节点 $f_k$ 就对应一个残差项。

3. 因子图的作用

因子图在现代SLAM中扮演着优化问题描述者的角色，它实现了三大核心功能：

1. 模块化与可扩展性：每种传感器（IMU、Lidar、Camera、GPS）都成为一种独立的因子，可以轻松地“插入”或“拔出”因子图，例如，LIO-SAM在GPS信号良好的地方加入GPS因子，在进入隧道失去信号时就不加入，这种灵活性是传统滤波器难以实现的。

2. 表示稀疏性 ：大多数传感器只提供局部约束（如IMU约束相邻时刻，激光约束当前和地图，回环约束两个相隔较远的时刻），这意味着因子图是稀疏的，即一个因子只连接少数几个变量节点，这种稀疏性是高效优化的关键，著名的SLAM优化库GTSAM（LIO-SAM所用）和Ceres Solver都利用了因子图的稀疏性来极大地加速求解。

3. 全局优化：因子图不是渐进的，当新的因子（如回环因子）加入时，它触发的是对整个轨迹的优化调整，而不仅仅是最近的状态。这可以有效地将回环闭合的校正量传递到整个轨迹，从而最大限度地消除累积误差，这是基于滤波的方法（如LOAM）的短板。

4. 示例说明：基于LIO-SAM的实例

• 场景： 机器人从 x₁ 点移动到 x₄ 点，并在 x₄ 处检测到了与 x₁ 的回环。

• 变量节点（待估计状态）：x₁, x₂, x₃, x₄： 四个关键帧时刻的位姿。

• 因子节点（约束）：

  1. 先验因子f₀： 连接 x₁，提供起始点的绝对坐标（例如从GPS初始化得来），将整个图“锚定”在世界上。
  2. IMU预积分因子 f₁₁, f₁₂, f₁₃： 分别连接 (x₁, x₂), (x₂, x₃), (x₃, x₄)，约束相邻位姿间的相对运动。
  3. 激光里程计因子 fₗ₁, fₗ₂, fₗ₃： 分别连接 (x₁, x₂), (x₂, x₃), (x₃, x₄)，通过点云匹配提供相邻位姿间的相对变换约束。
  4. 回环因子f_loop： 连接 (x₁, x₄)，当检测到回环时，它约束 x₁ 和 x₄ 应该非常接近。

• 优化过程：

1. 在 x₄ 加入之前，因子图优化的是 x₁, x₂, x₃, x₄ 的状态，但由于累积误差，估计的 x₄ 位置可能并不准确。
2. 当回环检测到 x₁ 和 x₄ 应该是同一个点（或非常接近）时，系统添加回环因子 f_loop。
3. 优化器（如GTSAM）开始工作，它要最小化所有因子的残差之和：
   minimize { ||f₀||² + ||f₁₁||² + … + ||fₗ₃||² + ||f_loop||² }
4. 为了满足新的强约束 f_loop（x₁ 和 x₄ 要接近），优化器必须调整所有变量的状态，f_loop 的误差会通过 IMU 和 Lidar 因子构成的路径（x₁->x₂->x₃->x₄）反向传播，从而将 x₂, x₃ 的位姿也进行修正。
5. 最终，整个轨迹被“拉直”，累积误差被有效消除。

这就是因子图的直观体现：一个局部的新信息（回环）可以触发全局的调整，从而得到全局一致的最优估计。

总结

特性	说明
是什么	一种表示全局概率分布分解为局部因子乘积的二分图模型。
节点	变量节点（状态）、因子节点（约束/测量）。
原理	将最大后验概率估计（MAP） 问题转化为最小化所有因子残差平方和的非线性最小二乘问题。
功能	1. 模块化：轻松集成多传感器。 2. 稀疏性：实现高效优化。 3. 全局性：支持回环校正等全局优化。
优势	比滤波器方案更灵活、更精确，能更好地处理回环和全局一致性。