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

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

$$f(x)=\sum _i\Vert e_i(x){\Vert }^2$$

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

$$H=J^{\top }J+\sum _i{e}_i^{\top }{H}_{e_i}$$

通常我们近似为：

$$H\approx J^{\top }J$$

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

---

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

最小化目标函数：

$$f(\mathbf{x})=\frac{1}{2}\sum _i\Vert {\mathbf{r}}_i(\mathbf{x}){\Vert }^2$$

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

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

展开形式如下：

$$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}$$

---

2、构建雅可比与 Hessian

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

• 梯度项：

$$\nabla f(\mathbf{x})=\sum _i{\mathbf{J}}_i^{\top }{\mathbf{r}}_i$$

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

$$H=\sum _i{\mathbf{J}}_i^{\top }{\mathbf{J}}_i$$

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

---

3、图优化中的应用场景

1. GTSAM / Ceres / g2o 中

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

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

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

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

  • 梯度向量 $\mathbf{b}=-{\sum }_i{\mathbf{J}}_i^{\top }{\mathbf{r}}_i$
  • Hessian 矩阵 $H={\sum }_i{\mathbf{J}}_i^{\top }{\mathbf{J}}_i$

构建最终线性系统：

$$\boxed{H\Delta \mathbf{x}=-\mathbf{b}}$$

更新状态：

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

在李群中，更新为 ${\mathbf{T}}_{k+1}=\exp (\Delta {\xi }^{\wedge })\cdot {\mathbf{T}}_k$

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);// 初始化完毕，计算 Hessianoptimizer.initializeOptimization();optimizer.computeActiveErrors();// 手动计算一次误差、雅可比和 Hessianedge->computeError();          // e = prediction - measurementedge->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 和 bEigen::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 / 点云优化中

残差形式：

$$r_i={\mathbf{p}}_i^{\text{target}}-T\cdot {\mathbf{p}}_i^{\text{source}}$$

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

$$H=\sum _i{J}_i^{\top }{J}_i,b=\sum _i{J}_i^{\top }{r}_i$$

代码示例（部分内容）

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：

  $$\frac{1}{2}{\mathbf{x}}^{\top }H\mathbf{x}-{\mathbf{x}}^{\top }\mathbf{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^{\top }J$
Levenberg-Marquardt	在 Gauss-Newton Hessian 上加阻尼项 $H+\lambda I$
Dogleg	使用 Hessian 构建信赖域
Ceres Solver	自动构建稀疏 Jacobian 与 Hessian，支持 Dense/Schur

---

8、总结表格

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

---