4、图优化基础：图模型、节点与边、误差函数、信息矩阵、最小二乘问题的一般形式
在3D激光SLAM的后端优化中，图优化（Graph-based Optimization）是当前工程实践与学术研究的主流框架。无论是GTSAM还是g2o，其底层都建立在“将状态估计问题建模为图结构，并转化为最小二乘问题求解”这一核心思想上。本章将系统拆解图优化的五个基本要素：图模型、节点与边、误差函数、信息矩阵以及最小二乘问题的一般形式，帮助你在进入具体代码与算法之前，建立清晰、牢固的数学与结构认知。
4.1 图模型：从SLAM问题到图结构的映射
图优化将复杂的SLAM状态估计问题转化为一个直观的图结构。在这个图中，节点（Vertex）代表待优化的变量，边（Edge）代表变量之间的约束关系。这种建模方式的最大优势在于：它将SLAM问题中“时间顺序”和“空间关联”解耦，使得回环检测、多传感器融合等复杂约束可以自然地加入图中。
在3D激光SLAM中，典型的图模型包含两类节点：位姿节点（表示激光雷达在不同时刻的6自由度位姿，即平移x,y,z和旋转roll,pitch,yaw）和路标节点（表示环境中特征点的三维坐标）。边则根据约束来源分为：连续帧约束（由帧间配准如ICP、NDT产生）、回环约束（由回环检测算法产生）、路标观测约束（由特征匹配产生）以及先验约束（如GPS、IMU提供的位置信息）。
4.2 节点与边：变量与约束的数学定义
在g2o中，节点通过VertexSE3或VertexPointXYZ等类表示，每个节点内部存储待优化的参数向量。边的定义则更为关键，它需要实现两个核心函数：误差计算函数（计算当前状态下的残差）和雅可比矩阵（误差对状态的导数）。
以激光SLAM中最常见的位姿-位姿边为例，假设我们有相邻两帧的位姿节点Ti和Tj，通过帧间配准得到的相对变换为Zij（观测值）。那么误差函数定义为：
e_ij = log( Z_ij⁻¹ · (T_i⁻¹ · T_j) )∨其中log(·)∨将SE(3)变换映射到6维向量空间（平移3维+旋转3维）。这个误差的物理含义是：当前估计的位姿关系与观测到的相对变换之间的偏差。当误差为零时，说明当前位姿估计完全符合帧间配准的结果。
| 边类型 | 节点连接 | 观测值来源 | 误差维度 |
|---|---|---|---|
| 位姿-位姿边 | Ti → Tj | ICP/NDT帧间配准 | 6维 |
| 位姿-路标边 | Ti → pk | 特征点观测 | 3维 |
| 回环边 | Ta → Tb | 回环检测+配准 | 6维 |
| 先验边 | Ti → 固定点 | GPS/IMU/磁力计 | 3或6维 |
4.3 误差函数与信息矩阵：约束的“质量”表达
误差函数e(x)描述了当前状态与观测的偏离程度，但并非所有约束都同等重要。例如，激光帧间配准在特征丰富的环境中精度高，在走廊等退化环境中精度低。信息矩阵Ω正是用来量化这种“可信度”的数学工具，它定义为观测协方差矩阵的逆：Ω = Σ⁻¹。
信息矩阵在优化中的实际作用体现在目标函数中：
F(x) = Σ eᵢ(x)ᵀ · Ωᵢ · eᵢ(x)当某个约束的信息矩阵较大时，意味着该约束的协方差小（观测精度高），优化算法会赋予它更大的权重，迫使状态估计更靠近该约束。反之，信息矩阵较小的约束（如退化环境中的配准结果）对优化的影响就小。
4.4 最小二乘问题的一般形式与求解框架
将所有约束的误差项累加，图优化最终归结为求解一个非线性最小二乘问题：
x* = argmin_x Σ eᵢ(x)ᵀ Ωᵢ eᵢ(x)由于误差函数通常是非线性的（涉及旋转矩阵、李代数映射等），无法直接求闭式解。工程中普遍采用高斯-牛顿法（Gauss-Newton）或列文伯格-马夸尔特法（Levenberg-Marquardt）迭代求解。其核心思想是：在每次迭代中，将误差函数在当前状态附近线性化（一阶泰勒展开），然后求解一个线性最小二乘问题来更新状态增量。
以高斯-牛顿法为例，迭代步骤为：
- 给定初始估计x0（通常由里程计提供）
- 对每个边计算误差ei和雅可比矩阵Ji
- 构建线性系统：H·Δx = b，其中H = Σ JᵢᵀΩᵢJᵢ，b = -Σ JᵢᵀΩᵢeᵢ
- 求解Δx，更新状态：x ← x + Δx
- 重复2-4步直到收敛（Δx足够小或达到最大迭代次数）
这里的H矩阵（海森矩阵的近似）正是图优化稀疏性的体现——由于每个边只连接少量节点，H矩阵是稀疏的，这使得我们可以使用稀疏Cholesky分解或共轭梯度法等高效算法求解大规模问题。
4.5 案例拆解：回环闭合中的图优化实战
为了将上述概念串联起来，我们分析一个典型的回环闭合场景。假设一个机器人绕行一圈，激光里程计累计漂移导致起点和终点位姿不一致（轨迹不闭合）。此时系统检测到回环，并计算出一个回环约束。
场景参数：轨迹包含2000个位姿节点，回环约束连接第50帧和第1950帧，回环配准给出的相对变换为Zloop，信息矩阵Ωloop根据配准分数设置为对角矩阵，平移部分权重为100（对应0.1m标准差），旋转部分权重为400（对应0.05rad标准差）。
优化过程：
- 初始状态下，回环边的误差eloop较大（因为漂移导致两帧位姿估计偏差大）
- 构建H矩阵时，回环边对应的Jloop将第50帧和第1950帧的变量关联起来
- 求解线性系统后，Δx会同时调整这两个节点及其之间的所有节点（通过链式传播）
- 经过3-5次LM迭代，轨迹被“拉回”闭合状态，回环误差显著下降
结果对比：优化前，起点和终点位置偏差约1.8m；优化后，偏差降至0.05m以内。同时，地图中原本错位的墙面、柱子等结构得到对齐。
4.6 总结与工程要点
图优化的核心价值在于将SLAM问题转化为一个可扩展、可增量更新的优化框架。理解节点、边、误差函数、信息矩阵和最小二乘形式之间的关系，是掌握g2o和GTSAM的关键前提。在实际工程中，需要注意以下几点：
- 节点参数化：3D激光SLAM中位姿通常使用SE(3)或四元数+平移表示，优化时需注意旋转的流形更新（避免欧拉角的万向锁问题）
- 边的时间效率：每条边的误差计算和雅可比求解应尽量高效，因为优化过程中会反复调用
- 信息矩阵的初始化：可以从配准算法的输出中提取（如ICP的协方差估计），也可以根据经验设定后在线调整
- 稀疏性利用：g2o和GTSAM都提供了稀疏求解器，但需要正确设置节点的“排序”策略以获得最佳性能