SLAM非線性優化

經過前面的學習，我們的問題變爲如何在有噪聲的數據中進行準確的狀態估計。

狀態估計問題

經典SLAM模型，由一個運動方程和一個觀測方程構成：

\[\begin{cases} x_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j} \end{cases} \]

這裏的\(x_k\)就是相機的位姿，可以由\(SE(3)\)（特殊歐氏羣，歐氏變換組成的）來描述。觀測方程由針孔模型給定。

假如在\(x_k\)處對路標\(y_j\)進行了一次觀測，對應到圖像上的像素位置爲\(z_{k,j}\)，那麼，觀測方程可以表示成：

\[sz_{k,j}=K(R_ky_j+t_k) \]

在運動和觀測方程，我們通常假設兩個噪聲項\(w_k,v_{k,j}\)滿足零均值的高斯分佈：

\[w_k\sim \Nu(0,R_k),v_k\sim \Nu(0,Q_{k,j}) \]

其中\(\Nu\)表示高斯分佈，0表示零均值，\(R_k\),\(Q_{k,j}\)爲協方差矩陣。

在這些噪聲的影響下，我們希望通過帶噪聲的數據z和u推斷位姿和地圖y以及他們的概率分佈（u是啥？)，這就是一個狀態估計問題。
目前解決狀態估計問題的思路：

• 增量/漸進的方式：濾波器
• 批量的方法

批量狀態估計和最大後驗估計

這裏我們討論批量方法，考慮1到N的所有時刻，並假設M個路標點。定義所有時刻的機器人位姿和路標點座標爲

\[x=\{x_1,...,x_N\}, y=\{y_1,...,y_M\} \]

u表示所有時刻的輸入，z表示所有時刻的觀測數據。則就是求：

\[P(x,y|z,u) \]

特別的，當我們不知道控制輸入，只有一張張圖片時，即只考慮觀測方程帶來的數據時，可以轉化爲：

\[P(x,y|z) \]

此問題也稱爲SfM問題，即如何從許多圖像中重建三維空間結構。
利用貝葉斯法則：

\[P(x,y|z,u)=\frac {P(z,u|x,y)P(x,y)}{P(z,u)}\propto P(z,u|x,y)P(x,y) \]

\(P(x,y|z,u)\) 稱作後驗概率，\(P(z|x)\)稱爲似然，\(P(x)\)稱爲先驗。
直接求後驗是困難的，但是求一個狀態的最優估計，使得在該狀態下後驗概率最大是可行的：

\[(x,y)^*_{MAP}=argmaxP(x,y|z,u)=argmax P(z,u|x,y)P(x,y) \]

\(P(x,y)\)與待估計的部分無關，所以可以忽略。
貝葉斯告訴我們最大後驗概率等價於最大化似然和先驗的乘積。但是我們也可以忽略機器人位姿或路標位置，此時就沒有了先驗。那麼可以求解最大似然(Maximize Likelihood Estimation,MLE)

\[(x,y)^*_{MLE} = argmax P(z,u|x,y) \]

最大似然可以理解爲，“在什麼狀態下，最可能產生現在觀測到的數據”

非線性最小二乘

\[min_xF(X)=\frac {1}{2}\lvert f(x) \rvert^2 \]

f(x)爲非線性函數，有些導函數可能形式複雜，我們可以使用迭代的方式來接近局部極值
1.給定初始值x_0
2.對第k次迭代，尋找增量\(\triangle x_k\)，使得\(\lvert f(x_k+\triangle x_k) \rvert^2\)達到極小值
3.若\(\delta x_k\) 足夠小，則停止
4.否則，令\(x_{k+1}=x_k+\triangle x_k\)返回第2步

此時，問題從求解導函數爲0轉變到不斷尋找下降增量\(\delta x_k\) 的問題。

一階和二階梯度法

泰勒展開：

\[F(x_k+\triangle x_k)\approx F(x_k)+J(x_k)^T \triangle x_k+\frac{1}{2}\triangle x_k^TH(x_k)\triangle x_k \]

其中\(J(x_k)\)是\(F(x)\)關於x的一階導數（也叫梯度、雅可比矩陣）；H則是二階導數（海塞矩陣）
一階梯度：
方向：\(\triangle x^*=-J(x_k)\)
再指定步長，完成下降。

二階梯度/牛頓法：
\(\triangle x^*=argmin(F(x)+J(x)^T \triangle x+\frac{1}{2}\triangle x^TH(x)\triangle x)\)
右側對\(\triangle x\)求導，並令爲0：

\[J+H\triangle x = 0 \]

則，$$H\triangle x=-J$$

高斯牛頓法

它的思想是將\(f(x)\)進行一階泰勒展開而不是\(F(x)\):

\[f(x_k+\triangle x_k)\approx f(x_k)+J(x_k)^T \triangle x_k \]

求解問題變爲：

\[\triangle x^*=argmin _{\triangle x}\frac {1}{2}\lvert f(x)+J(x)^T\triangle x\rvert ^2 \]

\[\frac {1}{2}\lvert {f(x)+J(x)^T\triangle x}\rvert ^2=\frac {1}{2}(\lvert f(x) \rvert ^2+2f(x)J(x)^T\triangle x+\triangle x^TJ(x)J(x)^T\triangle x \]

右側求導，$$J(x)f(x)+J(x)J^T(x)\triangle x=0$$
則，可得 高斯-牛頓方程：

\[J(x)J^T(x)\triangle x=-J(x)f(x) \]

左邊的定義爲\(H\)右邊的定義爲\(g\)：
\(H\triangle x=g\)
對比牛頓法，其實高斯-牛頓法運用\(JJ^T\)作爲牛頓法中二階Hessian矩陣的近似，從而省略了H的估計。
算法流程：
1.給定初始值\(x_0\)
2.對於第k次迭代，求出當前的雅可比矩陣\(J(x_k)\)和誤差\(f(x_k)\)
3.求解增量方程：\(H\triangle x_k =g\)
4.若\triangle x_k足夠小則停止。否則，令\(x_{k+1}=x_k+\triangle x_k\)，返回第2步

列文伯格-馬夸爾特方法

高斯牛頓法中採用的近似二階泰勒展開只能在展開點附近有較好的近似效果。我們可以增加信賴區間，在區間範圍內可以認爲二階近似是有效的。

\[\rho=\frac{f(x+\triangle x)-f(x)}{J(x)^T\triangle x} \]

算法流程：
1.給定初始值x_0，以及初始優化半徑\(\mu\);
2.對於第k次迭代，在高斯牛頓的基礎上加上信賴區域：

\[min\frac{1}{2}\lvert f(x)+J(x_k)^T\triangle x \rvert ^2，s.t. \lvert D \triangle x_k \rvert ^2 \leq \mu \]

3.計算\(\rho\)
4.若\(\rho \ge \frac {3}{4}\)，則設置\(\mu = 2\mu\)
5.若\(\rho \le \frac {1}{4}\)，則設置\(\mu = 0.5\mu\)
6.如果\rho大於某閾值，則認爲近似可行。令x_k+1=x_k+\triangle x_k;
7.判斷算法是否收斂。如不收斂返回第2步，否則結束。

D、$\mu $求解：

\[lagelangri(\triangle x,\lambda)=\frac{1}{2}\lvert f(x)+J(x_k)^T\triangle x \rvert ^2+\frac{\lambda}{2}(\lvert D\triangle x_k \rvert ^2 - \mu) \]

右側等於0 可得：

\[(H+\lambda D^TD)\triangle x_k=g \]

簡化爲

\[(H+\lambda I)\triangle x_k=g \]

可以理解爲，利用參數\(\lambda\)控制兩種方法的比例。
$\lambda $ 較小時，\(H\) 佔主要位置。列文伯格-馬夸爾特方法更接近於高斯牛頓法。
$\lambda $ 較大時，\(\lambda I\) 佔主要位置，列文伯格-馬夸爾特方法更接近於梯度下降法。