第六章 非線性優化
1. 理解最小二乘法的含義和處理方式。
2. 理解 Gauss-Newton, Levenburg-Marquadt 等下降策略。
3. 學習 Ceres 庫和 g2o 庫的基本使用方法。
因為我們的運動方程和觀測方程，受各種噪聲影響，所以要討論如何進行准確的狀態估計。
6.1 狀態估計問題
運動方程和觀測方程，Xk是位姿，w，v是噪聲。
位姿變量 xk 可以由 Tk 或 exp(ξk^) 表達，二者是等價的
由於運動方程在視覺 SLAM 中沒有特殊性，我們暫且不討論它，主要討論觀測方程。假設在 xk 處對路標 yj 進行了一次
觀測，對應到圖像上的像素位置 zk;j，那么，觀測方程可以表示成
K 為相機內參， s 為像素點的距離
假設兩個噪聲項 wk; vk;j 滿足零均值的高斯分布：
這些噪聲的影響下，我們希望通過帶噪聲的數據 z 和 u，推斷位姿 x 和地圖 y（以及它們的概率分布），這構成了一個狀態
估計問題 。歷史上很多人使用擴展濾波器（EKF）求解 ，該方法關心當前時刻的狀態估計，近年來普遍使用的非線性優化方法，使用所有時刻采集到的數據進行
狀態估計
所有待估計的變量
對機器人狀態的估計，就是求已知輸入數據 u 和觀測數據 z 的條件下，計算狀態 x 的條件概率分布
后驗概率
似然
直接求后驗分布是困難的，但是求一個狀態最優估計，使得在該狀態下，后驗概
率最大化（Maximize a Posterior， MAP），
， 
x 的最大似然估計（Maximize Likelihood Estimation, MLE）
最大似然估計，可以理解成：“在什么樣的狀態下，最可能產生現在觀測到的數據”
最小二乘的引出。
某次觀測;
觀測數據的條件概率 :
噪聲項 vk ∼ N (0; Qk;j)
略去第一項，只要最小化右側的二次型項，就得到了對狀態的最大似然估計。
代入 SLAM 的觀測模型 即求
得到了一個總體意義下的最小二乘問題（Least Square Problem）。 我們明白它的最優解等價於狀態的最大似然估計 。直觀來講，由於噪聲的存在，當我們把估計的軌跡與地圖代入 SLAM 的運動、觀測方程中時，它們並不會完美的成立。這時候怎么辦呢？我們把狀態的估計值進行微調，使得整體的誤差下降一些。當然這個下降也有限度，它一般會到達一個極小值。這就是一個典型非線性優化的過程
SLAM 中的最小二乘問題具有一些特定的結構：
1.整個問題的目標函數由許多個誤差的（加權的）平方和組成 ，且誤差項簡單
2.整體誤差由很多小型誤差項之和組成的問題，其增量方程的求解會具有一定的稀疏
性（會在第十講詳細講解），使得它們在大規模時亦可求解
3.其次，如果使用李代數表示，則該問題是無約束的最小二乘問題
4.我們使用了平方形式（二范數）度量誤差 ，直觀的
6.2 非線性最小二乘
，f 是任意一個非線性函數 。如果 f 是個數學形式上很簡單的函數，那問題也許可以用解析形式來求。令目標函數
的導數為零，然后求解 x 的最優值，就和一個求二元函數的極值一樣： 
就得到了導數為零處的極值。它們可能是極大、極小或鞍點處的值，只要挨個兒比較它們的函數值大小即可。
對於不方便直接求解的最小二乘問題，我們可以用迭代的方式，從一個初始值出發，不斷地更新當前的優化變量，使目標函數下降。 步驟如下
1. 給定某個初始值 x0。
2. 對於第 k 次迭代，尋找一個增量 ∆xk，使得 ∥f (xk + ∆xk)∥2 2 達到極小值。
3. 若 ∆xk 足夠小，則停止。
4. 否則，令 xk+1 = xk + ∆xk，返回 2.
直到某個時刻增量非常小，無法再使函數下降。此時算法收斂，目標達到了一個極小，我們完成了尋找極小值的過程。
6.2.1一階和二階梯度法、
求解增量最直觀的方式是將目標函數在 x 附近進行泰勒展開：
J 是 ∥f(x)∥2 關於 x 的導數（雅可比矩陣）， H 則是二階導數
保留泰勒展開的一階或二階項，對應的求解方法則為一階梯度或二階梯度法。
保留一階梯度，那么增量的方向為
直觀意義非常簡單，只要我們沿着反向梯度方向前進即可
還需要該方向上取一個步長 λ，求得最快的下降方式。這種方法被稱為最速下降法
保留二階梯度信息，那么增量方程為 
右側等式關於 ∆x 的導數並令它為零， 得到了增量的解： 
。該方法也稱為牛頓法。
最速下降法過於貪心，容易走出鋸齒路線，反而增加了迭代次數。而牛頓法則需要計算目標函數的 H 矩陣，這在問題規模較大時非常困難，我們通常傾向於避免 H 的計算。所以，接下來我們詳細地介紹兩類更加實用的方
法：高斯牛頓法和列文伯格——馬誇爾特方法。
6.2.2 高斯-牛頓法
Gauss Newton 是最優化算法里面最簡單的方法之一。它的思想是將 f(x) 進行一階的泰勒展開 請注意不是目標函數 f(x)2
J(x) 為 f(x) 關於 x 的導數
當前的目標是為了尋找下降矢量 ∆x，使得 ∥f (x + ∆x)∥2 達到最小。為了求 ∆x，我們需要解一個線性的最小二乘問題
根據極值條件，將上述目標函數對 ∆x 求導，並令導數為零。由於這里考慮的是 ∆x 的導數（而不是 x），我們最后將得到一個線性的方程。
為此，先展開目標函數的平方項
這是一個線性方程組，我們稱它為增量方程，
左邊定義為H，右邊定義為g
。高斯牛頓法將JTJ近似為牛頓法二階H矩陣。
Gauss-Newton 的算法步驟可以寫成：
1. 給定初始值 x0。
2. 對於第 k 次迭代，求出當前的雅可比矩陣 J(xk) 和誤差 f(xk)。
3. 求解增量方程： H∆xk = g:
4. 若 ∆xk 足夠小，則停止。否則，令 xk+1 = xk + ∆xk，返回 2.
缺點：JTJ可能為奇異矩陣或者病態，算法不收斂
6.2.3 列文伯格 -馬誇爾特
給 ∆x 添加一個信賴區域（Trust Region），不能讓它太大而使得近似不准確。
根據我們的近似模型跟實際函數之間的差異來確定這個范圍：如果差異小，我們就讓范圍盡可能大；如果差異大，我們就縮小這個近似范圍。因此，考慮使用
ρ 的分子是實際函數下降的值，分母是近似模型下降的值
算法步驟：
1. 給定初始值 x0，以及初始優化半徑 µ。
2. 對於第 k 次迭代，求解：
這里 µ 是信賴區域的半徑， D 將在后文說明。
3. 計算 ρ。
4. 若 ρ > 3 4，則 µ = 2µ；
5. 若 ρ < 1 4，則 µ = 0:5µ；
6. 如果 ρ 大於某閾值，認為近似可行。令 xk+1 = xk + ∆xk。
7. 判斷算法是否收斂。如不收斂則返回 2，否則結束。
D 取成單位陣 I，相當於直接把 ∆x 約束在一個球中。
=ζ（∆XK,λ）
當參數 λ 比較小時， H 占主要地位，這說明二次近似模型在該范圍內是比較好的， L-M 方法更接近於 G-N 法。另一方面，當 λ 比較大時， λI 占據主要地位， L-M更接近於一階梯度下降法（即最速下降），這說明附近的二次近似不夠好。