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

参考博客

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

数学基础(泰勒展开)

泰勒展开公式:
$f(x)=\frac{f(x_{0})}{0!}+ \frac{f^{'}(x_{0})}{1!}(x-x_{0})+\frac{f^{''}(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^{'}(x)\Delta{x}}$
取$J(x)=f^{'}(x), J(x)$表示$f(x)$的一阶导数,是雅克比矩阵.

目的: 通过不断寻找下降矢量$\Delta{x}$, 使目标函数$||f(x+\Delta{x})||^{2}$达到最小值, 变为线性的最小二乘问题:
$\Delta{x}^{*}=\argmin_{\Delta{x}}\frac{1}{2}||f(x+\Delta{x})||^{2}=\argmin_{\Delta{x}}\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*X, \frac{dY}{dX}=A^{T}$
$Y=X*A, \frac{dY}{dX}=A$
$\frac{dX^{T}}{dX}=I$

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

1.2 GN迭代算法步骤

• 给定初始值$x_{0}$, 即取$x=x_{0}$.
• 对于第k次迭代,期初一阶导数雅克比矩阵$J(x_{k})=f^{'}(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\geq{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 \max_{i}(a_{ii}^{0})$其中$\tau$需要自己设定, $a_{ii}$为$A_{0}$的对角线元素.

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

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

  • (1) 若$\rho_{k}\leq0.25$,说明步子过大, 泰勒近似较差,应缩小信赖域半径, 取$u_{k+1}=\frac{1}{2}u_{k}$
  • (2) 若$\rho_{k}\geq0.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}\leq0$,说明误差向着上升而非下降的趋势变化了（与最优化的目标相反），这说明这一步迈得错得“离谱”了，这时不应该走到下一点，而应“原地踏步”，即取$x_{k+1}=x_{k}$
  • (2) 若$\rho_{k}\geq0$,说明可以向下一步走, 取$x_{k+1}=x_{k}+\Delta x_{k}$

2.3 优缺点

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

3. 手抄版