04 牛顿法、高斯牛顿法及 Cpp 实现

4.1 非线性最小二乘

考虑最小二乘问题：

$$\underset{x}{\min }F(x)=\frac{1}{2}\Vert f(x){\Vert }_2^2$$

对于简单的函数，我们可以令其导数 $\frac{\mathrm{d}\mathrm{F}}{\mathrm{d}\mathbf{x}}=0$，得到导数为零处的极值点。

而 SLAM 中的非线性函数形式复杂，常常难以用求导法得到最值，而采用梯度下降法，即，通过不断迭代 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，使得 $||f({\mathbf{x}}_k+\Delta {\mathbf{x}}_k)|{|}^2$ 达到极小值，当 $\Delta \mathbf{x}$ 足够小时，即停止。关键在于增量 $\Delta \mathbf{x}$ 的选取。

过程如下：

——————————————————————————————————————————————————————

• 给定某个初值 ${\mathbf{x}}_0$；

• 对于第 $k$ 次迭代，寻找增量 $\Delta {\mathbf{x}}_k$；

• 当 $\Delta {\mathbf{x}}_k$ 足够小时，即停止；否则，重复第二步，继续寻找。

——————————————————————————————————————————————————————

下面介绍几个常用的优化方法。

4.2 一阶和二阶梯度法

对于非线性函数 $F(\mathbf{x})$，考虑第 $k$ 次迭代，将其在 ${\mathbf{x}}_k$ 附近泰勒展开（将 $\Delta {\mathbf{x}}_k$ 看做未知数），

$$F(\mathbf{x})=F({\mathbf{x}}_k+\Delta {\mathbf{x}}_k)\approx F({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^T\mathbf{H}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k$$

其中，$\mathbf{J}({\mathbf{x}}_k)$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数，$\mathbf{H}({\mathbf{x}}_k)$ 是二阶导数。

（1）最速下降法

仅保留一阶导数时，取增量为反向梯度即可，即

$$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}({\mathbf{x}}_k)$$

也被称为最速下降法。

（2）牛顿法

保留二阶梯度信息。此时增量方程为

$$\Delta {\mathbf{x}}^{\ast }=\arg \min \left(F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{H}\Delta \mathbf{x}\right)$$

右侧各项分别为 $\Delta \mathbf{x}$ 的零次、一次和二次项，将其对 $\Delta \mathbf{x}$ 求导，并令其为零

$$\mathbf{J}+\mathbf{H}\Delta \mathbf{x}=0\Rightarrow \mathbf{H}\Delta \mathbf{x}=-\mathbf{J}$$

求解这个方程，即得到增量。此方法也被称为牛顿法。但在实际中，$\mathbf{H}$ 矩阵计算较为困难。

4.3 高斯牛顿法

将 $f(\mathbf{x})$ 一阶泰勒展开（注意不是 $F(\mathbf{x})$）：

$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}$$

其中，$\mathbf{J}(\mathbf{x})$ 是 $f(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数，为 $n\times 1$ 列向量。

此时，我们的问题变为找到 $\Delta \mathbf{x}$ ，使得 $\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{\Vert }^2$ 最小。将其展开

$$\begin{array}{rl}\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{\Vert }^2& =\frac{1}{2}(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{)}^T(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x})\\ & =\frac{1}{2}(\Vert f(\mathbf{x}{\Vert }^2+2f(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x})\end{array}$$

将 $\Delta \mathbf{x}$ 看做未知数，对其求导，并令其为零：

$$f(\mathbf{x})\mathbf{J}(\mathbf{x})+\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}=0$$

得

$$\underset{\mathbf{H}(\mathbf{x})}{\underbrace{\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{\underbrace{-\mathbf{J}(\mathbf{x})f(\mathbf{x})}}$$

即

$$\mathbf{H}(\mathbf{x})\Delta \mathbf{x}=\mathbf{g}(\mathbf{x})$$

前面说到牛顿法中的二阶矩阵 $\mathbf{H}$ 求解较为困难，所以这里用 ${\mathbf{JJ}}^{\top }$ 作为 $\mathbf{H}$ 的近似，从而省略了计算 $\mathbf{H}$ 的过程。

4.4 总结

（1）最速下降法

$$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}({\mathbf{x}}_k)$$

（2）牛顿法

$$\mathbf{H}\Delta \mathbf{x}=-\mathbf{J}$$

（3）高斯牛顿法

$$\underset{\mathbf{H}(\mathbf{x})}{\underbrace{\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{\underbrace{-\mathbf{J}(\mathbf{x})f(\mathbf{x})}}$$

需要注意的是，最速下降法和牛顿法属于同一类型，他们的 $\mathbf{J}$ 和 $\mathbf{H}$ 都是针对 $\mathbf{F}(\mathbf{x})$（也就是整个目标函数）；而高斯牛顿法的 $\mathbf{J}$ 是针对 $\mathbf{f}(\mathbf{x})$ （也就是误差项）。 此外，为减小数值，目标函数 $\mathbf{F}(\mathbf{x})$ 可用均方差函数 。

4.5 代码实现

考虑一条满足以下方程的曲线

$$y=\exp (a{x}^2+bx+c)+w$$

其中，$a,b,c$ 为曲线参数，$w$ 为高斯噪声，满足 $w\sim N(0,{\sigma }^2)$。假设有 $N$ 个关于 $x,y$ 的观测数据点，落在该直线附近，我们想用该方程进行拟合，也就是求出使残差最小的曲线参数 $a,b,c$。定义残差

$$(a,b,c{)}^{\ast }=\arg \underset{a,b,c}{\min }\frac{1}{2}\sum _{i=1}^N\Vert y_i-\exp (a{x}_i^2+b{x}_i+c){\Vert }_2^2$$

注意，这个问题的带估计变量是 $a,b,c$，而不是 $x$。

我们先根据模型生成 $x,y$ 的真值，再加入高斯噪声，构成观测数据点。定义每一项的误差

$$e_i=y_i-\exp (a{x}_i^2+b_{i}x+c)$$

（1）最速下降法

$$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}$$

其中，$\mathbf{J}=\sum {\mathbf{J}}_i$， ${\mathbf{J}}_i=[\frac{\partial \mathbf{F}}{\partial a},\frac{\partial \mathbf{F}}{\partial b},\frac{\partial \mathbf{F}}{\partial c}{]}^{\top }$。

$$\begin{gathered} \frac{\partial \mathbf{F}}{\partial a}=e_i(-x_i^2\exp (a{x}_i^2+b{x}_i+c)) \\ \frac{\partial \mathbf{F}}{\partial a}=e_i(-x_i\exp (a{x}_i^2+b{x}_i+c)) \\ \frac{\partial \mathbf{F}}{\partial a}=e_i(\exp (a{x}_i^2+b{x}_i+c)) \end{gathered}$$

/***********************************************************                                          *
* Time: 2023/8/10
* Author: xiaocong
* Function: 最速下降法
* 注意这里用的是 ae * xi * xi + be * xi + sin(ce)
* 原指数函数对误差和初值敏感，造成计算无穷大。
***********************************************************/#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <cmath>using namespace std;
using namespace Eigen;const int N = 100;                 // 数据点个数
const int MAX_INTER = 1000;        // 最大迭代次数void steepestDescent(const VectorXd& x_data, const VectorXd& y_data, Vector3d& params)
{double cost = 0;for (int iter = 0; iter < MAX_INTER; iter++){Vector3d J = Vector3d::Zero();           // 雅可比矩阵cost = 0;double ae = params(0);double be = params(1);double ce = params(2);for (int i = 0;i < N;i++){double xi = x_data(i), yi = y_data(i);double ei = (yi - (ae * xi * xi + be * xi + sin(ce)));    // 残差项Vector3d Ji;                                          // 雅可比矩阵Ji(0) = ei * (-xi * xi);Ji(1) = ei * (-xi);Ji(2) = ei * (-cos(ce));J += Ji;cost += ei * ei;}Vector3d dx = -J / N;          // 求解 dxif (isnan(dx[0])){cout << "result is nan!" << endl;break;}if (dx.squaredNorm() < 1e-6)      // dx 足够小{break;}// 迭代params += dx;cout << "iter= " << iter + 1 << endl;cout << dx(0) << endl;cout << dx(1) << endl;cout << dx(2) << endl;cout << "cost: " << cost << endl;cout << "estimated abc= " << params(0) << ", " << params(1) << ", " << params(2) << endl;}}int main(int argc, char** argv)
{double ar = 1.0, br = 2.0, cr = 1;         // 真实参数值Vector3d params(2.0, 1.0, 0);            // 初始参数值// 生成数据VectorXd x_data(N);VectorXd y_data(N);for (int i = 0; i < N; i++){double xi = (i / 100.0);                                     // [0~1.0]double sigma = 0.02 * (rand() % 1000) / 1000.0 - 0.01;     // 随机噪声，[-0.01, 0.01]double yi = ar * xi * xi + br * xi + sin(cr) + sigma;x_data(i) = xi;y_data(i) = yi;}steepestDescent(x_data, y_data, params);return 0;
}

iter= 454
-0.000666889
0.000694395
-0.000279458
cost: 0.0427055
estimated abc= 1.26563, 1.72471, 1.09882

（2）牛顿法

$$\mathbf{H}\Delta \mathbf{x}=-\mathbf{J}$$

这里还需要求出海森矩阵即二阶导

$${\mathbf{H}}_i=\left[\begin{array}{ccc}\frac{{\partial }^2\mathbf{F}}{\partial a^2}& \frac{{\partial }^2\mathbf{F}}{\partial a\partial b}& \frac{{\partial }^2\mathbf{F}}{\partial a\partial c}\\ \frac{{\partial }^2\mathbf{F}}{\partial b\partial a}& \frac{{\partial }^2\mathbf{F}}{\partial b^2}& \frac{{\partial }^2\mathbf{F}}{\partial b\partial c}\\ \frac{{\partial }^2\mathbf{F}}{\partial c\partial a}& \frac{{\partial }^2\mathbf{F}}{\partial c\partial b}& \frac{{\partial }^2\mathbf{F}}{\partial c^2}\end{array}\right]$$

则 $\mathbf{H}=\sum {\mathbf{H}}_i$

/***********************************************************                                          *
* Time: 2023/8/10
* Author: xiaocong
* Function: 牛顿法
* 注意这里用的是 ae * xi * xi + be * xi + sin(ce)
***********************************************************/#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <cmath>using namespace std;
using namespace Eigen;const int N = 100;                 // 数据点个数
const int MAX_INTER = 1000;        // 最大迭代次数void steepestDescent(const VectorXd& x_data, const VectorXd& y_data, Vector3d& params)
{double cost = 0;for (int iter = 0; iter < MAX_INTER; iter++){Vector3d J = Vector3d::Zero();           // 雅可比矩阵Eigen::MatrixXd H(3, 3);                 // 海森矩阵H.setZero();cost = 0;double ae = params(0);double be = params(1);double ce = params(2);for (int i = 0;i < N;i++){double xi = x_data(i), yi = y_data(i);double ei = (yi - (ae * xi * xi + be * xi + sin(ce))) / N;    // 残差项J(0) += ei * (-xi * xi);J(1) += ei * (-xi);J(2) += ei * (-cos(ce));H(0, 0) += xi * xi * xi * xi;H(0, 1) += xi * xi * xi;H(0, 2) += xi * xi * cos(ce);H(1, 0) += xi * xi * xi;H(1, 1) += xi * xi;H(1, 2) += xi * cos(ce);H(2, 0) += xi * xi * cos(ce);H(2, 1) += xi * cos(ce);H(2, 2) += cos(2 * ce);cost += ei * ei;}Vector3d dx = H.ldlt().solve(-J);   // 求解 dxif (isnan(dx[0])){cout << "result is nan!" << endl;break;}if (dx.squaredNorm() < 1e-6)      // dx 足够小{break;}// 迭代params += dx;cout << "iter= " << iter + 1 << endl;cout << dx(0) << endl;cout << dx(1) << endl;cout << dx(2) << endl;cout << "cost: " << cost << endl;cout << "estimated abc= " << params(0) << ", " << params(1) << ", " << params(2) << endl;}}int main(int argc, char** argv)
{double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值Vector3d params(2.0, 1.0, 0.0);            // 初始参数值// 生成数据VectorXd x_data(N);VectorXd y_data(N);for (int i = 0; i < N; i++){double xi = (i / 100.0);                                     // [0~1.0]double sigma = 0.02 * (rand() % 1000) / 1000.0 - 0.01;     // 随机噪声，[-0.01, 0.01]double yi = ar * xi * xi + br * xi + sin(cr) + sigma;x_data(i) = xi;y_data(i) = yi;}steepestDescent(x_data, y_data, params);return 0;
}

iter= 470
-0.000656148
0.000766084
5.77874e-06
cost: 3.52463e-06
estimated abc= 1.09233, 1.89203, 1.01297

（3）高斯牛顿法

求出每个误差项关于状态变量的导数（注意 $a,b,c$ 才是未知数）：

$$\begin{gathered} \frac{\partial e_i}{\partial a}=-x_i^2\exp (a{x}_i^2+b_{i}x+c) \\ \frac{\partial e_i}{\partial b}=-x_i\exp (a{x}_i^2+b_{i}x+c) \\ \frac{\partial e_i}{\partial c}=-\exp (a{x}_i^2+b_{i}x+c) \end{gathered}$$

记 ${\mathbf{J}}_i=[\frac{\partial e_i}{\partial a},\frac{\partial e_i}{\partial b},\frac{\partial e_i}{\partial c}{]}^{\top }$。和前面不一样。

$$\underset{\mathbf{H}(\mathbf{x})}{\underbrace{\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{\underbrace{-\mathbf{J}(\mathbf{x})f(\mathbf{x})}}$$

此处即为

$$(\sum _{i=1}^{100}{\mathbf{J}}_i{\mathbf{J}}_i^{\top })\Delta \mathbf{x}=-\sum _{i=1}^{100}{\mathbf{J}}_i{e}_i$$

/***********************************************************                                          *
* Time: 2023/8/10
* Author: xiaocong
* Function: 高斯牛顿法
***********************************************************/#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <cmath>using namespace std;
using namespace Eigen;const int N = 100;                // 数据点个数
const int MAX_INTER = 100;        // 最大迭代次数void gaussNewton(const VectorXd& x_data, const VectorXd& y_data, Vector3d& params)
{double cost = 0;for (int iter = 0; iter < MAX_INTER; iter++){Matrix3d H = Matrix3d::Zero();      // H=JJ^TVector3d b = Vector3d::Zero();      // b=-Jecost = 0;double ae = params(0);double be = params(1);double ce = params(2);for (int i = 0;i < N;i++){double xi = x_data(i), yi = y_data(i);double ei = yi - exp(ae * xi * xi + be * xi + ce);    // 残差项Vector3d Ji;                                          // 雅可比矩阵Ji(0) = -xi * xi * exp(ae * xi * xi + be * xi + ce);Ji(1) = -xi * exp(ae * xi * xi + be * xi + ce);Ji(2) = -exp(ae * xi * xi + be * xi + ce);H += Ji * Ji.transpose();b += -Ji * ei;cost += ei * ei;}// 求解 dxVector3d dx = H.ldlt().solve(b);if (isnan(dx[0])){cout << "result is nan!" << endl;break;}if (dx.squaredNorm() < 1e-6)      // dx 足够小{break;}// 迭代params += dx;cout << "iter= " << iter + 1 << endl;cout << dx(0) << endl;cout << dx(1) << endl;cout << dx(2) << endl;cout << "cost= " << cost << endl;cout << "estimated abc= " << params(0) << ", " << params(1) << ", " << params(2) << endl;}
}int main(int argc, char** argv)
{double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值Vector3d params(2.0, -1.0, 5.0);            // 初始参数值// 生成数据VectorXd x_data(N);VectorXd y_data(N);for (int i = 0; i < N; i++){double xi = i / 100.0;                                     // [0~1]double sigma = 0.02 * (rand() % 1000) / 1000.0 - 0.01;     // 随机噪声，[-0.01, 0.01]double yi = exp(ar * xi * xi + br * xi + cr) + sigma;x_data(i) = xi;y_data(i) = yi;}gaussNewton(x_data, y_data, params);return 0;
}

iter= 7
-0.00134314
0.00213517
-0.000823118
cost= 0.00356444
estimated abc= 0.999314, 2.00098, 0.999659

4.6 三种方法优缺点

最速下降法、牛顿法和高斯牛顿法都是常用的优化算法，用于解决最小化问题。它们各自有不同的优缺点，下面是对这三种算法的优缺点进行总结：

最速下降法：

优点：

1. 简单直观： 最速下降法的核心思想简单易懂，容易实现。
2. 适用广泛： 适用于一般的优化问题，不需要太多的先验知识。
3. 内存消耗小： 只需要存储当前参数和梯度，内存消耗相对较小。

缺点：

1. 收敛速度慢： 最速下降法的收敛速度通常较慢，特别是在存在长轴状谷底或细长山谷的情况下。
2. 依赖初始值： 初始参数的选择会影响收敛性能，不合适的初始值可能导致陷入局部最小值。
3. 不适用于高维问题： 在高维问题中，计算梯度和参数更新可能变得耗时，从而影响效率。

牛顿法：

优点：

1. 收敛速度快： 牛顿法在逼近局部最小值时的收敛速度通常比最速下降法更快。
2. 二阶信息利用： 牛顿法利用了函数的二阶导数信息，可以更准确地估计局部几何特性。

缺点：

1. 计算复杂： 牛顿法需要计算函数的二阶导数（Hessian矩阵），计算成本较高，特别是在高维问题中。
2. 不稳定性： Hessian矩阵可能不是正定的，导致算法不稳定。此外，在某些情况下，牛顿法可能在非凸问题中陷入局部最小值。

高斯牛顿法：

优点：

1. 适用于非线性问题： 高斯牛顿法适用于非线性优化问题，尤其是拟合参数化模型时效果较好。
2. 不需要二阶导数： 高斯牛顿法不需要直接计算Hessian矩阵，而是通过近似方法来计算。

缺点：

1. 对初始值敏感： 高斯牛顿法对初始参数的选择敏感，不合适的初始值可能导致算法发散。
2. 可能陷入局部最小值： 高斯牛顿法有时可能陷入局部最小值，特别是在存在多个局部最小值的问题中。

综合来看，选择哪种算法取决于问题的性质和实际需求。最速下降法简单但收敛速度慢，适用于简单问题或作为其他优化算法的初始步骤。牛顿法收敛速度快，但在计算复杂和非凸问题中可能不稳定。高斯牛顿法适用于非线性问题，但对初始值敏感，需要权衡利弊。在实际应用中，可能需要结合多种算法来优化不同的问题。