1. 项目概述从理论到实践的SLAM算法复现最近在重读《视觉SLAM十四讲》这本经典教材每次翻到非线性优化那一章特别是关于高斯牛顿法的部分总感觉纸上得来终觉浅。书上的公式推导很清晰但如果不亲手把代码敲一遍把每个矩阵的维度、每个雅可比的计算都捋清楚心里总是不踏实。这就像学游泳看再多教程不下水扑腾几下永远学不会。所以我决定以书中第六章的“手写高斯牛顿法求解曲线拟合问题”为蓝本进行一次彻底的代码复现与笔记整理。这个项目看似简单就是一个拟合yexp(ax^2bxc)w这样的曲线但它麻雀虽小五脏俱全几乎涵盖了视觉SLAM后端优化中最小二乘问题的所有核心要素误差定义、雅可比矩阵推导、增量方程构建与求解。通过这个练习我们能真正理解高斯牛顿法如何将复杂的非线性优化问题转化为一系列线性最小二乘问题去迭代求解这对于后续理解Bundle Adjustment、直接法SLAM等至关重要。无论你是SLAM初学者想夯实基础还是有一定经验想回头补足理论短板跟着这篇笔记一步步推导和实现都会有不小的收获。2. 核心思路与数学模型拆解2.1 问题定义一个经典的非线性最小二乘我们面对的问题非常典型给定一组带噪声的观测数据点去估计一个非线性模型的参数。具体来说我们有N个观测数据点 (x_i, y_i)它们由模型 y exp(ax^2 bx c) 生成并附加了高斯噪声 w。我们的目标是仅从这些观测数据 (x_i, y_i) 中估计出模型参数 [a, b, c]^T。这本质上是一个非线性最小二乘问题。我们需要找到一组参数使得模型预测值与实际观测值之间的误差平方和最小。用数学语言描述就是最小化以下目标函数F(a, b, c) 0.5 * Σ_i ( y_i - exp(a*x_i^2 b*x_i c) )^2这里的 0.5 系数是为了后续求导方便不影响最优解。在SLAM中这个误差项就对应着重投影误差、光度误差等。2.2 高斯牛顿法一阶近似的迭代艺术直接求解这个非线性目标函数的最小值很困难。高斯牛顿法的核心思想是进行局部线性化。假设我们有一个参数初始估计值x [a, b, c]^T我们希望在当前估计值附近找到一个增量Δx使得更新后的参数x Δx能降低目标函数值。具体做法是对误差函数进行一阶泰勒展开。定义第i个数据点的误差为e_i(x) y_i - exp(a*x_i^2 b*x_i c)。那么在当前点x处误差函数关于增量Δx的近似为e_i(x Δx) ≈ e_i(x) J_i(x) Δx其中J_i(x)是误差e_i(x)关于参数x的雅可比矩阵行向量J_i(x) [∂e_i/∂a, ∂e_i/∂b, ∂e_i/∂c]。将所有的误差项堆叠起来记e(x) [e_1(x), ..., e_N(x)]^TJ(x)为整个误差向量的雅可比矩阵N×3维。那么目标函数的增量形式近似为F(x Δx) ≈ 0.5 * || e(x) J(x) Δx ||^2这变成了一个关于Δx的线性最小二乘问题其目标函数是Δx的二次型。为了最小化它我们令其关于Δx的导数为零从而得到著名的增量方程或称正规方程Normal EquationJ(x)^T J(x) Δx - J(x)^T e(x)记H J^T J为高斯牛顿法中的近似海森矩阵Hessiang -J^T e为梯度方向。我们求解这个线性方程H Δx g得到当前迭代步的最优增量Δx然后更新参数x : x Δx。不断重复这个过程直到增量Δx足够小或达到最大迭代次数。注意这里H J^T J是海森矩阵∇^2 F(x)的近似它忽略了误差函数e_i(x)的二阶导数项。这使得计算大大简化只需计算一阶雅可比但同时也带来了一个问题这个近似只有在展开点附近误差较小即e_i(x)较小时才比较准确。如果初始值离最优解太远或者问题本身非线性很强这个近似可能很差导致算法不收敛。这是高斯牛顿法的一个主要缺陷也是后续列文伯格-马夸尔特方法要改进的地方。2.3 与SLAM后端优化的直接关联你可能已经发现了这个曲线拟合问题的数学形式和SLAM后端优化中的Bundle Adjustment (BA) 如出一辙。在BA中我们的参数x变成了所有相机位姿和三维路标点的集合。误差e_i变成了重投影误差观测到的像素坐标减去由当前位姿和地图点投影计算得到的像素坐标。雅可比矩阵J描述了重投影误差如何随相机位姿李代数表示和路标点位置变化。增量方程H Δx g的规模变得极其庞大通常有成千上万个参数但具有特殊的稀疏结构可以利用舒尔消元等技巧高效求解。因此这个手写高斯牛顿拟合曲线的练习是理解大规模视觉SLAM后端优化基石的最佳热身。搞懂了这里每一步的矩阵维度、计算和意义再看g2o、Ceres等优化库的源码就会有一种豁然开朗的感觉。3. 关键实现步骤与代码逐行解析接下来我们抛开任何优化库用纯C配合Eigen库进行矩阵运算来实现这个高斯牛顿法。我会把重点放在容易出错和需要深刻理解的地方。3.1 数据生成模拟真实观测过程任何优化问题都要有数据。我们首先根据真实的参数值生成带噪声的观测数据。这模拟了SLAM中传感器获取带噪声观测值的过程。#include iostream #include cmath #include vector #include Eigen/Core #include Eigen/Dense using namespace std; using namespace Eigen; int main() { // 真实参数值 double ar 1.0, br 2.0, cr 1.0; // 估计参数初始值 (故意给一个偏离真实值的初始值比如全零) double ae 0.0, be 0.0, ce 0.0; int N 100; // 数据点数量 double w_sigma 1.0; // 噪声的标准差 double inv_sigma 1.0 / w_sigma; // 信息矩阵这里简单设为1/sigma cv::RNG rng; // OpenCV随机数生成器也可以用C11的random vectordouble x_data, y_data; // 观测数据容器 for (int i 0; i N; i) { double x i / 100.0; // x在[0, 1]区间均匀分布 x_data.push_back(x); // 生成带噪声的y观测值: y exp(a*x^2 b*x c) noise double y exp(ar * x * x br * x cr) rng.gaussian(w_sigma); y_data.push_back(y); }实操心得噪声水平w_sigma的设置很重要。如果噪声太大可能掩盖信号导致优化困难甚至失败如果噪声太小问题过于简单体现不出优化算法的鲁棒性。w_sigma1.0对于y值范围在几十的量级来说是一个合理的设置能产生肉眼可见的偏差。3.2 迭代优化核心循环高斯牛顿法是一个迭代过程。我们设定一个最大迭代次数和收敛阈值。int iterations 100; // 最大迭代次数 double cost 0, lastCost 0; // 本次迭代成本和上次迭代成本用于判断收敛 for (int iter 0; iter iterations; iter) { Matrix3d H Matrix3d::Zero(); // 近似海森矩阵 H J^T * J, 3x3 Vector3d g Vector3d::Zero(); // 梯度 g -J^T * e, 3x1 cost 0; // 遍历所有数据点累加计算 H 和 g for (int i 0; i N; i) { double xi x_data[i], yi y_data[i]; // 1. 计算当前参数下的预测值 y_hat 和误差 e_i double y_hat exp(ae * xi * xi be * xi ce); double error yi - y_hat; // 注意是 观测值 - 预测值 // 2. 计算该误差项对应的雅可比矩阵 J_i [de/da, de/db, de/dc] // 根据链式法则: de/da -d(y_hat)/da -y_hat * x_i^2 // de/db -y_hat * x_i // de/dc -y_hat * 1 Vector3d J; // 3x1 的列向量但这里每个点的J是行向量计算时转置处理 J[0] -y_hat * xi * xi; // de/da J[1] -y_hat * xi; // de/db J[2] -y_hat; // de/dc // 3. 累加 H 和 g H J * J.transpose(); // J_i^T * J_i, 外积得到一个3x3矩阵 g -J * error; // -J_i^T * e_i // 4. 累加本次迭代的总成本误差平方和 cost error * error; } // 求解线性方程 H * dx g Vector3d dx H.ldlt().solve(g); // 使用LDLT分解求解因为H是对称正定理论上 // 检查求解是否出现数值问题 if (isnan(dx[0])) { cout 迭代 iter 结果不是数值NaN。 endl; break; } // 判断是否收敛如果成本上升说明这一步走错了可能发散 if (iter 0 cost lastCost) { cout 迭代 iter 成本未下降当前成本 cost 上次成本 lastCost endl; // 在实际更鲁棒的算法中如LM这里不会直接退出而是调整步长 break; } // 更新参数 ae dx[0]; be dx[1]; ce dx[2]; lastCost cost; // 输出迭代信息 cout 迭代 iter 成本 cost 估计参数 a ae , b be , c ce 增量 dx dx.transpose() endl; // 判断收敛如果增量非常小认为已收敛 if (dx.norm() 1e-6) { break; } }注意事项与排查技巧雅可比矩阵的符号这是最容易出错的地方。我们的误差定义为e y_observed - y_predicted。那么de/da 0 - dy_predicted/da -dy_predicted/da。务必根据你自己的误差定义来推导雅可比符号错了会导致梯度方向完全相反算法必然发散。海森矩阵 H 的累加H J * J.transpose()。这里J是Vector3dJ * J.transpose()是一个3x3矩阵的外积。千万不能写成H J.transpose() * J那是一个标量。求解线性方程我们使用H.ldlt().solve(g)。因为理论上H J^T J是半正定的使用LDLT分解针对对称矩阵是高效且稳定的。如果H奇异不可逆求解会失败。在实际SLAM中由于参数化或观测不足H可能奇异这时需要正则化或处理零空间这就引出了列文伯格-马夸尔特方法。收敛判断我们设置了双重判断成本上升则终止简单策略或增量范数小于阈值则终止。更完善的实现应该监控成本的相对变化率(lastCost - cost) / lastCost。初始值敏感性尝试将初始值ae, be, ce改为2.0, -1.0, 5.0等远离真值的数你会发现算法可能发散或收敛到错误的局部极小值。这揭示了高斯牛顿法对初始值的依赖。3.3 结果可视化与分析优化结束后我们将估计参数与真实参数对比并计算误差。cout 优化结果 endl; cout 估计参数: a ae , b be , c ce endl; cout 真实参数: a ar , b br , c cr endl; cout 参数误差: da ae - ar , db be - br , dc ce - cr endl; // 可视化部分可选需要安装绘图库如matplotlib-cpp或输出到文件 // 这里可以输出拟合曲线和原始数据点直观对比优化效果。运行程序你会看到类似下面的输出迭代 0成本4.19934e06估计参数 a0.208929, b0.655229, c0.425268增量 dx0.208929 0.655229 0.425268 迭代 1成本1.21124e06估计参数 a0.383085, b1.08484, c0.720639增量 dx0.174156 0.429608 0.295371 迭代 2成本252888估计参数 a0.548937, b1.41981, c0.937695增量 dx0.165852 0.334969 0.217056 ... 迭代 9成本97.1293估计参数 a1.00933, b1.97214, c1.00343增量 dx3.14684e-05 -1.66267e-05 -1.18184e-05 迭代 10成本97.1293估计参数 a1.00933, b1.97214, c1.00343增量 dx1.21499e-10 -6.45018e-11 -4.56694e-11 优化结果 估计参数: a1.00933, b1.97214, c1.00343 真实参数: a1, b2, c1可以看到经过约10次迭代算法收敛。估计参数(1.009, 1.972, 1.003)非常接近真实参数(1, 2, 1)。微小的偏差主要来源于我们添加到数据中的高斯噪声。成本从最初的数百万下降到了约97.1这个残差正是噪声的平方和100个点噪声方差为1期望总误差约为100。4. 深入原理与最速下降法、列文伯格-马夸尔特法的对比手写实现之后我们有必要跳出代码从更广阔的视角理解高斯牛顿法的位置。4.1 高斯牛顿 vs. 最速下降法最速下降法是优化领域最直观的方法沿着当前梯度负方向即函数下降最快的方向走一步。其更新公式为x : x - λ * g其中g是梯度λ是步长。高斯牛顿法则利用了问题的特殊结构最小二乘形式。它通过J^T J来近似海森矩阵H从而得到更新方程H Δx g。这个方程的解Δx实际上是在当前线性化模型下使得二次近似目标函数最小化的最优步长和方向。对比最速下降法方向好梯度负方向但步长λ难确定需要线搜索。在长峡谷形目标函数中会走“之”字形收敛慢。高斯牛顿法通过近似海森矩阵同时确定了方向和步长。在接近最优解时它能提供二阶收敛速度非常快但在远离最优解或近似H不准确时可能步长过大导致发散。你可以尝试用最速下降法实现同一个曲线拟合问题。你需要手动选择一个步长λ或者实现一个简单的线搜索如Armijo准则。你会发现收敛速度通常远慢于高斯牛顿法。4.2 高斯牛顿的缺陷与列文伯格-马夸尔特法的改进高斯牛顿法最大的问题在于其近似海森矩阵H_gn J^T J。这要求残差e_i(x)较小或者函数接近线性。否则忽略掉的二阶项Σ_i e_i * ∇^2 e_i可能很大导致H_gn严重偏离真实海森矩阵进而使算法步长估计错误可能发散。列文伯格-马夸尔特法LM算法通过引入一个阻尼因子 μ来改进它。其增量方程变为(J^T J μ I) Δx -J^T e其中I是单位矩阵。阻尼因子 μ 的作用当μ很大时μ I占主导方程近似为μ I Δx -J^T e即Δx ≈ - (1/μ) J^T e。这接近于最速下降法步长较小但方向稳定适合在远离最优解时使用。当μ很小时方程退化为高斯牛顿方程J^T J Δx -J^T e适合在接近最优解时快速收敛。LM算法在每次迭代中动态调整μ计算增益比ρ (实际成本下降) / (模型预测的成本下降)。如果ρ很大模型预测准确则减小μ更信任高斯牛顿步。如果ρ很小甚至为负模型预测不准实际成本上升则增大μ步长更保守接近最速下降。这种自适应机制使得LM算法比纯高斯牛顿法鲁棒得多即使初始值很差也更容易收敛。它成为了非线性最小二乘问题的实际标准算法如Ceres Solver, g2o的默认优化器。实操心得在你手写的高斯牛顿法代码基础上增加LM算法的阻尼项是极好的练习。你只需要修改增量方程的构建部分H J^T J μ * I然后实现一个简单的μ更新策略例如如果成本下降μ μ / 10如果成本上升μ μ * 10并重新计算步长。你会立刻体会到算法鲁棒性的提升。5. 工程实践扩展从玩具问题到SLAM框架通过这个简单的曲线拟合我们已经掌握了非线性最小二乘优化的核心。如何将这套思维应用到真实的视觉SLAM中呢5.1 误差定义的泛化在曲线拟合中误差是标量y - y_hat。在视觉SLAM中误差通常是向量。特征点法BA误差是2维的重投影误差e u_observed - π(T * P)其中u是像素坐标π是投影函数T是相机位姿李群SE(3)P是三维点。直接法光度误差误差是标量光度差e I_ref(p) - I_cur(π(T * P))即同一个三维点在参考帧和当前帧的亮度差。无论误差形式如何优化框架不变定义误差 → 计算误差关于参数的雅可比 → 构建增量方程 → 迭代求解。5.2 雅可比矩阵的计算链式法则与李代数这是SLAM优化中的关键难点。以重投影误差为例参数包括相机位姿ξ李代数和三维点坐标P。误差e关于ξ的雅可比需要用到李代数的扰动模型∂e/∂ξ lim_{δξ→0} ( e(exp(δξ^∧) * T, P) - e(T, P) ) / δξ这可以通过链式法则和SE(3)的伴随性质推导出解析形式。同样e关于P的雅可比是∂e/∂P ∂e/∂P * ∂P/∂P其中P T * P是变换到相机坐标系下的点。在手写SLAM中你需要亲自推导这些雅可比矩阵。而在使用g2o或Ceres时你可以使用自动求导数值导数和自动微分但理解其背后的数学对于调试和深入理解问题至关重要。5.3 稀疏性与求解效率在我们的曲线拟合例子中雅可比矩阵J是 N×3 的稠密矩阵H是 3×3 的小矩阵求解非常快。但在大型BA问题中参数数量位姿数m 路标点数n可能上万H矩阵是 (mn)×(mn) 的巨矩阵。直接存储和求解H Δx g是不可能的。幸运的是SLAM问题的H矩阵具有特殊的稀疏块结构。这是因为一个路标点通常只被少数几帧观测到导致误差项只与它所在的相机位姿和自身位置有关与其他参数无关。这使得H矩阵中大量的块为零。利用这种稀疏性增量方程可以通过舒尔消元Schur Elimination被高效求解。其思想是先将方程按位姿和路标点分块[ H_pp H_pl ] [ Δξ ] [ b_p ] [ H_lp H_ll ] [ ΔP ] [ b_l ]然后通过高斯消元消去路标点增量变量ΔP得到一个只关于位姿增量Δξ的、规模小得多的方程称为缩减相机系统Reduced Camera System( H_pp - H_pl * H_ll^{-1} * H_lp ) Δξ b_p - H_pl * H_ll^{-1} * b_l求解出Δξ后再回代求解ΔP。由于H_ll是对角块矩阵每个路标点独立其求逆非常快。这是g2o、Ceres等库能高效求解大规模BA问题的核心。给进阶者的挑战尝试将你的曲线拟合问题“伪装”成一个稀疏问题。假设你有100条不同的曲线每条曲线有自己的参数 a_i, b_i, c_i但共享一部分数据点。设计一个更大的联合优化问题并尝试手动构建其稀疏的H矩阵观察其结构。这能帮你直观理解SLAM中稀疏性的来源。6. 常见调试问题与性能优化技巧在实际手写实现中你几乎一定会遇到各种问题。这里记录一些典型的坑和解决方法。6.1 算法不收敛或发散这是最常见的问题。可能的原因和排查步骤雅可比矩阵计算错误这是头号嫌疑犯。务必逐项检查导数推导。一个有效的调试方法是使用数值差分来验证解析导数。对于参数a计算数值导数(e(a h) - e(a - h)) / (2h)其中h是一个很小的数如1e-7。将其与你计算的解析导数de/da对比。如果差异显著说明你的解析导数公式错了。初始值太差高斯牛顿法只在局部有效。如果初始值离最优解太远线性近似失效导致H矩阵病态求出的Δx方向错误。解决方案尝试多个不同的初始值或者使用更鲁棒的算法如LM算法或者先使用更简单的方法如随机采样得到一个粗糙的初始估计。数据噪声过大或存在外点如果观测数据中存在大量噪声或错误数据外点最小二乘的目标函数会被严重干扰。解决方案使用鲁棒核函数如Huber核、Cauchy核。核函数的作用是降低大误差项的权重使其对整体目标函数的影响变小。在你的代码中这体现在计算误差和雅可比时每个误差项要乘以一个权重ρ(e)。double error yi - y_hat; double weight robustKernel.weight(error); // 例如Huber核 H weight * J * J.transpose(); // 加权 g -weight * J * error; cost robustKernel.loss(error); // 使用核函数后的损失而非平方误差数值稳定性问题在构建H J^T J时如果J的某些列线性相关或近似相关H会奇异或病态导致方程无法求解或解不稳定。现象dx中出现NaN或异常大的值。排查打印H矩阵的条件数H.eigenvalues()最大最小特征值之比。条件数过大说明病态。解决方案LM算法中的阻尼项μI本身就是一种正则化可以改善病态问题。也可以考虑添加更小的正则项如(J^T J λ * diag(J^T J))马夸尔特阻尼的一种变体。6.2 收敛速度慢如果算法能收敛但需要很多次迭代可能的原因问题本身性态某些参数对目标函数影响很小曲率低导致沿该方向的进展缓慢。线性化效果差即使在当前点非线性程度依然很高高斯牛顿近似不准导致每一步的改进都很小。没有使用线搜索纯高斯牛顿法有时步长会过长或过短。实现一个简单的线搜索Armijo准则可以保证每次迭代成本都下降并能适当放大步长加速收敛。基本思路是在得到dx方向后尝试步长α 1, 0.5, 0.25...直到F(x α*dx) F(x)成立。6.3 性能优化当数据点很多N很大时循环计算H和g可能成为瓶颈。优化方法向量化计算利用Eigen库的矩阵块操作和向量化指令。例如可以一次性计算所有数据点的预测值向量和误差向量。并行化如果循环迭代间无依赖可以使用OpenMP多线程并行累加H和g。注意对H和g的累加操作需要加锁或使用原子操作或者每个线程计算私有的副本最后再合并。#pragma omp parallel for reduction(:cost) for (int i 0; i N; i) { // ... 计算 J_i, error_i // 注意直接累加到共享变量H, g需要临界区效率低。 // 更好的方法是每个线程有自己的H_local, g_local循环结束后合并。 }提前计算不变量对于每个数据点x_i是常数。可以预先计算x_i,x_i^2并存储避免在循环中重复计算。手写这个高斯牛顿法求解器就像亲手搭建了一个乐高模型的基础模块。它简单但包含了所有核心概念。当你未来面对g2o中复杂的顶点、边或者Ceres中五花八门的损失函数和参数块时你会清楚地知道它们本质上都是在构建那个庞大的J矩阵并求解H Δx g这个方程。这份从零搭建的理解是调用任何现成库函数都无法替代的。我建议你在吃透这个例子后下一步可以尝试实现一个简单的视觉里程计用高斯牛顿或LM法优化相机位姿那时你会对SLAM有更立体、更深刻的认识。 从高斯牛顿法到SLAM后端优化：非线性最小二乘原理与C++实现
1. 项目概述从理论到实践的SLAM算法复现最近在重读《视觉SLAM十四讲》这本经典教材每次翻到非线性优化那一章特别是关于高斯牛顿法的部分总感觉纸上得来终觉浅。书上的公式推导很清晰但如果不亲手把代码敲一遍把每个矩阵的维度、每个雅可比的计算都捋清楚心里总是不踏实。这就像学游泳看再多教程不下水扑腾几下永远学不会。所以我决定以书中第六章的“手写高斯牛顿法求解曲线拟合问题”为蓝本进行一次彻底的代码复现与笔记整理。这个项目看似简单就是一个拟合yexp(ax^2bxc)w这样的曲线但它麻雀虽小五脏俱全几乎涵盖了视觉SLAM后端优化中最小二乘问题的所有核心要素误差定义、雅可比矩阵推导、增量方程构建与求解。通过这个练习我们能真正理解高斯牛顿法如何将复杂的非线性优化问题转化为一系列线性最小二乘问题去迭代求解这对于后续理解Bundle Adjustment、直接法SLAM等至关重要。无论你是SLAM初学者想夯实基础还是有一定经验想回头补足理论短板跟着这篇笔记一步步推导和实现都会有不小的收获。2. 核心思路与数学模型拆解2.1 问题定义一个经典的非线性最小二乘我们面对的问题非常典型给定一组带噪声的观测数据点去估计一个非线性模型的参数。具体来说我们有N个观测数据点 (x_i, y_i)它们由模型 y exp(ax^2 bx c) 生成并附加了高斯噪声 w。我们的目标是仅从这些观测数据 (x_i, y_i) 中估计出模型参数 [a, b, c]^T。这本质上是一个非线性最小二乘问题。我们需要找到一组参数使得模型预测值与实际观测值之间的误差平方和最小。用数学语言描述就是最小化以下目标函数F(a, b, c) 0.5 * Σ_i ( y_i - exp(a*x_i^2 b*x_i c) )^2这里的 0.5 系数是为了后续求导方便不影响最优解。在SLAM中这个误差项就对应着重投影误差、光度误差等。2.2 高斯牛顿法一阶近似的迭代艺术直接求解这个非线性目标函数的最小值很困难。高斯牛顿法的核心思想是进行局部线性化。假设我们有一个参数初始估计值x [a, b, c]^T我们希望在当前估计值附近找到一个增量Δx使得更新后的参数x Δx能降低目标函数值。具体做法是对误差函数进行一阶泰勒展开。定义第i个数据点的误差为e_i(x) y_i - exp(a*x_i^2 b*x_i c)。那么在当前点x处误差函数关于增量Δx的近似为e_i(x Δx) ≈ e_i(x) J_i(x) Δx其中J_i(x)是误差e_i(x)关于参数x的雅可比矩阵行向量J_i(x) [∂e_i/∂a, ∂e_i/∂b, ∂e_i/∂c]。将所有的误差项堆叠起来记e(x) [e_1(x), ..., e_N(x)]^TJ(x)为整个误差向量的雅可比矩阵N×3维。那么目标函数的增量形式近似为F(x Δx) ≈ 0.5 * || e(x) J(x) Δx ||^2这变成了一个关于Δx的线性最小二乘问题其目标函数是Δx的二次型。为了最小化它我们令其关于Δx的导数为零从而得到著名的增量方程或称正规方程Normal EquationJ(x)^T J(x) Δx - J(x)^T e(x)记H J^T J为高斯牛顿法中的近似海森矩阵Hessiang -J^T e为梯度方向。我们求解这个线性方程H Δx g得到当前迭代步的最优增量Δx然后更新参数x : x Δx。不断重复这个过程直到增量Δx足够小或达到最大迭代次数。注意这里H J^T J是海森矩阵∇^2 F(x)的近似它忽略了误差函数e_i(x)的二阶导数项。这使得计算大大简化只需计算一阶雅可比但同时也带来了一个问题这个近似只有在展开点附近误差较小即e_i(x)较小时才比较准确。如果初始值离最优解太远或者问题本身非线性很强这个近似可能很差导致算法不收敛。这是高斯牛顿法的一个主要缺陷也是后续列文伯格-马夸尔特方法要改进的地方。2.3 与SLAM后端优化的直接关联你可能已经发现了这个曲线拟合问题的数学形式和SLAM后端优化中的Bundle Adjustment (BA) 如出一辙。在BA中我们的参数x变成了所有相机位姿和三维路标点的集合。误差e_i变成了重投影误差观测到的像素坐标减去由当前位姿和地图点投影计算得到的像素坐标。雅可比矩阵J描述了重投影误差如何随相机位姿李代数表示和路标点位置变化。增量方程H Δx g的规模变得极其庞大通常有成千上万个参数但具有特殊的稀疏结构可以利用舒尔消元等技巧高效求解。因此这个手写高斯牛顿拟合曲线的练习是理解大规模视觉SLAM后端优化基石的最佳热身。搞懂了这里每一步的矩阵维度、计算和意义再看g2o、Ceres等优化库的源码就会有一种豁然开朗的感觉。3. 关键实现步骤与代码逐行解析接下来我们抛开任何优化库用纯C配合Eigen库进行矩阵运算来实现这个高斯牛顿法。我会把重点放在容易出错和需要深刻理解的地方。3.1 数据生成模拟真实观测过程任何优化问题都要有数据。我们首先根据真实的参数值生成带噪声的观测数据。这模拟了SLAM中传感器获取带噪声观测值的过程。#include iostream #include cmath #include vector #include Eigen/Core #include Eigen/Dense using namespace std; using namespace Eigen; int main() { // 真实参数值 double ar 1.0, br 2.0, cr 1.0; // 估计参数初始值 (故意给一个偏离真实值的初始值比如全零) double ae 0.0, be 0.0, ce 0.0; int N 100; // 数据点数量 double w_sigma 1.0; // 噪声的标准差 double inv_sigma 1.0 / w_sigma; // 信息矩阵这里简单设为1/sigma cv::RNG rng; // OpenCV随机数生成器也可以用C11的random vectordouble x_data, y_data; // 观测数据容器 for (int i 0; i N; i) { double x i / 100.0; // x在[0, 1]区间均匀分布 x_data.push_back(x); // 生成带噪声的y观测值: y exp(a*x^2 b*x c) noise double y exp(ar * x * x br * x cr) rng.gaussian(w_sigma); y_data.push_back(y); }实操心得噪声水平w_sigma的设置很重要。如果噪声太大可能掩盖信号导致优化困难甚至失败如果噪声太小问题过于简单体现不出优化算法的鲁棒性。w_sigma1.0对于y值范围在几十的量级来说是一个合理的设置能产生肉眼可见的偏差。3.2 迭代优化核心循环高斯牛顿法是一个迭代过程。我们设定一个最大迭代次数和收敛阈值。int iterations 100; // 最大迭代次数 double cost 0, lastCost 0; // 本次迭代成本和上次迭代成本用于判断收敛 for (int iter 0; iter iterations; iter) { Matrix3d H Matrix3d::Zero(); // 近似海森矩阵 H J^T * J, 3x3 Vector3d g Vector3d::Zero(); // 梯度 g -J^T * e, 3x1 cost 0; // 遍历所有数据点累加计算 H 和 g for (int i 0; i N; i) { double xi x_data[i], yi y_data[i]; // 1. 计算当前参数下的预测值 y_hat 和误差 e_i double y_hat exp(ae * xi * xi be * xi ce); double error yi - y_hat; // 注意是 观测值 - 预测值 // 2. 计算该误差项对应的雅可比矩阵 J_i [de/da, de/db, de/dc] // 根据链式法则: de/da -d(y_hat)/da -y_hat * x_i^2 // de/db -y_hat * x_i // de/dc -y_hat * 1 Vector3d J; // 3x1 的列向量但这里每个点的J是行向量计算时转置处理 J[0] -y_hat * xi * xi; // de/da J[1] -y_hat * xi; // de/db J[2] -y_hat; // de/dc // 3. 累加 H 和 g H J * J.transpose(); // J_i^T * J_i, 外积得到一个3x3矩阵 g -J * error; // -J_i^T * e_i // 4. 累加本次迭代的总成本误差平方和 cost error * error; } // 求解线性方程 H * dx g Vector3d dx H.ldlt().solve(g); // 使用LDLT分解求解因为H是对称正定理论上 // 检查求解是否出现数值问题 if (isnan(dx[0])) { cout 迭代 iter 结果不是数值NaN。 endl; break; } // 判断是否收敛如果成本上升说明这一步走错了可能发散 if (iter 0 cost lastCost) { cout 迭代 iter 成本未下降当前成本 cost 上次成本 lastCost endl; // 在实际更鲁棒的算法中如LM这里不会直接退出而是调整步长 break; } // 更新参数 ae dx[0]; be dx[1]; ce dx[2]; lastCost cost; // 输出迭代信息 cout 迭代 iter 成本 cost 估计参数 a ae , b be , c ce 增量 dx dx.transpose() endl; // 判断收敛如果增量非常小认为已收敛 if (dx.norm() 1e-6) { break; } }注意事项与排查技巧雅可比矩阵的符号这是最容易出错的地方。我们的误差定义为e y_observed - y_predicted。那么de/da 0 - dy_predicted/da -dy_predicted/da。务必根据你自己的误差定义来推导雅可比符号错了会导致梯度方向完全相反算法必然发散。海森矩阵 H 的累加H J * J.transpose()。这里J是Vector3dJ * J.transpose()是一个3x3矩阵的外积。千万不能写成H J.transpose() * J那是一个标量。求解线性方程我们使用H.ldlt().solve(g)。因为理论上H J^T J是半正定的使用LDLT分解针对对称矩阵是高效且稳定的。如果H奇异不可逆求解会失败。在实际SLAM中由于参数化或观测不足H可能奇异这时需要正则化或处理零空间这就引出了列文伯格-马夸尔特方法。收敛判断我们设置了双重判断成本上升则终止简单策略或增量范数小于阈值则终止。更完善的实现应该监控成本的相对变化率(lastCost - cost) / lastCost。初始值敏感性尝试将初始值ae, be, ce改为2.0, -1.0, 5.0等远离真值的数你会发现算法可能发散或收敛到错误的局部极小值。这揭示了高斯牛顿法对初始值的依赖。3.3 结果可视化与分析优化结束后我们将估计参数与真实参数对比并计算误差。cout 优化结果 endl; cout 估计参数: a ae , b be , c ce endl; cout 真实参数: a ar , b br , c cr endl; cout 参数误差: da ae - ar , db be - br , dc ce - cr endl; // 可视化部分可选需要安装绘图库如matplotlib-cpp或输出到文件 // 这里可以输出拟合曲线和原始数据点直观对比优化效果。运行程序你会看到类似下面的输出迭代 0成本4.19934e06估计参数 a0.208929, b0.655229, c0.425268增量 dx0.208929 0.655229 0.425268 迭代 1成本1.21124e06估计参数 a0.383085, b1.08484, c0.720639增量 dx0.174156 0.429608 0.295371 迭代 2成本252888估计参数 a0.548937, b1.41981, c0.937695增量 dx0.165852 0.334969 0.217056 ... 迭代 9成本97.1293估计参数 a1.00933, b1.97214, c1.00343增量 dx3.14684e-05 -1.66267e-05 -1.18184e-05 迭代 10成本97.1293估计参数 a1.00933, b1.97214, c1.00343增量 dx1.21499e-10 -6.45018e-11 -4.56694e-11 优化结果 估计参数: a1.00933, b1.97214, c1.00343 真实参数: a1, b2, c1可以看到经过约10次迭代算法收敛。估计参数(1.009, 1.972, 1.003)非常接近真实参数(1, 2, 1)。微小的偏差主要来源于我们添加到数据中的高斯噪声。成本从最初的数百万下降到了约97.1这个残差正是噪声的平方和100个点噪声方差为1期望总误差约为100。4. 深入原理与最速下降法、列文伯格-马夸尔特法的对比手写实现之后我们有必要跳出代码从更广阔的视角理解高斯牛顿法的位置。4.1 高斯牛顿 vs. 最速下降法最速下降法是优化领域最直观的方法沿着当前梯度负方向即函数下降最快的方向走一步。其更新公式为x : x - λ * g其中g是梯度λ是步长。高斯牛顿法则利用了问题的特殊结构最小二乘形式。它通过J^T J来近似海森矩阵H从而得到更新方程H Δx g。这个方程的解Δx实际上是在当前线性化模型下使得二次近似目标函数最小化的最优步长和方向。对比最速下降法方向好梯度负方向但步长λ难确定需要线搜索。在长峡谷形目标函数中会走“之”字形收敛慢。高斯牛顿法通过近似海森矩阵同时确定了方向和步长。在接近最优解时它能提供二阶收敛速度非常快但在远离最优解或近似H不准确时可能步长过大导致发散。你可以尝试用最速下降法实现同一个曲线拟合问题。你需要手动选择一个步长λ或者实现一个简单的线搜索如Armijo准则。你会发现收敛速度通常远慢于高斯牛顿法。4.2 高斯牛顿的缺陷与列文伯格-马夸尔特法的改进高斯牛顿法最大的问题在于其近似海森矩阵H_gn J^T J。这要求残差e_i(x)较小或者函数接近线性。否则忽略掉的二阶项Σ_i e_i * ∇^2 e_i可能很大导致H_gn严重偏离真实海森矩阵进而使算法步长估计错误可能发散。列文伯格-马夸尔特法LM算法通过引入一个阻尼因子 μ来改进它。其增量方程变为(J^T J μ I) Δx -J^T e其中I是单位矩阵。阻尼因子 μ 的作用当μ很大时μ I占主导方程近似为μ I Δx -J^T e即Δx ≈ - (1/μ) J^T e。这接近于最速下降法步长较小但方向稳定适合在远离最优解时使用。当μ很小时方程退化为高斯牛顿方程J^T J Δx -J^T e适合在接近最优解时快速收敛。LM算法在每次迭代中动态调整μ计算增益比ρ (实际成本下降) / (模型预测的成本下降)。如果ρ很大模型预测准确则减小μ更信任高斯牛顿步。如果ρ很小甚至为负模型预测不准实际成本上升则增大μ步长更保守接近最速下降。这种自适应机制使得LM算法比纯高斯牛顿法鲁棒得多即使初始值很差也更容易收敛。它成为了非线性最小二乘问题的实际标准算法如Ceres Solver, g2o的默认优化器。实操心得在你手写的高斯牛顿法代码基础上增加LM算法的阻尼项是极好的练习。你只需要修改增量方程的构建部分H J^T J μ * I然后实现一个简单的μ更新策略例如如果成本下降μ μ / 10如果成本上升μ μ * 10并重新计算步长。你会立刻体会到算法鲁棒性的提升。5. 工程实践扩展从玩具问题到SLAM框架通过这个简单的曲线拟合我们已经掌握了非线性最小二乘优化的核心。如何将这套思维应用到真实的视觉SLAM中呢5.1 误差定义的泛化在曲线拟合中误差是标量y - y_hat。在视觉SLAM中误差通常是向量。特征点法BA误差是2维的重投影误差e u_observed - π(T * P)其中u是像素坐标π是投影函数T是相机位姿李群SE(3)P是三维点。直接法光度误差误差是标量光度差e I_ref(p) - I_cur(π(T * P))即同一个三维点在参考帧和当前帧的亮度差。无论误差形式如何优化框架不变定义误差 → 计算误差关于参数的雅可比 → 构建增量方程 → 迭代求解。5.2 雅可比矩阵的计算链式法则与李代数这是SLAM优化中的关键难点。以重投影误差为例参数包括相机位姿ξ李代数和三维点坐标P。误差e关于ξ的雅可比需要用到李代数的扰动模型∂e/∂ξ lim_{δξ→0} ( e(exp(δξ^∧) * T, P) - e(T, P) ) / δξ这可以通过链式法则和SE(3)的伴随性质推导出解析形式。同样e关于P的雅可比是∂e/∂P ∂e/∂P * ∂P/∂P其中P T * P是变换到相机坐标系下的点。在手写SLAM中你需要亲自推导这些雅可比矩阵。而在使用g2o或Ceres时你可以使用自动求导数值导数和自动微分但理解其背后的数学对于调试和深入理解问题至关重要。5.3 稀疏性与求解效率在我们的曲线拟合例子中雅可比矩阵J是 N×3 的稠密矩阵H是 3×3 的小矩阵求解非常快。但在大型BA问题中参数数量位姿数m 路标点数n可能上万H矩阵是 (mn)×(mn) 的巨矩阵。直接存储和求解H Δx g是不可能的。幸运的是SLAM问题的H矩阵具有特殊的稀疏块结构。这是因为一个路标点通常只被少数几帧观测到导致误差项只与它所在的相机位姿和自身位置有关与其他参数无关。这使得H矩阵中大量的块为零。利用这种稀疏性增量方程可以通过舒尔消元Schur Elimination被高效求解。其思想是先将方程按位姿和路标点分块[ H_pp H_pl ] [ Δξ ] [ b_p ] [ H_lp H_ll ] [ ΔP ] [ b_l ]然后通过高斯消元消去路标点增量变量ΔP得到一个只关于位姿增量Δξ的、规模小得多的方程称为缩减相机系统Reduced Camera System( H_pp - H_pl * H_ll^{-1} * H_lp ) Δξ b_p - H_pl * H_ll^{-1} * b_l求解出Δξ后再回代求解ΔP。由于H_ll是对角块矩阵每个路标点独立其求逆非常快。这是g2o、Ceres等库能高效求解大规模BA问题的核心。给进阶者的挑战尝试将你的曲线拟合问题“伪装”成一个稀疏问题。假设你有100条不同的曲线每条曲线有自己的参数 a_i, b_i, c_i但共享一部分数据点。设计一个更大的联合优化问题并尝试手动构建其稀疏的H矩阵观察其结构。这能帮你直观理解SLAM中稀疏性的来源。6. 常见调试问题与性能优化技巧在实际手写实现中你几乎一定会遇到各种问题。这里记录一些典型的坑和解决方法。6.1 算法不收敛或发散这是最常见的问题。可能的原因和排查步骤雅可比矩阵计算错误这是头号嫌疑犯。务必逐项检查导数推导。一个有效的调试方法是使用数值差分来验证解析导数。对于参数a计算数值导数(e(a h) - e(a - h)) / (2h)其中h是一个很小的数如1e-7。将其与你计算的解析导数de/da对比。如果差异显著说明你的解析导数公式错了。初始值太差高斯牛顿法只在局部有效。如果初始值离最优解太远线性近似失效导致H矩阵病态求出的Δx方向错误。解决方案尝试多个不同的初始值或者使用更鲁棒的算法如LM算法或者先使用更简单的方法如随机采样得到一个粗糙的初始估计。数据噪声过大或存在外点如果观测数据中存在大量噪声或错误数据外点最小二乘的目标函数会被严重干扰。解决方案使用鲁棒核函数如Huber核、Cauchy核。核函数的作用是降低大误差项的权重使其对整体目标函数的影响变小。在你的代码中这体现在计算误差和雅可比时每个误差项要乘以一个权重ρ(e)。double error yi - y_hat; double weight robustKernel.weight(error); // 例如Huber核 H weight * J * J.transpose(); // 加权 g -weight * J * error; cost robustKernel.loss(error); // 使用核函数后的损失而非平方误差数值稳定性问题在构建H J^T J时如果J的某些列线性相关或近似相关H会奇异或病态导致方程无法求解或解不稳定。现象dx中出现NaN或异常大的值。排查打印H矩阵的条件数H.eigenvalues()最大最小特征值之比。条件数过大说明病态。解决方案LM算法中的阻尼项μI本身就是一种正则化可以改善病态问题。也可以考虑添加更小的正则项如(J^T J λ * diag(J^T J))马夸尔特阻尼的一种变体。6.2 收敛速度慢如果算法能收敛但需要很多次迭代可能的原因问题本身性态某些参数对目标函数影响很小曲率低导致沿该方向的进展缓慢。线性化效果差即使在当前点非线性程度依然很高高斯牛顿近似不准导致每一步的改进都很小。没有使用线搜索纯高斯牛顿法有时步长会过长或过短。实现一个简单的线搜索Armijo准则可以保证每次迭代成本都下降并能适当放大步长加速收敛。基本思路是在得到dx方向后尝试步长α 1, 0.5, 0.25...直到F(x α*dx) F(x)成立。6.3 性能优化当数据点很多N很大时循环计算H和g可能成为瓶颈。优化方法向量化计算利用Eigen库的矩阵块操作和向量化指令。例如可以一次性计算所有数据点的预测值向量和误差向量。并行化如果循环迭代间无依赖可以使用OpenMP多线程并行累加H和g。注意对H和g的累加操作需要加锁或使用原子操作或者每个线程计算私有的副本最后再合并。#pragma omp parallel for reduction(:cost) for (int i 0; i N; i) { // ... 计算 J_i, error_i // 注意直接累加到共享变量H, g需要临界区效率低。 // 更好的方法是每个线程有自己的H_local, g_local循环结束后合并。 }提前计算不变量对于每个数据点x_i是常数。可以预先计算x_i,x_i^2并存储避免在循环中重复计算。手写这个高斯牛顿法求解器就像亲手搭建了一个乐高模型的基础模块。它简单但包含了所有核心概念。当你未来面对g2o中复杂的顶点、边或者Ceres中五花八门的损失函数和参数块时你会清楚地知道它们本质上都是在构建那个庞大的J矩阵并求解H Δx g这个方程。这份从零搭建的理解是调用任何现成库函数都无法替代的。我建议你在吃透这个例子后下一步可以尝试实现一个简单的视觉里程计用高斯牛顿或LM法优化相机位姿那时你会对SLAM有更立体、更深刻的认识。