SLAM代碼（優化及常用庫）

在SLAM的後端優化中，比較常用的一種方法是g2o，該圖優化方法在優化時現成可以選用的有２種優化方法。一是Ｇauss-Newton，另外是LM方法。這裏首先介紹Gauss-Newton方法，其次介紹g2o使用時的一般流程和例程。

Gauss-Newton

Gauss-Newton算法是解決非線性最優問題的常見算法之一.

基本概念定義

非線性方程定義及最優化方法簡述

指因變量與自變量之間的關係不是線性的關係，比如平方關係、對數關係、指數關係、三角函數關係等等。對於此類方程，求解n元實函數f在整個n維向量空間Rn上的最優值點往往很難得到精確解，經常需要求近似解問題。
求解該最優化問題的方法大多是逐次一維搜索的迭代算法，基本思想是在一個近似點處選定一個有利於搜索方向，沿這個方向進行一維搜索，得到新的近似點。如此反覆迭代，知道滿足預定的精度要求爲止。根據搜索方向的取法不同，這類迭代算法可分爲兩類：
- 解析法：需要用目標函數的到函數，
- 梯度法：又稱最速下降法，是早期的解析法，收斂速度較慢
- 牛頓法：收斂速度快，但不穩定，計算也較困難。高斯牛頓法基於其改進，但目標作用不同
- 共軛梯度法：收斂較快，效果好
- 變尺度法：效率較高，常用DFP法(Davidon Fletcher Powell)
- 直接法：不涉及導數，只用到函數值。有交替方向法(又稱座標輪換法)、模式搜索法、旋轉方向法、鮑威爾共軛方向法和單純形加速法等。

非線性最小二乘問題

非線性最小二乘問題來自於非線性迴歸，即通過觀察自變量和因變量數據，求非線性目標函數的係數參數，使得函數模型與觀測量儘量相似。高斯牛頓法解決非線性最小二乘問題的最基本方法，並且它只能處理二次函數。

Unlike Newton’smethod, the Gauss–Newton algorithm can only be used to minimize a sum ofsquared function values

基本數學表達

• 梯度gradient，由多元函數的各個偏導數組成的向量
  以二元函數爲例，其梯度爲：
  
  ∇f(x1,x2)=(∂f∂x1,∂f∂x2)
• Hessian matrix 由多元函數的二階偏導數組成的方陣，描述函數的局部曲率，以二元函數爲例，
  
  H(f(x1,x2))=⎡⎣⎢⎢⎢⎢⎢∂2f∂x21∂2f∂x1∂x2∂2f∂x1∂x2∂2f∂x22⎤⎦⎥⎥⎥⎥⎥
• 雅可比矩陣 Jacobian matrix，是多元函數一階偏導數以一定方式排列成的矩陣，體現了一個可微方程與給出點的最優線性逼近。以二元函數爲例，
  
  J(f(x1,x2))=⎡⎣⎢⎢⎢⎢⎢⎢∂y1∂x1...∂ym∂x1........∂y1∂xn...∂ym∂xn⎤⎦⎥⎥⎥⎥⎥⎥

雅可比矩陣作用，如果P 是Rn 中的一點，F 在P 點可微分，那麼在這一點的導數由JF(P) 給出，在此情況下，由F(P) 描述的線性算子即接近點P的F的最優線性逼近：

F(x)≈F(P)+JF(P)(X−P)

newton method

若非線性目標函數f(x) 具有二階連續偏導，在xk 爲其極小點的某一近似，在這一點取f(x) 的二階泰勒展開，即：

ϕ(x)≈f(xk)+f(xk)TΔx+12ΔxTH(xk)Δx

f(x) 的梯度爲

∇f(x)≈∇f(xk)+H(xk)Δx

在極小值點滿足

∇f(x)≈∇f(xk)+H(xk)Δx=0

so we have

Δx=x−xk=−H(xk)∇f(xk)

如果f(x) 是二次函數，則其黑森矩陣H 爲常數，在這種情況下，從任意一點出發，只要一步可求出f(x) 的極小點(假設黑森矩陣正定，所有特徵值大於0)
如果f(x) 不是二次函數，梯度表達僅是一個近似表達式，此時，按賦值０求得的極小點，只是f(x) 的近似極小點。在這種情況下，常按照下面選取搜索方向：

pk=−H(xk)∇f(xk)

Δxk+1=xk−λkpk

λk:=minf(xk+λkpk)

牛頓法收斂的速度很快，當f(x) 的二階導數及其黑森矩陣的逆矩陣便於計算時，這一方法非常有效。【但通常黑森矩陣很不好求】

Gauss-Newton

gauss-newton是如何由上述派生的

有時候爲了擬合數據，比如根據重投影誤差求相機位姿(R ,t 爲方程係數)，常常將求解模型轉化爲非線性最小二乘問題。高斯牛頓法正是用於解決非線性最小二乘問題，達到數據擬合、參數估計和函數估計的目的。
假設我們研究如下形式的非線性最小二乘問題：

minx∈R∗f(x)=rT(x)r(x)

r(x) 爲ｘ的非線性函數，比如對於重投影誤差。
位置殘差爲

rij=zij−z^(Ti,yj)

如果有大量觀測點(多維)，我們可以通過選擇合理的T使得殘差的平方和最小求得兩個相機之間的位姿。機器視覺這塊暫時不擴展，接着說怎麼求非線性最小二乘問題。

若用牛頓法求式3，則牛頓迭代公式爲：

Δxk+1=xk−H−1∇f

here

∇f=2∑i=1mri∂ri∂xj

Hjk=2∑i=1m(∂ri∂xj∂ri∂xk+ri∂2ri∂xj∂xk)

Hjk=2∑i=1mJijJjk

用矩陣表示爲

r=(r1,...,rm)

Δxk+1=xk−(JTJ)−1JTr

here

Jij=∂ri(xk)∂xjk

看到這裏大家都明白高斯牛頓和牛頓法的差異了吧，就在這迭代項上。經典高斯牛頓算法迭代步長λ爲1.

那回過頭來，高斯牛頓法裏爲啥要捨棄黑森矩陣的二階偏導數呢？主要問題是因爲牛頓法中Hessian矩陣中的二階信息項通常難以計算或者花費的工作量很大，而利用整個H 的割線近似也不可取，因爲在計算梯度 時已經得到J(x) ，這樣H中的一階信息項JTJ 幾乎是現成的。鑑於此，爲了簡化計算，獲得有效算法，我們可用一階導數信息逼近二階信息項。 注意這麼幹的前提是，殘差r 接近於零或者接近線性函數從而接近與零時，二階信息項纔可以忽略。通常稱爲“小殘量問題”，否則高斯牛頓法不收斂.

refer

1. http://blog.csdn.net/dsbatigol/article/details/12448627
2. http://www.voidcn.com/blog/jinshengtao/article/p-6004352.html

g2o

g2o使用舉例
主要步驟
1. SparseOptimizer 是我們最終要維護的東東。它是一個Optimizable Graph，從而也是一個Hyper Graph。一個 SparseOptimizer 含有很多個頂點 （都繼承自 Base Vertex）和很多個邊（繼承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。這些 Base Vertex 和 Base Edge 都是抽象的基類，而實際用的頂點和邊，都是它們的派生類。我們用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一個圖中添加頂點和邊，最後調用 SparseOptimizer.optimize 完成優化。
2. 在優化之前，需要指定我們用的求解器和迭代算法。從圖中下半部分可以看到，一個 SparseOptimizer 擁有一個 Optimization Algorithm，繼承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 三者之一（我們常用的是GN或LM）。同時，這個 Optimization Algorithm 擁有一個Solver，它含有兩個部分。一個是 SparseBlockMatrix ，用於計算稀疏的雅可比和海塞； 一個是用於計算迭代過程中最關鍵的一步
HΔx=−b
HΔx=−b
這就需要一個線性方程的求解器。而這個求解器，可以從 PCG, CSparse, Choldmod 三者選一。

在g2o中選擇優化方法

一共需要三個步驟：
- 選擇一個線性方程求解器，從 PCG, CSparse, Choldmod中選，實際則來自 g2o/solvers 文件夾中定義的東東。
- 選擇一個 BlockSolver 。
- 選擇一個迭代策略，從GN, LM, Doglog中選。
blog
1. http://blog.csdn.net/heyijia0327/article/details/47813405
2. http://www.cnblogs.com/gaoxiang12/p/5304272.html
3. http://www.cnblogs.com/gaoxiang12/p/3776107.html

alse their code and data is available in the github
1. https://github.com/gaoxiang12/g2o_ba_example
2. https://github.com/HeYijia/GraphSLAM_tutorials_code/tree/master/g2o_test