从零开始理解因子图模型

1. 概述

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

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

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

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

2. 因子图的机制与原理

2.1 图的构成

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

• 变量节点： 代表需要估计的状态变量，例如位姿 xi、速度 vi、偏差 bi 等。
• 因子节点 ： 代表对相关联变量的约束，来源于传感器测量或先验知识。

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

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

根据贝叶斯定理得到：P(X|z) ∝ P(z|X) * P(X)，假设测量值相互独立，这个联合概率可以分解为：

其中 Xk 是因子 zk 所约束的变量的子集。

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

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

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. 最终，整个轨迹被“拉直”，累积误差被有效消除。

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

总结