非線性優化問題以及在視覺SLAM中的應用

1.0 最小二乘基礎概念

定義

$\quad$ 找到一個 n 維的變數 $\mathbf{x}^{*} \in \mathbb{R}^{n}$ , 使得損失函數 $F(\mathbf{x})$ 取區域性最小值：

\[F(\mathbf{x})=\frac{1}{2} \sum_{i=1}^{m}\left(f_{i}(\mathbf{x})\right)^{2} \]

$\quad$其中 $f_{i}$ 是殘差函數, 比如測量值和預測值之間的差, 且有 $m \geq n$ 。部最小值指對任意 $\left\|\mathbf{x}-\mathbf{x}^{*}\right\|<\delta$ 有 $F\left(\mathbf{x}^{*}\right) \leq F(\mathbf{x})$
$\quad$損失函數泰勒展開，假設損失函數 $F(\mathbf{x})$ 是可導並且平滑的, 因此, 二階泰勒展開:

\[F(\mathbf{x}+\Delta \mathbf{x})=F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x}+O\left(\|\Delta \mathbf{x}\|^{3}\right) \]

這裡要著重注意一下，這裡的 $\mathbf{J}$ 和 $\mathbf{H}$ 都是每一個殘差項的雅可比堆疊(計算)而來，實際上對於初學者來說並不直觀，後面我們會以一個曲線擬合和 $BA$ 問題來詳細分析一下如何通過連加來推算到 $\mathbf{J}$ 和 $\mathbf{H}$

$\quad$其中 $\mathbf{J}$ 和 $\mathbf{H}$ 分別為損失函數 $F$ 對變數 $x$ 的一階導和二階導矩陣，也就是我們通常所說的雅可比矩陣和海塞矩陣。(這裡的 $\mathbf{x}$ 包含了所有待優化的變數，在視覺SLAM問題中，一般是相機的 Pose 和已經三角化的點的座標或者逆深度，且由於相機一般能觀測到的3D點的個數是有限的，因此其雅可比矩陣也就是稀疏的，只有兩個地方的雅可比求導是不為0的，參考14講P247，那麼 $J_{i,j}^TJ_{i,j}$，則只有四個地方是不為0的)。

損失函數泰勒展開的性質

$\quad$ 忽略泰勒展開的高階項,損失函數變成了二次函數,可以輕易得到如下性質：

如果在點 $x_s$ 處有導數為 $0$ ,則稱這個點為穩定點。
在點 $x_s$ 處對應的 Hessian 為 $\mathbf{H}$：
- 如果是正定矩陣，即它的特徵值都大於 $0$，則在 $x_s$ 處有 $F (x)$ 為區域性最小值。
- 如果是復定矩陣，即它的特徵值都小於 $0$，則在 $x_s$ 處有 $F (x)$ 為區域性最大值。
- 如果是不定矩陣，即它的特徵值大於 $0$ 也有小於 $0$ 的,則 $x_s$ 處為鞍點。

求解方法主要有以下兩種：
- 直接求解：線性最小二乘（這裡不再贅述，為線性代數的內容，超定方程的通解為 $x=(A^TA)^{-1}A\ b$）
- 迭代下降法：適用於線性和非線性最小二乘

2.0 迭代下降求解方式

迭代法初衷：

找到一個下降方向使得損失函數隨著 $x$ 的迭代逐漸減少直到 $x^*$。

\[F(x_{k+1})<F(x_k) \]
分兩個步驟；第一，找到下降方向單位向量 $d$ ，第二，確定下降的步長 $a$。

假設 $a$ 足夠的小，又因為 $d$ 為單位向量，因此可以將 $ad$ 看作是一個微小的變化量 $\triangle{x}$，我們可以對損失函數進行一階泰勒展開：

\[F(\mathbf{x}+a \ \mathbf{d}) \approx F(\mathbf{x}) + a\mathbf{J}\mathbf{d} \]
只需要尋找下降方向，滿足：

\[\mathbf{Jd}<0 \]
通過 line search 的方法找到下降的步長：$a^*=argmin_{a>0} [F(x+ad)]$

2.1 最速下降法: 適用於迭代的開始階段

適用於迭代的開始階段

$\quad$ 從下降方向的條件(單位向量)可以知道: $\mathbf{Jd=||J||}cos\theta$ ，其中 $\theta$ 表示的是下降方向和梯度方向的夾角. 當 $\theta = \pi$ 有:

這裡為什麼能寫成向量的內積運算,筆者在剛開始看起來還認為是兩個矩陣相乘法,卻直接寫成了內積運算，仔細思索發現 $d$ 其實上是一個和 $x$ 相同維度的單位向量，其緯度為 $n\times 1$ ，而雅可比矩陣

\[\mathbf{d=\frac{-J^T}{||J||}} \]

$\quad$即梯度的負方向為最速下降方向。缺點:最優值附近震盪，收斂慢。

2.2 牛頓法: 適用於最優值附近

在區域性最優點 $x^∗$ 附近,如果 $x + ∆x$ 是最優解,則損失函數對 $∆x$ 的導數等於 $0$,對公式 (2) 取一階導有:

\[\begin{align*} \frac{\partial}{\partial \Delta x}\left (F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x} \right) &=\mathbf{J^T} + \mathbf{H}\Delta x =0 \end{align*} \]

得到:$∆x = -\mathbf{H^{-1}J^T}$ 。缺點:二階導矩陣計算複雜。

這裡我們其實既可以看作是多個殘差的分量相加後組成的 $H$，也可以看作是每個殘差單獨的 $H$。

2.3 阻尼法：防止 $\Delta x$ 的模過大

將損失函數的二階泰勒展開記作：

\[F(\mathbf{x}+\Delta x)\approx L(\Delta x) = F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x} \]

求以下函數的最小化:

\[\Delta x = arg \ \underset{\Delta x}{min} \left \{ L \left (\Delta x\right ) + \frac{1}{2}\mu \Delta x^T\Delta x \right \} \]

其中,$μ ≥ 0$ 為阻尼因子, $ \frac{1}{2}\mu \Delta x^T\Delta x $是懲罰項。對新的損失函數求一階導,並令其等於 $0$ 有:

\[\mathbf{L^`(\Delta x)} + \mu \mathbf{\Delta x} = 0 \\ (\mathbf{H}+\mu\mathbf{I})\Delta x = -\mathbf{J^T} \]

2.4 Gauss-Newton 和 LM

殘差函數 $f(x)$ 為非線性函數，對其進行一階泰勒近似有：

\[f(x+\Delta x)\approx \ell (\Delta x) \equiv f(x)+J\Delta x \]

帶入損失函數：

\[\begin{aligned} F(\mathbf{x}+\Delta \mathbf{x}) \approx L(\Delta \mathbf{x}) & \equiv \frac{1}{2} \ell(\Delta \mathbf{x})^{\top} \ell(\Delta \mathbf{x}) \\ & =\frac{1}{2} \mathbf{f}^{\top} \mathbf{f}+\Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \\ & =F(\mathbf{x})+\Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \end{aligned} \]

這裡我們假設暫時只關注其中的一項（其實也可以是所有損失項的疊加，最終形式是一樣的）。在 $x$ 處進行的泰勒展開，則認為 $x$ 是已知的，現在的損失函數是一個關於 $\Delta x$ 的函數，其最小值，則令關於 $\Delta x$ 的導數為 $0$ 即可。可以得到：

\[\mathbf{(J^T J)}\Delta x_{gn}=-\mathbf{J^T f} \]

上面這個形式就是我們在論文或者各種SLAM問題中經常見到的形式了，$\mathbf{H \Delta x =b}$，也叫做 normal equation

曲線擬合理解

現在我們通過非線性最小二乘來進行線性擬合實驗，將理論應用於實際中去。假設曲線方程為：

\[y=\exp (ax^2+bx+c) \]

其中設 $a=1,b=2,c=3$ 。

現在加入噪聲項生成帶有高斯分佈的噪聲資料，當然不是高斯分佈的資料也是可以的，但是在自己實驗的時候儘量不要出現外點資料，因為我們並沒有處理外點資料的策略。則生成資料的方程為：

\[y=\exp (ax^2+bx+c) + w \]

其中 $w$ 為符合高斯分佈的噪聲資料。

通過如下程式生成觀測資料：

double ar = 1.0, br = 2.0, cr = 1.0;         // 真實引數值
int N = 100;                                 					// 資料點
double w_sigma = 1.0;                      	 	 // 噪聲Sigma值
vector<double> x_data, y_data;        // 資料
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}

接下來我們關心雅可比如何計算，誤差項 $f_i(a,b,c)$ 可以寫成如下形式：

\[f_i(a,b,c)=y_i-exp(a_ex_i^2+b_ex_i+c_e) \]

我們知道這兩項相減是絕對不可能相等的，因為在生成資料的時候加入了高斯噪聲。我們這裡有 $N$ 個觀測，即 $i\in (1-100)$，我們將其寫成連加的形式

\[F(X)=\sum_{i=1}^{N}\left (y_i-exp(a_ex_i^2+b_ex_i+c_e) \right)^2 \]

該式中右邊就是殘差項的具體形式，我們對其進行平方，防止負的殘差和正的殘差抵消的情況，前面我們已經說過可以將殘差項通過一階泰勒展開進行近似，然後進行平方得到每一個殘差項的具體形式：

$f(x+\Delta x)\approx f(x)+J(x)\Delta x$

\[\begin{aligned} \frac{1}{2}\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\|^{2} & =\frac{1}{2}(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x})^{T}(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}) \\ & =\frac{1}{2}\left(\|f(\boldsymbol{x})\|_{2}^{2}+2 f(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\Delta \boldsymbol{x}^{T} \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\right) \\ &=\Omega(\Delta x) \end{aligned} \]

此時由於某時刻的觀測已知，因此誤差項是一個關於 $\Delta x$ 的二次函數，求該項的最小值只要讓關於 $\Delta x$ 的導數為 $0$ 即可。求導後可得：

\[\begin{array}{l} 2 \boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x})+2 \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{0} \\ \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=-\boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x}) \end{array} \]

這裡我們簡單的記：

\[\boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x}) = \mathbf{\eta}\\ \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{H\Delta x} \]

即我們常見的形式：

讀者要注意到這裡的 $b$ 其實就是上面的 $-\eta$

\[\mathbf{H\Delta x=b} \]

這裡我們假設殘差項記為 $\mathbf{e_i}$ 一共有 $N$ 個觀測，則有 $N$ 個殘差項。

\[F(X)=\mathbf{e_1^2 + e_2^2+ e_3^2+ e_4^2+ e_5^2+ \dots + e_N^2} \]

整個 $F(X)$ 此時是關於待優化變數的函數，每一項分別用各自的一階泰勒展開近似，注意這裡的每一項由於觀測的不同，每一項都是一個不同的函數表示式，但是優化變數都是一樣的。得到如下結果：

\[\begin{aligned} \frac{1}{2}\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\|^{2} &=\Omega(\Delta x) \end{aligned} \]

\[F(X)\approx \Omega(\Delta x)_1 + \Omega(\Delta x)_2+ \Omega(\Delta x)_3+ \Omega(\Delta x)_4 \dots + \Omega(\Delta x)_N \]

這裡的 $\Delta x$ 是我們在使用基於迭代下降的方法中所選中的步長和方向，如果 $F(X)$ 在 $\Delta x$ 為某個值時取得極小值，則 $\Delta x$無論是在任何一個方向加或者減函數值都會上升，此時這個點則為極小值點，這裡的敘述不太數學化，但是大家聯想一下極小值的定義，應該是可以理解的，當達到該條件後，那麼該點關於 $\Delta x$ 的導數一定為 $0$ 。所以對此時的$F(X)$求導並讓其等於 $0$ 得到：

\[\mathbf{H_1\Delta x } + \mathbf{\eta_1} + \mathbf{H_2\Delta x } + \mathbf{\eta_2} + \mathbf{H_3\Delta x } + \mathbf{\eta_3} \dots + \mathbf{H_N\Delta x } + \mathbf{\eta_N} = \mathbf{0} \]

再將該式子變形，將關於 $\Delta x$ 的項都移動到左邊，沒有關於 $\Delta x$ 的移動到右邊：

\[\mathbf{H_1\Delta x } + \mathbf{H_2\Delta x } + \mathbf{H_3\Delta x } + \dots + \mathbf{H_N\Delta x }= - \mathbf{\eta_1} - \mathbf{\eta_2} - \mathbf{\eta_3} \dots - \mathbf{\eta_N} \]

其實也就是:

\[\mathbf{H_1\Delta x } + \mathbf{H_2\Delta x } + \mathbf{H_3\Delta x } + \dots + \mathbf{H_N\Delta x }= \mathbf{b_1} + \mathbf{b_2} + \mathbf{b_3} \dots + \mathbf{b_N} \]

寫成連加的形式：

\[\Delta x\sum_{i=1}^{N}H = \sum_{i=1}^{N}b \]

這裡我們就通過每一項的一個具體形式來推倒出最後的 H 和 b 是怎麼來的了。也就是我們經常在程式中見到的 += 操作的原理：

H +=  J * J.transpose();
b += -J * error;

我們再次回到曲線擬合的題目中去，待優化的變數就三個 $a,b,c$ 則每一個殘差項都含有這三個引數，我們稱其雅可比為稠密的（雖然只有三個引數，視覺BA問題中由於相機觀測的特殊性，其雅可比矩陣是稀疏的），對每一個殘差向分別求雅可比，然後求和得到最終的 $H$ 和 $b$ ，然後求解一次 $\Delta x$ ，Step 一次，根據判斷條件選擇是否繼續進行迭代。每一個殘差項對於 $\Delta x$ 的雅可比為

\[\begin{array}{l} J[0]_a = -x_i^2 \exp(a_ex_i^2-b_ex_i-c) \\ J[1]_b = -x_i \exp(a_ex_i^2-b_ex_i-c) \\ J[2]_c = -\exp(a_ex_i^2-b_ex_i-c) \\ \end{array} \]

得到了雅可比，那麼剩下的就是迭代求解即可，完整程式碼如下，來自14講配套程式碼：

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
  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亂數產生器

  vector<double> x_data, y_data;      // 資料
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 開始Gauss-Newton迭代
  int iterations = 100;    // 迭代次數
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {

    Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;

    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i個資料點
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩陣
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

      H +=  J * J.transpose();
      b += -J * error;

      cost += error * error;
    }

    // 求解線性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

執行結果如下：

total cost: 3.19575e+06,                update: 0.0455771  0.078164 -0.985329           estimated params: 2.04558,-0.921836,4.01467
total cost: 376785,             update:  0.065762  0.224972 -0.962521           estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6,            update: -0.0670241   0.617616  -0.907497                estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01,            update: -0.522767   1.19192 -0.756452           estimated params: 1.52155,1.11267,1.3882
total cost: 174.853,            update: -0.537502  0.909933 -0.386395           estimated params: 0.984045,2.0226,1.00181
total cost: 102.78,             update: -0.0919666   0.147331 -0.0573675                estimated params: 0.892079,2.16994,0.944438
total cost: 101.937,            update: -0.00117081  0.00196749 -0.00081055             estimated params: 0.890908,2.1719,0.943628
total cost: 101.937,            update:   3.4312e-06 -4.28555e-06  1.08348e-06          estimated params: 0.890912,2.1719,0.943629
total cost: 101.937,            update: -2.01204e-08  2.68928e-08 -7.86602e-09          estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.00440302 seconds. 
estimated abc = 0.890912, 2.1719, 0.943629

和真實結果對比，這裡的準確度取決於我們噪聲方差的大小

	$a$	$b$	$c$
Estimate	$0.890912$	$2.1719$	$0.943629$
Real	$1$	$2$	$1$

下一節我們來討論一下視覺SLAM中的非線性優化問題的具體形式，以及其 $H$ 和 $b$ 的由來和構建方法。

	\(a\)	\(b\)	\(c\)
Estimate	\(0.890912\)	\(2.1719\)	\(0.943629\)
Real	\(1\)	\(2\)	\(1\)

非線性優化問題基本形式概述