海森矩阵（Hessian Matrix）在SLAM图优化和点云配准中的应用介绍_hessian 矩阵 非线性求解 hx=b

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

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

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

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

通常我们近似为：

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

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

---

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

最小化目标函数：

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

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

• $\Delta \mathbf{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Δxf(\\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⊤JiH = \\sum_{i} \\mathbf{J}_i^\\top \\mathbf{J}_iH=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=−∑i​Ji⊤​ri​
  • Hessian 矩阵 H=∑iJi⊤JiH = \\sum_i \\mathbf{J}_i^\\top \\mathbf{J}_iH=∑i​Ji⊤​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 #include #include #include #include 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⋅pisourcer_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⊤riH = \\sum_i J_i^\\top J_i, \\quad b = \\sum_i J_i^\\top r_iH=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}21​x⊤Hx−x⊤b

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

#include #include #include using namespace gtsam;// 假设你已有 factorGraph 和初始值 initialEstimateNonlinearFactorGraph 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 和向量 bMatrix 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+\lambda I$ Dogleg 使用 Hessian 构建信赖域 Ceres Solver 自动构建稀疏 Jacobian 与 Hessian，支持 Dense/Schur

---

8、总结表格

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

---