视觉SLAM笔记--第4篇: 高斯牛顿法(GN)和列文伯格-马夸特算法(LM)的算法流程，优劣分析

参考博客

参考博客: https://blog.csdn.net/heshaofeng2ly/article/details/105812746#3GN_50
参考博客:LM算法流程

数学基础(泰勒展开)

泰勒展开公式:
$$f(x)=\frac{f(x_0)}{0!}+\frac{f^{{}^{\prime }}(x_0)}{1!}(x-x_0)+\frac{f^{{}^{\prime \prime }}(x_0)}{2!}(x-x_0{)}^2+......+\frac{f^{(n)}(x_0)}{n!}(x-x_0{)}^n$$

该式表示 $f(x)$在 $x_0$处的 $n$阶泰勒展开.

1. 高斯牛顿法(GN法)

Gauss Newton 是最优化算法里面最简单的方法之一。它的思想是将$f(x)$ 进行一阶的泰勒展开.

1.1 基本原理

待优化的目标函数: $||f(x+\Delta x)|{|}^2$

将目标函数中$f(x+\Delta x)$ 进行一阶泰勒展开可得:
$$f(x+\Delta x)\approx f(x)+f^{{}^{\prime }}(x)\Delta x$$
取$J(x)=f^{{}^{\prime }}(x),J(x)$表示$f(x)$的一阶导数,是雅克比矩阵.

目的: 通过不断寻找下降矢量$\Delta x$, 使目标函数$||f(x+\Delta x)|{|}^2$达到最小值, 变为线性的最小二乘问题:
$$\Delta x^{\ast }=\underset{\Delta x}{\mathrm{argmin}}\frac{1}{2}||f(x+\Delta x)|{|}^2=\underset{\Delta x}{\mathrm{argmin}}\frac{1}{2}||f(x)+J(x)\Delta x|{|}^2$$将其展开:
$$\frac{1}{2}||f(x)+J(x)\Delta x|{|}^2=\frac{1}{2}[f(x)+J(x)\Delta x{]}^T[f(x)+J(x)\Delta x]$$$$=\frac{1}{2}[|f(x)|{|}^2+f^T(x)J(x)\Delta x+\Delta x^T{J}^T(x)f(x)+\Delta x^T{J}^T(x)J(x)\Delta x]$$这里需要注意的是$\Delta x^T{J}^T(x)f(x)=(f^T(x)J(x)\Delta x{)}^T$ 而转置不改变值的大小, 两者可以合并, 得到:
$$\frac{1}{2}||f(x)+J(x)\Delta x|{|}^2=\frac{1}{2}[|f(x)|{|}^2+2f^T(x)J(x)\Delta x+\Delta x^T{J}^T(x)J(x)\Delta x]$$求上式关于 ∆x 的导数，并令其为零：
$$2J^T(x)f(x)+2J^T(x)J(x)\Delta x=0$$

这里需要注意的是:
$Y=A\ast X,\frac{dY}{dX}=A^T$
$Y=X\ast A,\frac{dY}{dX}=A$
$\frac{d{X}^T}{dX}=I$

可以得到如下方程组:
$$J^T(x)J(x)\Delta x=-J^T(x)f(x)$$注意，我们要求解的变量是 ∆x，因此这是一个线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程或者正规方程.
其中$J(x)=f^{{}^{\prime }}(x)$表示$f(x)$的一阶导数,是雅克比矩阵.$f(x)$为x处的值

1.2 GN迭代算法步骤

• 给定初始值$x_0$, 即取$x=x_0$.
• 对于第k次迭代,期初一阶导数雅克比矩阵$J(x_k)=f^{{}^{\prime }}(x_k)$, 以及目标函数(误差)$f(x_k)$.
• 求解增量方程: $J^T(x_k)J(x_k)\Delta x_k=-J^T(x_k)f(x_k)$, 将(2)的值带入方程,求出$\Delta x_k$.
• 若$\Delta x_k$足够小,停止迭代. 否则令$x_{k+1}=x_k+\Delta x_k$, 返回(2), 继续迭代计算.

1.3 优缺点

• 优点: 高斯牛顿（Gauss-Newton）法是对牛顿法的一种改进，它用雅克比矩阵的乘积近似代替牛顿法中的二阶Hessian 矩阵，从而省略了求二阶Hessian 矩阵的计算,计算量降低.

• 缺点1: 在高斯牛顿法中，用来近似Hessian矩阵的$J^{T}J$可能是奇异矩阵(不可逆)或者病态的，此时会导致方程无解，稳定性很差，算法不收敛.

• 缺点2: 由于采用二阶泰勒展开来进行的推导，而泰勒展开只是在一个较小的范围内的近似，因此如果高斯牛顿法计算得到的步长较大的话，上述的近似将不再准确，也会导致算法不收敛.

2. 列文伯格-马夸特法(LM法)

Levenberg-Marquardt (LM)在一定程度上修正了高斯牛顿法的缺点，因此它比高斯牛顿法更加鲁棒，不过是以牺牲一定的收敛速度为代价–它的收敛速度比高斯牛顿法慢. 也被称为阻尼牛顿法.

2.1 基本原理

LM法加入一个正定对角阵$uI$, 一定程度上修正了GN的缺点.

LM算法增量方程:

$$(J^T(x)J(x)+uI)\Delta x=-J^T(x)f(x)$$其中$u\ge 0,u$表示信赖域半径.

• 当$u=0$时,LM退化为高斯牛顿法(GN)
• 当$u$很大时,LM退化为一阶梯度下降法

LM法会在每一次迭代计算因子$\rho$来判断泰勒近似是否良好,并根据因子$\rho$,动态扩大或缩小信赖域半径$u$.
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$$

• 若因子$\rho$接近于1, 则认为泰勒近似比较准确, 可以扩大信赖域半径$u$
• 若因子$\rho$远小于1, 则认为泰勒近似结果较差, 可以缩小信赖域半径$u$

2.2 LM迭代算法步骤

• 给定初始值$x_0$, 设置$u$初始值$u_0$
  $$A_0=J^T(x_0)J(x_0)$$$$u_0=\tau \underset{i}{\max }(a_{ii}^0)$$其中$\tau$需要自己设定, $a_{ii}$为$A_0$的对角线元素.

• 第k次迭代,根据前面的公式求出步长$\Delta x_k$, 并计算此时的${\rho }_k$.

• 根据${\rho }_k$的取值来调整信赖域半径:

  • (1) 若${\rho }_k\le 0.25$,说明步子过大, 泰勒近似较差,应缩小信赖域半径, 取$u_{k+1}=\frac{1}{2}{u}_k$
  • (2) 若${\rho }_k\ge 0.75$,说明步子较小, 泰勒近似准确,应扩大信赖域半径, 取$u_{k+1}=2u_k$
  • (3) 若$0.25<{\rho }_k<0.75$,说明 泰勒近似介于两者之间,应保持此时的信赖域半径, 取$u_{k+1}=u_k$

• 若$\Delta x_k$足够小, 则停止迭代, 否则根据${\rho }_k$大小判断$x_{k+1}$应该如何更新. 计算出$x_{k+1}$后返回(2), 继续进行迭代:

  • (1) 若${\rho }_k\le 0$,说明误差向着上升而非下降的趋势变化了（与最优化的目标相反），这说明这一步迈得错得“离谱”了，这时不应该走到下一点，而应“原地踏步”，即取$x_{k+1}=x_k$
  • (2) 若${\rho }_k\ge 0$,说明可以向下一步走, 取$x_{k+1}=x_k+\Delta x_k$

2.3 优缺点

• 优点: 在一定程度上修正了高斯牛顿算法不收敛的缺点，同时具备高斯牛顿法和一阶梯度算法的特点, 因此它比高斯牛顿法更加鲁棒.
• 缺点: 由于需要不断计算更新收敛域半径$u$,不断变化梯度下降步长,会导致收敛速度较慢.

3. 手抄版