3、因子图定义与表示：因子图节点（变量节点、因子节点）、边与连接关系、因子图与贝叶斯网络的转换、因子图构建规则
因子图（Factor Graph）是概率图模型中的一种无向图表示方法，它通过显式地表达全局函数（通常是联合概率分布）的因子分解结构，将复杂的多变量推断问题转化为图上的消息传递问题。在多源传感器融合优化中，因子图能够自然地融合IMU、视觉、GPS、轮速计等多种异构传感器的测量信息，并通过增量平滑与优化（如iSAM2）实现实时状态估计。本节将系统阐述因子图的核心定义、节点与边的语义、与贝叶斯网络的转换关系，以及构建因子图时必须遵循的工程规则。
上图展示了因子图与贝叶斯网络在结构上的核心差异：因子图通过因子节点（方块）显式编码测量约束与状态转移关系，而贝叶斯网络通过有向边表达条件依赖。在传感器融合场景中，因子图的无向性使得多源测量（视觉重投影、IMU预积分、GPS位置）可以以统一方式注入优化问题，无需人为指定因果方向，这是其相较于贝叶斯网络的核心优势。
3.1 因子图节点：变量节点与因子节点
核心原理：因子图由两类节点构成——变量节点（Variable Node）和因子节点（Factor Node）。变量节点表示待估计的状态量，如机器人位姿、路标点位置、速度、偏置等；因子节点表示对变量施加的约束，通常对应一个测量模型或先验信息。每个因子节点关联一个局部函数，所有因子的乘积构成全局目标函数：
F(X) = ∏ fᵢ(Xᵢ)其中 Xᵢ 是因子 fᵢ 关联的变量子集。在传感器融合中，每个因子代表一种传感器的测量约束，例如：
- IMU预积分因子：约束相邻时刻位姿、速度和偏置的相对变化。
- 视觉重投影因子：约束相机位姿与路标点之间的几何关系。
- GPS位置因子：直接约束全局位置变量。
- 先验因子：固定初始状态或提供基准参考。
判断方法：在构建因子图时，需要明确区分变量节点和因子节点的角色。变量节点是优化问题的未知数，因子节点是已知的测量约束。一个常见的判断标准是：如果某个量是传感器直接测量得到的，则它应被建模为因子节点；如果某个量需要通过优化求解，则它应被建模为变量节点。例如，GPS输出的经纬度是测量值，应作为因子节点；而机器人的位置坐标是需要估计的，应作为变量节点。
3.2 边与连接关系：因子图的拓扑结构
核心原理：因子图中的边连接变量节点与因子节点，表示该因子依赖于哪些变量。边是无向的，但因子节点内部定义了变量的组合方式。因子图的拓扑结构决定了优化问题的稀疏模式——每个因子只与少量变量相连，从而形成稀疏雅可比矩阵，这是高效求解的关键。
操作流程：构建因子图边连接关系时，遵循以下步骤：
- 确定变量节点集合：根据系统状态定义，列出所有需要估计的变量。
- 确定因子节点集合：根据传感器测量和运动模型，列出所有约束。
- 建立连接：对于每个因子，确定其涉及的变量子集，并添加边。
- 检查连通性：确保图中不存在孤立节点，且每个变量至少被一个因子约束（否则该变量不可观）。
案例拆解：考虑一个简单的二维机器人定位问题，机器人沿直线运动，配备轮速计和GPS。变量节点为 x₁, x₂, x₃（三个时刻的位置）。因子节点包括：先验因子 f₀(x₁) 固定初始位置；运动因子 f₁(x₁, x₂) 和 f₂(x₂, x₃) 来自轮速计；GPS因子 g₁(x₁), g₂(x₂), g₃(x₃) 来自GPS测量。该因子图的边连接关系如下表所示：
| 因子节点 | 关联变量 | 测量来源 | 信息矩阵 |
|---|---|---|---|
| f₀ | x₁ | 初始位置先验 | Σ₀⁻¹ |
| f₁ | x₁, x₂ | 轮速计里程 | Σ_odom⁻¹ |
| f₂ | x₂, x₃ | 轮速计里程 | Σ_odom⁻¹ |
| g₁ | x₁ | GPS测量 | Σ_gps⁻¹ |
| g₂ | x₂ | GPS测量 | Σ_gps⁻¹ |
| g₃ | x₃ | GPS测量 | Σ_gps⁻¹ |
该拓扑结构对应的雅可比矩阵呈现带状稀疏结构，可利用Cholesky分解或QR分解高效求解。
3.3 因子图与贝叶斯网络的转换
核心原理：贝叶斯网络（BN）是有向无环图（DAG），节点表示随机变量，有向边表示条件依赖关系。因子图与贝叶斯网络可以相互转换，但转换过程中信息表达方式发生变化。从贝叶斯网络到因子图的转换规则为：每个条件概率分布 P(Xᵢ|Pa(Xᵢ)) 转换为一个因子节点，连接该变量及其父变量。从因子图到贝叶斯网络的转换则较为复杂，需要引入辅助变量或使用变量消元法。
判断方法：在实际应用中，选择哪种图模型取决于任务需求：
- 贝叶斯网络：适合表达因果推理、处理先验知识、进行概率推断（如置信传播）。
- 因子图：适合表达测量约束、进行最大后验估计（MAP）、处理多源传感器融合。
案例拆解：以视觉SLAM为例，贝叶斯网络表达为：P(x₁)P(x₂|x₁)P(x₃|x₂)∏P(zᵢ|xᵢ)，其中 xᵢ 为位姿，zᵢ 为观测。转换为因子图后，每个条件概率 P(xᵢ|xᵢ₋₁) 变为运动因子 f(xᵢ₋₁, xᵢ)，每个观测似然 P(zᵢ|xᵢ) 变为观测因子 g(xᵢ)。转换后的因子图更适合增量优化，因为新增测量只需添加新的因子节点，无需重新构建整个图。
3.4 因子图构建规则与工程实践
核心原理：构建因子图需要遵循一系列工程规则，以确保优化问题的正确性、数值稳定性和计算效率。这些规则涵盖变量定义、因子建模、图结构设计、初始化策略等方面。
操作流程：因子图构建的标准流程如下：
- 定义状态变量：明确系统状态维度，如位姿（位置+姿态）、速度、传感器偏置等。变量应尽量紧凑，避免冗余。
- 设计因子类型：根据传感器模型定义因子函数，包括残差计算、雅可比矩阵推导、信息矩阵设置。
- 确定图结构：根据传感器配置和运动模型，规划变量节点与因子节点的连接关系。
- 初始化变量：提供合理的初始值，避免优化陷入局部极小值。
- 增量更新策略：设计因子图的动态扩展机制，支持新测量到达时的局部更新。
案例拆解：考虑一个无人机多源融合系统，包含IMU（200Hz）、视觉（30Hz）、GPS（10Hz）。构建因子图时，需要处理不同频率的测量：
- IMU因子：在相邻关键帧之间进行预积分，生成相对运动约束。
- 视觉因子：在关键帧之间建立特征匹配，生成重投影约束。
- GPS因子：在GPS测量时刻添加位置约束。
关键设计决策是：选择关键帧作为变量节点，而非每个IMU时刻。这样可以将变量数量控制在合理范围，同时利用IMU预积分保留高频运动信息。因子图构建完成后，使用iSAM2算法进行增量优化，每次新测量到达时只更新受影响的部分变量。
| 构建规则 | 说明 | 常见错误 |
|---|---|---|
| 变量可观测性 | 每个变量至少被一个因子约束 | 孤立变量导致优化发散 |
| 因子独立性 | 不同传感器的因子应相互独立 | 重复使用同一测量导致过约束 |
| 数值稳定性 | 信息矩阵应正定，避免奇异 | 缺乏先验因子导致矩阵奇异 |
| 稀疏性保持 | 因子应只连接局部变量 | 全局连接导致稠密矩阵 |
| 时间一致性 | 因子应正确关联时间戳 | 时间戳错位导致错误约束 |
总结复盘：因子图的构建质量直接决定了多源传感器融合系统的性能上限。优秀的因子图设计应具备以下特征：变量定义紧凑、因子建模准确、图结构稀疏、数值稳定、支持增量更新。在实际工程中，建议先通过仿真验证因子图的可观性和收敛性，再部署到实机系统。同时，应建立因子图的调试工具链，包括残差分析、信息矩阵可视化、因子权重检查等，以便快速定位融合异常。