从零实现高斯牛顿法：视觉SLAM非线性优化的核心原理与C++实践

1. 从理论到代码为什么高斯牛顿法是视觉SLAM的基石如果你正在啃《视觉SLAM十四讲》并且卡在了第六章前后感觉非线性优化一堆公式看得头大那这篇文章就是为你写的。我当年学到这里最大的困惑就是书上推导的“高斯牛顿法”和“列文伯格-马夸尔特法”看起来很美但一关书脑子里只剩下一堆雅可比和海塞矩阵的符号。直到我逼着自己从零开始用C把高斯牛顿法手敲了一遍去解一个最简单的曲线拟合问题那些抽象的矩阵才真正“活”了过来。今天我就把我手写高斯牛顿法的完整笔记、踩过的坑以及最重要的——如何将书上的数学公式一步步变成可以运行、可以调试的代码——毫无保留地分享给你。这不是对书本内容的简单复述而是一个实践者视角的“翻译”和“注解”目标是让你不仅能看懂更能亲手实现它真正理解视觉SLAM中优化器到底在干什么。视觉SLAM的核心问题之一就是根据传感器观测数据估计机器人的位姿和地图点的三维位置。这些观测方程比如相机投影模型几乎都是非线性的。我们无法直接求解只能寻求一个“最优”解使得预测值与观测值之间的误差最小。这就转化成了一个非线性最小二乘问题。而高斯牛顿法正是解决这类问题最经典、最常用的迭代优化方法之一它是更复杂的列文伯格-马夸尔特法的基础。可以说吃透了高斯牛顿你就拿到了打开视觉SLAM后端优化大门的钥匙。2. 问题定义从一个你能立刻跑起来的例子开始在直接冲击视觉SLAM的BABundle Adjustment之前我们先找一个更直观、更轻量的问题来练手曲线拟合。这是理解优化算法的“Hello World”。假设我们想用一条曲线 $y \exp(ax^2 bx c)$ 来拟合一些带噪声的数据点。为什么选这个模型因为它包含了指数和非线性项足够模拟SLAM中复杂的观测模型但又比相机投影矩阵简单便于我们聚焦于优化算法本身。我们的任务是已知N个数据点 $(x_i, y_i)$找到最优的参数 $[a, b, c]^T$使得曲线模型预测的 $\hat{y}_i \exp(ax_i^2 bx_i c)$ 与真实的 $y_i$ 尽可能接近。用最小二乘的语言描述就是最小化所有数据点的误差平方和 $$ \min_{a, b, c} \frac{1}{2} \sum_{i1}^{N} ||y_i - \exp(ax_i^2 bx_i c)||^2 $$ 这里乘以 $\frac{1}{2}$ 是为了后续求导时形式更整洁不影响最优解。我们先来生成一组模拟数据并添加高斯噪声。这是我们的“地面真值”和“观测数据”。// 生成带噪声的观测数据 int main() { // 真实参数 double ar 1.0, br 2.0, cr 1.0; // 估计参数初始值 double ae 2.0, be -1.0, ce 5.0; int N 100; // 数据点数量 double w_sigma 1.0; // 噪声sigma值 double inv_sigma 1.0 / w_sigma; cv::RNG rng; // OpenCV随机数生成器 vectordouble x_data, y_data; for (int i 0; i N; i) { double x i / 100.0; x_data.push_back(x); // 根据真实模型生成y并加上高斯噪声 y_data.push_back(exp(ar * x * x br * x cr) rng.gaussian(w_sigma * w_sigma)); } // 接下来将使用高斯牛顿法利用x_data, y_data来优化ae, be, ce }注意这里使用OpenCV的RNG生成噪声是为了方便你也可以用C11的random库。关键是要理解我们是在模拟一个从“真实世界”采样并带有噪声的观测过程这和SLAM中从图像提取带噪声的像素坐标是完全类似的。3. 高斯牛顿法拆解每一步的数学与代码对应现在进入正题。高斯牛顿法的核心思想是在当前估计值附近对非线性函数进行一阶泰勒展开将其近似为线性函数然后求解这个线性最小二乘问题得到参数的增量迭代更新。对于我们的问题定义单个数据点的误差为 $$ e_i y_i - \exp(ax_i^2 bx_i c) $$ 那么总体目标函数为 $$ F(\mathbf{x}) \frac{1}{2} \sum_{i1}^{N} e_i(a, b, c)^2 $$ 其中 $\mathbf{x} [a, b, c]^T$ 是待优化的参数向量。第一步误差函数的泰勒展开在当前参数估计值 $\mathbf{x}_k$ 处将误差函数 $e_i(\mathbf{x}_k \Delta \mathbf{x})$ 进行一阶泰勒展开 $$ e_i(\mathbf{x}_k \Delta \mathbf{x}) \approx e_i(\mathbf{x}_k) \mathbf{J}_i(\mathbf{x}_k) \Delta \mathbf{x} $$ 这里 $\mathbf{J}_i$ 是误差 $e_i$ 关于参数 $\mathbf{x}$ 的雅可比矩阵在这里是一个行向量因为 $e_i$ 是标量 $$ \mathbf{J}_i \left[ \frac{\partial e_i}{\partial a}, \frac{\partial e_i}{\partial b}, \frac{\partial e_i}{\partial c} \right] $$ 对于我们的模型 $e_i y_i - \exp(ax_i^2 bx_i c)$可以具体求导 $$ \begin{aligned} \frac{\partial e_i}{\partial a} -x_i^2 \cdot \exp(ax_i^2 bx_i c) \ \frac{\partial e_i}{\partial b} -x_i \cdot \exp(ax_i^2 bx_i c) \ \frac{\partial e_i}{\partial c} -\exp(ax_i^2 bx_i c) \end{aligned} $$ 注意这里有个负号因为误差是“观测值 - 预测值”。在代码中我们通常直接计算雅可比矩阵 $\mathbf{J}_i$。第二步构造线性最小二乘问题将泰勒展开式代入总目标函数 $$ \begin{aligned} F(\mathbf{x}k \Delta \mathbf{x}) \approx \frac{1}{2} \sum{i1}^{N} (e_i(\mathbf{x}_k) \mathbf{J}_i \Delta \mathbf{x})^2 \ \frac{1}{2} \sum (e_i^2 2 e_i \mathbf{J}_i \Delta \mathbf{x} \Delta \mathbf{x}^T \mathbf{J}_i^T \mathbf{J}_i \Delta \mathbf{x}) \end{aligned} $$ 这可以写成矩阵形式。令 $\mathbf{e} [e_1, e_2, ..., e_N]^T$ 为所有误差项组成的列向量$\mathbf{J}$ 为整个系统的雅可比矩阵其第 $i$ 行为 $\mathbf{J}_i$。则上式近似为 $$ F(\mathbf{x}_k \Delta \mathbf{x}) \approx \frac{1}{2} ||\mathbf{e} \mathbf{J} \Delta \mathbf{x}||^2 $$ 我们的目标是找到增量 $\Delta \mathbf{x}$ 使得这个近似函数最小。这是一个关于 $\Delta \mathbf{x}$ 的二次函数其最小值点可以通过令其导数为零求得 $$ \mathbf{J}^T \mathbf{J} \Delta \mathbf{x} -\mathbf{J}^T \mathbf{e} $$ 这个方程被称为高斯牛顿方程或正规方程。其中 $\mathbf{J}^T \mathbf{J}$ 是对海塞矩阵 $\mathbf{H}$ 的近似高斯牛顿法的核心近似$\mathbf{J}^T \mathbf{e}$ 是梯度 $\mathbf{g}$ 的负值。第三步迭代求解解这个线性方程 $\mathbf{H} \Delta \mathbf{x} \mathbf{g}$得到参数增量 $\Delta \mathbf{x}$然后更新参数 $$ \mathbf{x}_{k1} \mathbf{x}_k \Delta \mathbf{x} $$ 重复这个过程直到增量 $\Delta \mathbf{x}$ 的范数小于某个阈值或者目标函数 $F(\mathbf{x})$ 的变化不再显著。4. 手把手实现C代码逐行解析理解了数学我们来看代码实现。我将关键步骤拆解并加上详细注释。// 高斯牛顿迭代主循环 int iterations 10; // 最大迭代次数 double cost 0, lastCost 0; // 本次迭代成本和上次迭代成本 for (int iter 0; iter iterations; iter) { // 初始化本次迭代的线性方程系数矩阵 H 和向量 g Eigen::Matrix3d H Eigen::Matrix3d::Zero(); // H J^T * J 3x3矩阵因为3个参数 Eigen::Vector3d g Eigen::Vector3d::Zero(); // g -J^T * e 3x1向量 cost 0; // 遍历所有数据点累加每个点的贡献 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; // e_i y_i - y_hat // 2. 计算该误差项关于参数的雅可比矩阵 J_i (1x3的行向量) // 根据求导公式J_i - [x_i^2 * y_hat, x_i * y_hat, y_hat] // 注意我们这里计算的是误差e_i对参数的导数所以有负号。 // 但在构建 H 和 g 时公式中用的是 J_i我们直接计算这个向量即可。 Eigen::Vector3d J; // 雅可比向量 J[0] -xi * xi * y_hat; // de/da J[1] -xi * y_hat; // de/db J[2] -y_hat; // de/dc // 3. 累加到 H 和 g // H J_i^T * J_i (一个3x3的矩阵由列向量乘以行向量得到) H J * J.transpose(); // g -J_i^T * e_i g -J * error; // 4. 累加本次误差的平方用于计算总成本监控收敛 cost error * error; } // 求解线性方程 H * dx g Eigen::Vector3d dx H.ldlt().solve(g); // 使用LDLT分解求解对于小规模正定矩阵高效稳定 // 检查求解是否数值稳定 if (isnan(dx[0])) { cout 迭代 iter 结果不是数值。 endl; break; } // 判断是否收敛如果成本上升说明这一步走错了可能发散 if (iter 0 cost lastCost) { cout 迭代 iter 成本未下降当前成本 cost 上次成本 lastCost endl; break; } // 更新参数 ae dx[0]; be dx[1]; ce dx[2]; lastCost cost; // 输出迭代信息 cout 迭代 iter 总成本 cost 增量 dx dx.transpose() 估计参数 ae , be , ce endl; // 判断收敛如果增量非常小可以提前结束 if (dx.norm() 1e-6) { break; } } cout 最终估计参数: a ae , b be , c ce endl;关键点解析与踩坑记录雅可比矩阵的计算这是最容易出错的地方。一定要亲自动手推导一遍误差函数对每个参数的偏导数。注意我们代码中的J向量对应的是 $\frac{\partial e_i}{\partial \mathbf{x}}$。因为 $e_i y_i - \hat{y}_i$而 $\hat{y}_i$ 是关于参数的函数所以链式法则会带来一个负号。很多初学者在这里符号弄反导致优化朝错误方向进行。H和g的累加注意我们在循环中执行的是H J * J.transpose()和g -J * error。这完全对应了数学公式 $\mathbf{H} \sum \mathbf{J}_i^T \mathbf{J}_i$ 和 $\mathbf{g} -\sum \mathbf{J}_i^T e_i$。这里的J是行向量代码中用Eigen::Vector3d表示列向量但J.transpose()就是行向量所以J * J.transpose()得到一个3x3矩阵J * error得到一个3x1向量。线性方程求解我们使用了H.ldlt().solve(g)。这里有几个选择H.ldlt().solve(g)适用于正定或半正定矩阵HJ^T J通常是半正定的速度快且数值稳定是首选。H.colPivHouseholderQr().solve(g)更通用的QR分解适用于任何矩阵但速度稍慢。H.inverse() * g绝对不要这样做直接求逆在数值计算上既不稳定效率又低尤其是矩阵接近奇异时。收敛性判断我们设置了双重判断。一是成本函数cost是否下降这是最根本的。如果成本上升说明高斯牛顿的局部线性近似在当前步长下已经失效继续迭代会发散。二是增量dx的范数是否足够小这表示参数已经稳定接近局部最优解。5. 运行结果分析与可视化看到优化如何发生运行上面的代码你可能会看到类似如下的输出迭代 0总成本3.19575e06增量 dx 0.0459382 -0.0787507 -0.905529估计参数2.04594, -1.07875, 4.09447 迭代 1总成本376785增量 dx 0.065762 0.224986 -0.904598估计参数2.1117, -0.853767, 3.18987 迭代 2总成本35673.6增量 dx 0.0670241 0.234332 -0.590576估计参数2.17873, -0.619435, 2.59929 迭代 3总成本2195.44增量 dx 0.0500677 0.185299 -0.268508估计参数2.2288, -0.434136, 2.33078 迭代 4总成本174.342增量 dx 0.00998563 0.0380613 -0.0539026估计参数2.23878, -0.396075, 2.27688 迭代 5总成本102.78增量 dx 0.000644028 0.00245331 -0.00346863估计参数2.23943, -0.393622, 2.27341 迭代 6总成本101.937增量 dx 2.15344e-06 8.21097e-06 -1.16055e-05估计参数2.23943, -0.393614, 2.2734 迭代 7总成本101.937增量 dx 2.12328e-10 8.09869e-10 -1.14444e-09估计参数2.23943, -0.393614, 2.2734 最终估计参数: a2.23943, b-0.393614, c2.2734解读输出成本急剧下降从第一次迭代的3.19e06到第二次的3.76e05可以看到高斯牛顿法强大的收敛能力。初始估计(2, -1, 5)离真实值(1, 2, 1)很远但算法依然能将其拉回。参数变化参数a从2.0向1.0靠近最终2.24b从-1.0向2.0靠近最终-0.39c从5.0向1.0靠近最终2.27。虽然没完全收敛到真实值因为数据有噪声且模型存在非线性最优解本身就会偏移但趋势是正确的。收敛过程增量dx的范数在快速减小最后几次迭代已经达到1e-9量级说明算法已收敛到一个稳定点。为了更直观我们可以用OpenCV简单绘制拟合过程。// 可视化部分代码需在迭代循环中或之后加入 cv::Mat plot(500, 500, CV_8UC3, cv::Scalar(255, 255, 255)); // 绘制数据点 for (int i 0; i N; i) { int px int(x_data[i] * 50); // 缩放以便显示 int py int(y_data[i] * 10); cv::circle(plot, cv::Point(px, 500 - py), 3, cv::Scalar(0, 0, 255), -1); // 红色点 } // 绘制初始估计曲线 vectorcv::Point initial_curve; for (double x 0; x 1.0; x 0.01) { double y exp(2.0 * x * x - 1.0 * x 5.0); // 初始参数 int px int(x * 50); int py int(y * 10); initial_curve.push_back(cv::Point(px, 500 - py)); } cv::polylines(plot, initial_curve, false, cv::Scalar(255, 0, 0), 2); // 蓝色初始曲线 // 绘制最终拟合曲线 vectorcv::Point fitted_curve; for (double x 0; x 1.0; x 0.01) { double y exp(ae * x * x be * x ce); // 优化后参数 int px int(x * 50); int py int(y * 10); fitted_curve.push_back(cv::Point(px, 500 - py)); } cv::polylines(plot, fitted_curve, false, cv::Scalar(0, 255, 0), 2); // 绿色拟合曲线 cv::imshow(Gauss-Newton Curve Fitting, plot); cv::waitKey(0);通过图像你可以清晰地看到绿色的拟合曲线如何从蓝色的初始曲线偏离甚远一步步调整最终穿过红色数据点的中心区域。这个视觉反馈能极大地加深你对迭代优化过程的理解。6. 从曲线拟合到视觉SLAM思想迁移与核心挑战通过这个简单的例子我们亲手实现了高斯牛顿法。现在我们要把这种理解迁移到视觉SLAM更复杂的场景中。在视觉SLAM的BA问题中参数不再是简单的[a, b, c]而是所有相机位姿李代数表示 $\boldsymbol{\xi}_1, ..., \boldsymbol{\xi}_m$和所有地图点三维坐标$\mathbf{p}_1, ..., \mathbf{p}_n$拼接成的一个巨大的参数向量 $\mathbf{x}$。误差不再是标量y_i - y_hat而是重投影误差Reprojection Error。对于第i个相机位姿观测到第j个地图点误差是一个2维向量 $$ \mathbf{e}{ij} \mathbf{z}{ij} - \pi(\mathbf{T}_i, \mathbf{p}j) $$ 其中 $\mathbf{z}{ij}$ 是图像上观测到的2D像素坐标$\pi$ 是相机投影函数$\mathbf{T}_i$ 是第i个相机位姿变换矩阵$\mathbf{p}_j$ 是第j个地图点的3D坐标。雅可比矩阵计算变得复杂得多。我们需要求误差 $\mathbf{e}_{ij}$ 关于相机位姿李代数 $\boldsymbol{\xi}_i$ 的雅可比以及关于地图点坐标 $\mathbf{p}_j$ 的雅可比。这涉及到李代数的扰动模型、相机模型的链式求导。在《十四讲》的公式(7.45)和(7.47)给出了详细推导。增量方程形式依然是 $\mathbf{H} \Delta \mathbf{x} \mathbf{g}$但现在的 $\mathbf{H}$ 矩阵是稀疏的因为一个特定的误差项 $\mathbf{e}_{ij}$ 只与第i个位姿和第j个地图点有关所以它的雅可比矩阵只在对应的参数块处非零。这导致大 $\mathbf{H}$ 矩阵具有特殊的稀疏块结构可以利用舒尔消元Schur Elimination进行高效求解这就是所谓的稀疏性利用是SLAM后端优化能实时运行的关键。手写BA的挑战虽然原理相通但手写一个完整的BA优化器挑战巨大数据结构需要设计良好的类来管理相机参数、位姿、地图点以及它们之间的观测关系。雅可比计算推导和编码重投影误差的雅可比矩阵容易出错需要仔细验证。稀疏矩阵构建如何高效地组装庞大的、稀疏的 $\mathbf{H}$ 矩阵通常使用稀疏矩阵库如Eigen的SparseMatrix或专门的状态向量排序。大规模求解器直接对巨大的 $\mathbf{H}$ 矩阵进行LDLT分解可能内存和计算都无法承受。需要使用迭代法如共轭梯度法CG或利用稀疏Cholesky分解。因此在实际的视觉SLAM系统中我们通常使用成熟的优化库如g2o、Ceres Solver或GTSAM。这些库帮我们封装了稀疏矩阵的构建、高效的求解算法以及自动求导Ceres等功能。我们学习的价值在于当你使用g2o或Ceres定义一个BA问题添加参数块和残差块时你能清楚地知道底层在发生什么当优化结果不好时你知道可能是雅可比计算有误、初值太差还是问题本身不可观。7. 调试与验证如何确保你的高斯牛顿实现是正确的自己实现算法最怕的就是代码有隐藏错误。这里分享几个验证技巧与解析解对比对于简单问题对于线性回归等有解析解的问题可以用高斯牛顿法求解结果应与解析解一致。梯度检查Gradient Checking这是最可靠的验证方法。对于你的目标函数 $F(\mathbf{x})$在某个点 $\mathbf{x}_0$用你代码计算的梯度即 $-\mathbf{J}^T\mathbf{e}$应该与数值梯度近似相等。数值梯度的计算方法是 $$ \frac{\partial F}{\partial x_k} \approx \frac{F(\mathbf{x}_0 \epsilon \mathbf{e}_k) - F(\mathbf{x}_0 - \epsilon \mathbf{e}_k)}{2\epsilon} $$ 其中 $\mathbf{e}_k$ 是第k个单位向量$\epsilon$ 是一个很小的数如1e-6。如果两者差异很大说明你的雅可比计算有误。蒙特卡洛测试在真实参数附近随机生成大量不同的初始值运行你的优化器。如果大部分情况下都能收敛到同一个最优值附近说明你的算法是鲁棒的。可视化中间状态就像我们上面做的曲线绘制在SLAM中也可以可视化每次迭代后的相机轨迹和地图点观察它们是如何被“拉”到正确位置的。与成熟库的结果对比用Ceres Solver定义同一个曲线拟合问题对比最终优化结果和迭代次数。如果一致说明你的实现基本正确。一个常见的坑雅可比矩阵的维度在SLAM中误差维度如2维重投影误差和参数维度如6维李代数3维点坐标不同雅可比矩阵是2x(63)的矩阵但通常分为对位姿的2x6和对点的2x3两块。在累加HJ^T J时必须确保J的列数等于参数总维度并且累加时要放到H矩阵对应的位置。这是实现中最容易索引错乱的地方。手写高斯牛顿法就像学游泳时先在浅水区练习划水。它剥离了SLAM中复杂的几何和数据结构让你专注于优化算法最核心的迭代流程计算误差、求雅可比、构建线性方程、求解更新。当你透彻理解了这个流程再回头看g2o、Ceres那些复杂的模板和继承关系就不会再感到畏惧因为你明白它们最终都是在做你刚刚手写过的事情——求解那个庞大的 $\mathbf{H} \Delta \mathbf{x} \mathbf{g}$ 方程。这份从零构建的理解是任何现成库都无法替代的。