視覺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. 手抄版