基本概念
图优化（Graph Optimization）是 SLAM 后端中的核心技术，将位姿估计问题建模为一个图结构上的非线性最小二乘问题。图中的节点代表待估计的变量（如相机位姿、地图点位置、IMU 偏置等），边代表传感器提供的约束关系（如视觉观测、里程计测量、回环检测 约束等），每条边携带一个残差函数和对应的信息矩阵。
优化目标是调整所有节点变量，使得所有边上的加权残差平方和最小化：
其中 是第 条边的残差， 是信息矩阵（测量协方差的逆）， 是鲁棒核函数（如 Huber 核），用于抑制外点的影响。
与滤波方法的对比
| 特性 | 图优化 | 滤波（如 EKF） |
|---|---|---|
| 优化范围 | 全部或部分历史状态 | 仅当前状态 |
| 一致性 | 可通过重新线性化改善 | 线性化点固定，可能不一致 |
| 计算模式 | 批处理或增量式 | 递推式 |
| 内存 | 需存储图结构 | 仅维护当前状态和协方差 |
| 精度 | 通常更高 | 受限于首次线性化 |
常用求解库
现代 SLAM 系统常用以下图优化库来构建和求解因子图：
- g2o：ORB-SLAM 系列使用的通用图优化框架，支持自定义节点和边类型
- Ceres Solver：Google 开发的非线性最小二乘求解器，自动微分功能强大
- GTSAM：基于因子图的概率推理库，提供增量式求解（iSAM2），适合在线 SLAM
- Sophus：提供 李群 SE(3)/SO(3) 的操作，常配合上述库进行位姿参数化
在 SLAM 中的应用
图优化在 SLAM 系统中有两种主要形态：
- Bundle Adjustment：同时优化位姿和地图点，残差为 重投影误差，提供最精确的结果但计算开销较大
- 位姿图优化（Pose Graph Optimization）：仅优化位姿节点，边为位姿间的相对变换约束。通常在 回环检测 成功后执行，用于快速消除累积漂移
参考：