3、图优化入门：图论基础、因子图与SLAM中的图优化
各位同学好，我是你们的老朋友。今天咱们来聊聊图优化。说实话，我第一次接触这个概念时，也觉得有点抽象。但后来在组合导航项目里吃过亏，才真正体会到它的价值。嗯，咱们一步步来。
3.1 图论基础：节点、边、邻接矩阵
图论听起来高大上，其实说白了就是研究「东西」和「关系」的学问。这里的「东西」叫节点，「关系」叫边。
- 节点（Vertex）：代表一个实体。比如一个位置、一个状态量。
- 边（Edge）：代表节点之间的约束或关系。比如两个位置之间的相对运动。
- 邻接矩阵（Adjacency Matrix）：用矩阵来记录哪些节点之间有边相连。
举个例子。假设我们有三个位置点 A、B、C。A 到 B 有路，B 到 C 有路，但 A 到 C 没有直接的路。那么邻接矩阵就是：
A B C
A 0 1 0
B 1 0 1
C 0 1 01 表示有边，0 表示没有。我在项目中遇到过，很多人把邻接矩阵当成稀疏矩阵来存，结果内存爆了。其实对于组合导航这种场景，节点数通常不多，直接用稠密矩阵反而更简单。
核心要点：图论就是研究节点和边的学问。邻接矩阵是图的一种数学表示，方便计算机处理。
3.2 因子图定义
因子图是图论在概率模型中的一种特殊应用。它由两种节点组成：
- 变量节点（Variable Node）：用圆圈表示，代表我们想估计的未知量。比如位置、速度、姿态。
- 因子节点（Factor Node）：用方块表示，代表变量之间的约束关系。比如 GPS 观测、IMU 预积分。
因子图的核心思想是：把整个问题拆成一个个小因子，每个因子只关心它连接的变量。你想想看，这比直接求解一个巨大的联合概率分布要优雅得多。
我个人的习惯是，先画因子图，再写代码。因为图结构一旦确定，算法框架就清晰了。曾经有一次，我跳过画图直接写代码，结果调试了三天才发现是因子连接错了。嗯，血的教训。
小技巧：画因子图时，变量节点尽量放在中间，因子节点放在周围。这样视觉上更清晰，也方便后续添加新因子。
3.3 因子图与贝叶斯网络的关系
贝叶斯网络是有向图，因子图是无向图。两者都能表示概率关系，但侧重点不同。
| 特性 | 贝叶斯网络 | 因子图 |
|---|---|---|
| 边的方向 | 有向（因果） | 无向（约束） |
| 节点类型 | 只有变量节点 | 变量节点 + 因子节点 |
| 推理方式 | 消息传递（BP） | 和积算法 / 最大和算法 |
| 适用场景 | 因果推理、专家系统 | 传感器融合、SLAM |
为什么会这样？因为贝叶斯网络强调「谁导致了谁」，而因子图强调「谁和谁有关系」。在组合导航中，我们更关心约束关系，而不是因果关系。所以因子图更合适。
我记得有一次和做贝叶斯网络的同事争论，他说因子图就是贝叶斯网络的变种。我说不对，因子图是更通用的框架。后来我们各自用自己熟悉的方法解同一个问题，结果因子图收敛更快。当然，这不代表因子图永远更好，只是在这个场景下更合适。
注意：不要试图把贝叶斯网络直接转换成因子图。两者的语义不同，强行转换可能会丢失信息或引入错误约束。
3.4 SLAM中的图优化简介
SLAM（同步定位与地图构建）是图优化最经典的应用之一。简单来说，SLAM 中的图优化就是：
- 把机器人的位姿作为变量节点
- 把观测（如激光雷达、视觉特征）作为因子节点
- 通过优化所有节点的位置，使得所有因子的误差最小
你想想看，这就像在玩一个拼图游戏。每个因子告诉你两块拼图应该怎么拼，但可能有噪声。图优化就是找到一种拼法，让所有拼图都尽量吻合。
在组合导航中，我们通常把 IMU 预积分作为连续因子，把 GPS 观测作为离散因子。这样既能利用 IMU 的高频信息，又能用 GPS 修正长期漂移。我在项目中遇到过，如果只依赖 IMU，十分钟后位置误差就能漂到几百米。加上 GPS 因子后，误差被牢牢限制在几米内。
下面这张图展示了因子图在组合导航中的基本结构：
图中，x0 到 x3 是不同时刻的位姿。IMU 因子连接相邻位姿，提供连续的运动约束。GPS 因子连接单个位姿，提供绝对位置观测。优化时，所有因子共同作用，找到一组最合理的位姿。
总结一下：图优化就是把 SLAM 问题建模成因子图，然后通过最小化所有因子的误差来求解。这种方法天然支持多传感器融合，是组合导航的利器。
好了，这一章的内容就到这里。图论基础、因子图定义、与贝叶斯网络的关系、SLAM 中的图优化简介，这些概念是后续章节的基石。我个人建议你动手画一画因子图，哪怕只是简单的两三个节点，也能帮你建立直觉。