HessianFactor在GTSAM中的数学原理与应用实践

1. HessianFactor的本质与数学基础

1.1 二次型误差因子解析

在GTSAM框架中，HessianFactor代表的是已完成线性化并"平方完成"的高斯因子。其数学本质是一个二次型误差函数，可以表示为：

f(x) = 1/2 [x 1]ᵀ H [x 1]

其中x是拼接后的状态向量（可能包含多个变量），H是增广信息矩阵（augmented information matrix）。这个形式在数值优化中非常常见，特别是在高斯-牛顿法和LM算法中。

展开后等价于：
f(x) = 1/2 xᵀGx - xᵀg + 1/2 f

这个表达式中的三个关键组成部分：

• G：Hessian矩阵（二次项）
• g：信息向量（一次项）
• f：常数项

这种表示方法实际上是高斯-牛顿法中的正态方程形式，它直接对应于最小二乘问题的解析解。

1.2 增广信息矩阵的结构

HessianFactor的核心数据结构是info_成员变量，它是一个块对称矩阵，存储形式如下：

| G11 G12 ... g1 |
| G22 ... g2 |
| ... gk |
| f |

这个矩阵有几个重要特点：

1. 只存储上三角部分（Upper Triangle），但语义上代表完整的对称矩阵
2. 左上角块是Hessian矩阵G
3. 最后一列（不含右下角）是线性项g
4. 右下角是常数项f

这种存储方式既节省了内存，又保持了矩阵的完整数学性质。

2. HessianFactor与JacobianFactor的对比

2.1 表达形式的差异