SLAM圖優化g2o
圖優化g2o框架
圖優化的英文是 graph optimization 或者 graph-based optimization， “圖”其實是數據結構中的graph。凸優化的英文是 convex optimization，這里的“凸”其實是凸函數的意思，所以單從英文就能區分開。
圖優化有什么優勢？
SLAM的后端一般分為兩種處理方法，一種是以擴展卡爾曼濾波（EKF）為代表的濾波方法，一種是以圖優化為代表的非線性優化方法。SLAM研究的主流熱點幾乎都是基于圖優化。
濾波方法尤其是EKF方法，在SLAM發展很長的一段歷史中一直占據主導地位，早期的大神們研究了各種各樣的濾波器來改善濾波效果，SLAM，EKF是必須要掌握的。濾波方法的優缺點：
優點：在計算資源受限、待估計量比較簡單的情況下，EKF為代表的濾波方法比較有效，經常用在激光SLAM中。
缺點：存儲量和狀態量是平方增長關系，存儲的是協方差矩陣，不適合大型場景。基于視覺的SLAM方案，路標點（特征點）數據很大，濾波方法根本吃不消，所以此時濾波的方法效率非常低。
圖優化在視覺SLAM中效率很高嗎？
以前都是用濾波方法，因為在圖優化里，Bundle Adjustment（后面簡稱BA）起到了核心作用。大量特征點和相機位姿的BA計算量其實很大，根本沒辦法實時。
在視覺SLAM中，雖然包含大量特征點和相機位姿，其實BA是稀疏的，稀疏的就好辦了，就可以加速了啊！比較代表性的就是2009年，幾個大神發表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且計算機硬件發展也很快，因此基于圖優化的視覺SLAM也可以實時了！
圖優化是什么？
圖優化里的圖就是數據結構里的圖，一個圖由若干個頂點（vertex），以及連接這些頂點的邊（edge）組成。
比如一個機器人在房屋里移動，在某個時刻 t 的位姿（pose）就是一個頂點，這個也是待優化的變量。而位姿之間的關系就構成了一個邊，比如時刻 t 和時刻 t+1 之間的相對位姿變換矩陣就是邊，邊通常表示誤差項。
在SLAM里，圖優化一般分解為兩個任務：
1、構建圖。機器人位姿作為頂點，位姿間關系作為邊。
2、優化圖。調整機器人的位姿（頂點）來盡量滿足邊的約束，使得誤差最小。
根據機器人位姿來作為圖的頂點，這個位姿可以來自機器人的編碼器，也可以是ICP匹配得到的，圖的邊就是位姿之間的關系。由于誤差的存在，實際上機器人建立的地圖是不準的，如下圖左。通過設置邊的約束，使得圖優化向著滿足邊約束的方向優化，最后得到了一個優化后的地圖（如下圖中所示），與真正的地圖（下圖右）非常接近。

g2o 框架
圖優化如何編程實現呢？
在SLAM領域，基于圖優化的一個用的非常廣泛的庫就是g2o，是General Graphic Optimization 的簡稱，是一個用來優化非線性誤差函數的c++框架。
這個g2o怎么用呢？
先安裝，其實g2o安裝很簡單，參考GitHub上官網：
https://github.com/RainerKuemmerle/g2o
按照步驟來安裝就行了。需要注意的是安裝之前確保電腦上已經安裝好了第三方依賴。
第一次接觸g2o，確實有這種感覺，而且官網文檔寫的也比較“不通俗不易懂”，不過如果理順了框架，再去看代碼，應該很快能夠入手了。
g2o實現了很多內部的算法，在進行構造的時候，需要遵循一些規則，畢竟一個程序不可能滿足所有的要求，g2o的使用中還是應該多看多記，這樣才能更好的使用這個庫。
首先看一下下面這個圖，是g2o的基本框架結構。看圖的時候要注意箭頭類型。

1、圖的核心
要知道這個圖中哪個最重要，就去看看箭頭源頭在哪里
最左側的SparseOptimizer？SparseOptimizer是整個圖的核心，右上角的 is-a 實心箭頭，這個SparseOptimizer是一個Optimizable Graph，從而也是一個超圖（HyperGraph）。
2、頂點和邊
先來看上面的結構。has-many 箭頭，這個超圖包含了許多頂點（HyperGraph::Vertex）和邊（HyperGraph::Edge）。而這些頂點繼承自 Base Vertex，也就是OptimizableGraph::Vertex，而邊可以繼承自 BaseUnaryEdge（單邊）, BaseBinaryEdge（雙邊）或BaseMultiEdge（多邊），都稱為OptimizableGraph::Edge
頂點和邊在編程中很重要的，看底部的結構。
3、配置SparseOptimizer的優化算法和求解器
整個圖的核心SparseOptimizer 包含一個優化算法（OptimizationAlgorithm）的對象。OptimizationAlgorithm是通過OptimizationWithHessian 來實現的。其中迭代策略可以從Gauss-Newton（高斯牛頓法，簡稱GN）, Levernberg-Marquardt（簡稱LM法）, Powell’s dogleg 三者中間選擇一個（常用的是GN和LM）
GN和LM就是非線性優化方法中常用的兩種。
4、如何求解
OptimizationWithHessian 內部包含一個求解器（Solver），這個Solver實際是由一個BlockSolver組成的。這個BlockSolver有兩個部分，一個是SparseBlockMatrix ，用于計算稀疏的雅可比和Hessian矩陣；一個是線性方程的求解器（LinearSolver），用于計算迭代過程中最關鍵的一步HΔx=?b，LinearSolver有幾種方法可以選擇：PCG, CSparse, Choldmod，上面圖的一個簡單理解。
g2o編程流程
從底層開始搭建框架一直到頂層。g2o的整個框架就是按照下圖中標的這個順序來寫的。

SLAM十四講中g2o求解曲線參數的例子來說明，源代碼地址
https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp
為了方便理解，重新加了注釋。如下所示，
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每個誤差項優化變量維度為3，誤差值維度為1

// 第1步：創建一個線性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDenseBlock::PoseMatrixType();
// 第2步：創建BlockSolver。并用上面定義的線性求解器初始化
Block* solver_ptr = new Block( linearSolver );
// 第3步：創建總求解器solver。并從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步：創建終極大boss 稀疏優化器（SparseOptimizer）
g2o::SparseOptimizer optimizer; // 圖模型
optimizer.setAlgorithm( solver ); // 設置求解器
optimizer.setVerbose( true ); // 打開調試輸出
// 第5步：定義圖的頂點和邊。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往圖中增加頂點
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ ) // 往圖中增加邊
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 設置連接的頂點
edge->setMeasurement( y_data[i] ); // 觀測數值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()1/(w_sigmaw_sigma) ); // 信息矩陣：協方差矩陣之逆
optimizer.addEdge( edge );
}
// 第6步：設置優化參數，開始執行優化
optimizer.initializeOptimization();
optimizer.optimize(100);
結合上面的流程圖和代碼。下面一步步解釋具體步驟。
1、創建一個線性求解器LinearSolver
增量方程的形式是：H△X=-b，通常的方法就是直接求逆，也就是△X=-H.inv*b。看起來好像很簡單，但這有個前提，就是H的維度較小，此時只需要矩陣的求逆就能解決問題。但是當H的維度較大時，矩陣求逆變得很困難，求解問題也變得很復雜。
需要一些特殊的方法對矩陣進行求逆，看下圖是GitHub上g2o相關部分的代碼

可以分別查看每個方法的解釋。
LinearSolverCholmod ：使用sparse cholesky分解法。繼承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。繼承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，繼承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。繼承自LinearSolver
LinearSolverEigen： 依賴項只有eigen，使用eigen中sparse Cholesky 求解，因此編譯好后可以方便的在其他地方使用，性能和CSparse差不多。繼承自LinearSolver
2、創建BlockSolver。并用上面定義的線性求解器初始化。
BlockSolver 內部包含 LinearSolver，用上面我們定義的線性求解器LinearSolver來初始化。它的定義在如下文件夾內：
g2o/g2o/core/block_solver.h
BlockSolver有兩種定義方式
一種是指定的固定變量的solver，定義
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
其中p代表pose的維度（注意一定是流形manifold下的最小表示），l表示landmark的維度
另一種是可變尺寸的solver，定義如下
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
為何會有可變尺寸的solver呢？
這是因為在某些應用場景， Pose和Landmark在程序開始時并不能確定，此時這個塊狀求解器就沒辦法固定變量，此時使用這個可變尺寸的solver，所有的參數都在中間過程中被確定
看block_solver.h的最后，預定義了比較常用的幾種類型，如下所示：
BlockSolver_6_3 ：表示pose 是6維，觀測點是3維。用于3D SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基礎上多了一個scale
BlockSolver_3_2：表示pose 是3維，觀測點是2維
以后遇到了知道這些數字是什么意思就行了
3、創建總求解器solver。并從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化
g2o/g2o/core/ 目錄下， Solver的優化方法有三種：分別是高斯牛頓（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下圖所示，也和前面的圖相匹配

上圖最后那個OptimizationAlgorithmWithHessian 是干嘛的？
GN、 LM、 Doglet算法內部，都繼承自同一個類：OptimizationWithHessian，如下圖所示，這也和最前面那個圖是相符。

看 OptimizationAlgorithmWithHessian，繼承自OptimizationAlgorithm，這也和前面的相符。

總之，在該階段，可以選三種方法：
g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg
4、創建終極大boss 稀疏優化器（SparseOptimizer），用已定義求解器作為求解方法。
創建稀疏優化器
g2o::SparseOptimizer optimizer;
用前面定義好的求解器作為求解方法：
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
其中setVerbose是設置優化過程輸出信息用的
SparseOptimizer::setVerbose(bool verbose)
看一下定義

5、定義圖的頂點和邊。并添加到SparseOptimizer中。
這部分比較復雜，不展開介紹。
6、設置優化參數，開始執行優化。
設置SparseOptimizer的初始化、迭代次數、保存結果等。
初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
設置迭代次數，然后就開始執行圖優化了。
SparseOptimizer::optimize(int iterations, bool online)

總結

以上是生活随笔為你收集整理的SLAM图优化g2o的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。