海森矩阵（Hessian Matrix）在SLAM图优化和点云配准中的应用介绍

在非线性最小二乘问题中（如 SLAM 或点云配准），通常我们有一个误差函数：

f(x)=∑i∥ei(x)∥2 f(x) = \sum_i \| e_i(x) \|^2 f(x)=i∑∥ei(x)∥2

其中 ei(x)e_i(x)ei(x) 是残差项，对它求 Hessian 就需要用雅可比矩阵：

H=J⊤J+∑iei⊤Hei H = J^\top J + \sum_i e_i^\top H_{e_i} H=J⊤J+i∑ei⊤Hei

通常我们近似为：

H≈J⊤J H \approx J^\top J H≈J⊤J

这就是 高斯-牛顿法 或 Levenberg-Marquardt（LM）方法 中用的近似 Hessian。

---

1、从误差平方和构建优化系统

最小化目标函数：

f(x)=12∑i∥ri(x)∥2 f(\mathbf{x}) = \frac{1}{2} \sum_{i} \| \mathbf{r}_i(\mathbf{x}) \|^2 f(x)=21i∑∥ri(x)∥2

对其做泰勒二阶展开，令：

• Δx\Delta \mathbf{x}Δx：当前状态变量的小增量（在李代数中则是 δξ\delta \xiδξ）
• Ji=∂ri∂x\mathbf{J}_i = \frac{\partial \mathbf{r}_i}{\partial \mathbf{x}}Ji=∂x∂ri：残差对状态的 Jacobian

展开形式如下：

f(x+Δx)≈f(x)+∇f(x)⊤Δx+12Δx⊤HΔx f(\mathbf{x} + \Delta \mathbf{x}) \approx f(\mathbf{x}) + \nabla f(\mathbf{x})^\top \Delta \mathbf{x} + \frac{1}{2} \Delta \mathbf{x}^\top H \Delta \mathbf{x} f(x+Δx)≈f(x)+∇f(x)⊤Δx+21Δx⊤HΔx

---

2、构建雅可比与 Hessian

根据误差平方和结构，可以推导出：

• 梯度项：

∇f(x)=∑iJi⊤ri \nabla f(\mathbf{x}) = \sum_{i} \mathbf{J}_i^\top \mathbf{r}_i ∇f(x)=i∑Ji⊤ri

• Hessian 矩阵（高效构建方式）：

H=∑iJi⊤Ji H = \sum_{i} \mathbf{J}_i^\top \mathbf{J}_i H=i∑Ji⊤Ji

这是 Gauss-Newton 近似的 Hessian（忽略二阶导项 ∂2r∂x2\frac{\partial^2 r}{\partial x^2}∂x2∂2r）。

---

3、图优化中的应用场景

1. GTSAM / Ceres / g2o 中

它们都将优化建模为图结构：

• 节点：状态变量（位姿、地图点）；

• 边：观测（误差函数）；

• 每条边贡献一个残差项 ri(x)r_i(\mathbf{x})ri(x)，其 Jacobian 用于构建：

  • 梯度向量 b=−∑iJi⊤ri\mathbf{b} = - \sum_i \mathbf{J}_i^\top \mathbf{r}_ib=−∑iJi⊤ri
  • Hessian 矩阵 H=∑iJi⊤JiH = \sum_i \mathbf{J}_i^\top \mathbf{J}_iH=∑iJi⊤Ji

构建最终线性系统：

HΔx=−b \boxed{H \Delta \mathbf{x} = -\mathbf{b}} HΔx=−b

更新状态：

xk+1=xk+Δx \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} xk+1=xk+Δx

在李群中，更新为 Tk+1=exp⁡(Δξ∧)⋅Tk\mathbf{T}_{k+1} = \exp(\Delta \xi^\wedge) \cdot \mathbf{T}_kTk+1=exp(Δξ∧)⋅Tk

G2O中自定义边类中计算 Hessian 的代码示例

以经典的图优化中 EdgeSE2 为例，创建顶点和边。

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam2d/types_slam2d.h>

using namespace g2o;

int main() {
    SparseOptimizer optimizer;
    optimizer.setVerbose(true);

    // 设置线性求解器
    auto linearSolver = g2o::make_unique<LinearSolverDense<BlockSolverX::PoseMatrixType>>();
    auto solver = g2o::make_unique<BlockSolverX>(std::move(linearSolver));
    optimizer.setAlgorithm(new OptimizationAlgorithmLevenberg(std::move(solver)));

    // 添加两个顶点
    auto v1 = new VertexSE2();
    v1->setId(0);
    v1->setEstimate(SE2(0, 0, 0));
    optimizer.addVertex(v1);

    auto v2 = new VertexSE2();
    v2->setId(1);
    v2->setEstimate(SE2(1, 0, 0));
    optimizer.addVertex(v2);

    // 添加边（测量为 v1 到 v2 的位姿）
    auto edge = new EdgeSE2();
    edge->setId(0);
    edge->setVertex(0, v1);
    edge->setVertex(1, v2);
    edge->setMeasurement(SE2(1, 0, 0));
    edge->setInformation(Eigen::Matrix3d::Identity());
    optimizer.addEdge(edge);

    // 初始化完毕，计算 Hessian
    optimizer.initializeOptimization();
    optimizer.computeActiveErrors();

    // 手动计算一次误差、雅可比和 Hessian
    edge->computeError();          // e = prediction - measurement
    edge->linearizeOplus();        // J1, J2

    // 获取雅可比矩阵
    Eigen::Matrix<double, 3, 3, Eigen::ColMajor> J1 = edge->jacobianOplusXi();
    Eigen::Matrix<double, 3, 3, Eigen::ColMajor> J2 = edge->jacobianOplusXj();
    Eigen::Matrix3d Omega = edge->information(); // 信息矩阵

    // 手动构造 H 和 b
    Eigen::Matrix<double, 3, 3> H11 = J1.transpose() * Omega * J1;
    Eigen::Matrix<double, 3, 3> H12 = J1.transpose() * Omega * J2;
    Eigen::Matrix<double, 3, 3> H22 = J2.transpose() * Omega * J2;

    Eigen::Vector3d e = edge->error();

    Eigen::Vector3d b1 = J1.transpose() * Omega * e;
    Eigen::Vector3d b2 = J2.transpose() * Omega * e;

    std::cout << "H11:\n" << H11 << "\n";
    std::cout << "H12:\n" << H12 << "\n";
    std::cout << "H22:\n" << H22 << "\n";
    std::cout << "b1:\n" << b1 << "\n";
    std::cout << "b2:\n" << b2 << "\n";

    return 0;
}

---

2. 在 ICP / 点云优化中

残差形式：

ri=pitarget−T⋅pisource r_i = \mathbf{p}_i^{\text{target}} - T \cdot \mathbf{p}_i^{\text{source}} ri=pitarget−T⋅pisource

残差对位姿变量 δξ\delta \xiδξ 求导，得到 Jacobian，然后构造：

H=∑iJi⊤Ji,b=∑iJi⊤ri H = \sum_i J_i^\top J_i, \quad b = \sum_i J_i^\top r_i H=i∑Ji⊤Ji,b=i∑Ji⊤ri

代码示例（部分内容）

Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();

for (int i = 0; i < N; ++i) {
    Eigen::Vector3d pi = source[i];
    Eigen::Vector3d qi = target[i];

    Eigen::Vector3d transformed = R * pi + t;
    Eigen::Vector3d r = qi - transformed;

    Eigen::Matrix<double, 3, 6> Ji;
    Ji.block<3,3>(0,0) = -Eigen::Matrix3d::Identity();
    Ji.block<3,3>(0,3) = R * skewSymmetric(pi);

    H += Ji.transpose() * Ji;
    b += Ji.transpose() * r;
}

其中 skewSymmetric(p) 为构造叉乘矩阵的函数。

---

---

4、Hessian 矩阵的结构特性

• 稀疏性：每个残差只与少量变量有关（如仅涉及两个相邻位姿），因此 Hessian 是稀疏对称正定矩阵；
• 块结构：Hessian 可分块处理（每个块是某对变量之间的二阶互导）；
• 可增量维护：可通过 iSAM 增量式维护 Hessian / 信息矩阵。

---

5、实际系统构建 Hessian 的代码结构（伪代码）

Matrix H = Zero(n, n); // n 变量总数
Vector b = Zero(n);

for (Residual& r : residuals) {
    Vector r_i = r.evaluate(x);
    Matrix J_i = r.jacobian(x);

    H += J_i.transpose() * J_i;
    b += J_i.transpose() * r_i;
}

Solve(H * dx = -b);
x = x + dx;

---

6、GTSAM 中的 Hessian 与信息矩阵

在 GTSAM 中：

• 每个误差项对应一个 GaussianFactor；

• 系统构建整体的 HessianFactor：

  12x⊤Hx−x⊤b \frac{1}{2} \mathbf{x}^\top H \mathbf{x} - \mathbf{x}^\top \mathbf{b} 21x⊤Hx−x⊤b

用于求解最大后验估计（MAP）问题。
获取信息矩阵代码示例

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/HessianFactor.h>

using namespace gtsam;

// 假设你已有 factorGraph 和初始值 initialEstimate
NonlinearFactorGraph graph;
Values initialEstimate;

// Step 1: 线性化
GaussianFactorGraph::shared_ptr linearGraph = graph.linearize(initialEstimate);

// Step 2: 转为单个 HessianFactor（组合所有项）
boost::shared_ptr<HessianFactor> hessianFactor = 
    boost::dynamic_pointer_cast<HessianFactor>(linearGraph->toHessianFactor());

// Step 3: 提取 Hessian 矩阵 H 和向量 b
Matrix H;
Vector b;
Vector d; // 误差项常数项
hessianFactor->info(H, b, d); // info = 信息矩阵（H）

std::cout << "Hessian Matrix H:\n" << H << std::endl;
std::cout << "Gradient vector b:\n" << b << std::endl;

---

7、相关优化方法对 Hessian 的依赖

方法	Hessian 使用方式
Gauss-Newton	使用近似 Hessian H=J⊤JH = J^\top JH=J⊤J
Levenberg-Marquardt	在 Gauss-Newton Hessian 上加阻尼项 H+λIH + \lambda IH+λI
Dogleg	使用 Hessian 构建信赖域
Ceres Solver	自动构建稀疏 Jacobian 与 Hessian，支持 Dense/Schur

---

8、总结表格

内容	作用
Hessian 矩阵 HHH	描述误差函数的局部二阶结构
如何构建	H=∑iJi⊤JiH = \sum_i J_i^\top J_iH=∑iJi⊤Ji，基于残差 Jacobian
在线性系统中作用	解 HΔx=−bH \Delta x = -bHΔx=−b，得到优化步长
稀疏性	对图优化系统极为重要，可用稀疏求解器（如 CHOLMOD、Eigen::SimplicialLDLT）
GTSAM 应用	HessianFactor、GaussianFactorGraph、稀疏线性解系统
点云注册中作用	提高配准精度、支持高效的刚体估计

---