從零開始一塊兒學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼

首發於公衆號：計算機視覺life 旗下知識星球「從零開始學習SLAM」

這多是最清晰講解g2o代碼框架的文章

理解圖優化，一步步帶你看懂g2o框架

小白：師兄師兄，最近我在看SLAM的優化算法，有種方法叫「圖優化」，之前學習算法的時候還有一個優化方法叫「凸優化」，這兩個不是一個東西吧？

師兄：哈哈，這個問題有意思，雖然它們中文發音同樣，可是意思差異大着呢！咱們來看看英文表達吧，圖優化的英文是 graph optimization 或者 graph-based optimization，你看，它的「圖」實際上是數據結構中的graph。而凸優化的英文是 convex optimization，這裏的「凸」實際上是凸函數的意思，因此單從英文就能區分開它們。

小白：原來是這樣，我看SLAM中圖優化用的不少啊，我看了一下高博的書，仍是迷迷糊糊的，求科普啊師兄

師兄：圖優化真的蠻重要的，概念其實不負責，主要是編程稍微有點複雜。。

小白：不能贊成更多。。，那個代碼看的我一臉懵逼

圖優化有什麼優點？

師兄：按照慣例，我仍是先說說圖優化的背景吧。SLAM的後端通常分爲兩種處理方法，一種是以擴展卡爾曼濾波（EKF）爲表明的濾波方法，一種是以圖優化爲表明的非線性優化方法。不過，目前SLAM研究的主流熱點幾乎都是基於圖優化的。

小白：據我所知，濾波方法很早就有了，前人的研究也很深。爲何如今圖優化變成了主流了？

師兄：你說的沒錯。濾波方法尤爲是EKF方法，在SLAM發展很長的一段歷史中一直佔據主導地位，早期的大神們研究了各類各樣的濾波器來改善濾波效果，那會入門SLAM，EKF是必需要掌握的。順便總結下濾波方法的優缺點：

優勢：在當時計算資源受限、待估計量比較簡單的狀況下，EKF爲表明的濾波方法比較有效，常常用在激光SLAM中。

缺點：它的一個大缺點就是存儲量和狀態量是平方增加關係，由於存儲的是協方差矩陣，所以不適合大型場景。而如今基於視覺的SLAM方案，路標點（特徵點）數據很大，濾波方法根本吃不消，因此此時濾波的方法效率很是低。

小白：原來如此。那圖優化在視覺SLAM中效率很高嗎？

師兄：這個其實說來話長了。好久好久之前，其實就是不到十年前吧(感受好像好久)，你們還都是用濾波方法，由於在圖優化裏，Bundle Adjustment（後面簡稱BA）起到了核心做用。可是那會SLAM的研究者們發現包含大量特徵點和相機位姿的BA計算量其實很大，根本沒辦法實時。

小白：啊？後來發生了什麼？（認真聽故事ing）

師兄：後來SLAM研究者們發現了其實在視覺SLAM中，雖然包含大量特徵點和相機位姿，但其實BA是稀疏的，稀疏的就好辦了，就能夠加速了啊！比較表明性的就是2009年，幾個大神發表了本身的研究成果《SBA：A software package for generic sparse bundle adjustment》，並且計算機硬件發展也很快，所以基於圖優化的視覺SLAM也能夠實時了！

小白：厲害厲害！向大牛們致敬！

圖優化是什麼？

小白：圖優化既然是主流，那我能夠跳過濾波方法直接學習圖優化吧，反正濾波方法也看不懂。。

師兄：額，圖優化確實是主流，之後有須要你能夠再去看濾波方法，那咱們今天就只講圖優化好啦

小白：好滴，那問題來了，究竟什麼是圖優化啊？

師兄：圖優化裏的圖就是數據結構裏的圖，一個圖由若干個頂點（vertex），以及鏈接這些頂點的邊（edge）組成，給你舉個例子

好比一個機器人在房屋裏移動，它在某個時刻 t 的位姿（pose）就是一個頂點，這個也是待優化的變量。而位姿之間的關係就構成了一個邊，好比時刻 t 和時刻 t+1 之間的相對位姿變換矩陣就是邊，邊一般表示偏差項。

在SLAM裏，圖優化通常分解爲兩個任務：

一、構建圖。機器人位姿做爲頂點，位姿間關係做爲邊。

二、優化圖。調整機器人的位姿（頂點）來儘可能知足邊的約束，使得偏差最小。

下面就是一個直觀的例子。咱們根據機器人位姿來做爲圖的頂點，這個位姿能夠來自機器人的編碼器，也能夠是ICP匹配獲得的，圖的邊就是位姿之間的關係。因爲偏差的存在，實際上機器人創建的地圖是不許的，以下圖左。咱們經過設置邊的約束，使得圖優化向着知足邊約束的方向優化，最後獲得了一個優化後的地圖（以下圖中所示），它和真正的地圖（下圖右）很是接近。

小白：哇塞，這個圖優化效果這麼明顯啊！剛開始偏差那麼大，最後都校訂過來了

師兄：是啊，因此圖優化在SLAM中舉足輕重啊，必定得掌握！

小白：好，有學習的動力了！咱們開啓編程模式吧！

先了解g2o 框架

師兄：前面咱們簡單介紹了圖優化，你也看到了它的神通廣大，那如何編程實現呢？

小白：對啊，有沒有現成的庫啊，我還只是個「調包俠」。。

師兄：這個必須有啊！在SLAM領域，基於圖優化的一個用的很是普遍的庫就是g2o，它是General Graphic Optimization 的簡稱，是一個用來優化非線性偏差函數的c++框架。這個庫能夠知足你調包俠的夢想~

小白：哈哈，太好了，不然打死我也寫不出來啊！那這個g2o怎麼用呢？

師兄：我先說安裝吧，其實g2o安裝很簡單，參考GitHub上官網：

https://github.com/RainerKuemmerle/g2o

按照步驟來安裝就好了。須要注意的是安裝以前確保電腦上已經安裝好了第三方依賴。

小白：好的，這個看起來很好裝。不過問題是，我看相關的代碼，感受很複雜啊，不知如何下手啊

師兄：別急，第一次接觸g2o，確實有這種感受，並且官網文檔寫的也比較「不通俗不易懂」，不過若是你能捋順了它的框架，再去看代碼，應該很快可以入手了

小白：是的，先對框架了然於胸才行，否則即便能湊合看懂別人代碼，本身也不會寫啊！

師兄：嗯嗯，其實g2o幫助咱們實現了不少內部的算法，只是在進行構造的時候，須要遵循一些規則，在我看來這是能夠接受的，畢竟一個程序不可能知足全部的要求，所以在之後g2o的使用中仍是應該多看多記，這樣才能更好的使用這個庫。

小白：記住了。養成記筆記的好習慣，還要多練習。

師兄：好，那咱們首先看一下下面這個圖，是g2o的基本框架結構。若是你查資料的話，你會在不少地方都能看到。看圖的時候要注意箭頭類型

一、圖的核心

小白：師兄，這個圖該從哪裏開始看？感受好多東西。。

師兄：若是你想要知道這個圖中哪一個最重要，就去看看箭頭源頭在哪裏

小白：我看看。。。好像是最左側的SparseOptimizer？

師兄：對的，SparseOptimizer是整個圖的核心，咱們注意右上角的 is-a 實心箭頭，這個SparseOptimizer它是一個Optimizable Graph，從而也是一個超圖（HyperGraph）。

小白：我去，師兄，怎麼忽然冒出來這麼多奇怪的術語，都啥意思啊？

師兄：這個你不須要一個個弄懂，否則可能黃花菜都涼了。你先暫時只須要了解一下它們的名字，有些之後用不到，有些之後用到了再回看。目前若是遇到重要的我會具體解釋。

小白：好。那下一步看哪裏？

二、頂點和邊

師兄：咱們先來看上面的結構吧。注意看 has-many 箭頭，你看這個超圖包含了許多頂點（HyperGraph::Vertex）和邊（HyperGraph::Edge）。而這些頂點頂點繼承自 Base Vertex，也就是OptimizableGraph::Vertex，而邊能夠繼承自 BaseUnaryEdge（單邊）, BaseBinaryEdge（雙邊）或BaseMultiEdge（多邊），它們都叫作OptimizableGraph::Edge

小白：頭有點暈了，師兄

師兄：哈哈，不用一個個記，現階段瞭解這些就行。頂點和邊在編程中很重要的，關於頂點和邊的定義咱們之後會詳細說的。下面咱們來看底部的結構。

小白：嗯嗯，知道啦！

三、配置SparseOptimizer的優化算法和求解器

師兄：你看下面，整個圖的核心SparseOptimizer 包含一個優化算法（OptimizationAlgorithm）的對象。OptimizationAlgorithm是經過OptimizationWithHessian 來實現的。其中迭代策略能夠從Gauss-Newton（高斯牛頓法，簡稱GN）, Levernberg-Marquardt（簡稱LM法）, Powell's dogleg 三者中間選擇一個（咱們經常使用的是GN和LM）

小白：GN和LM就是咱們之前講過的非線性優化方法中經常使用的兩種吧 師兄：是的，若是不瞭解的話具體看《從零開始學習「張氏相機標定法」（四）優化算法前傳》《從零開始學習「張氏相機標定法」（五）優化算法正傳》這兩篇文章。

四、如何求解

師兄：那麼如何求解呢？OptimizationWithHessian 內部包含一個求解器（Solver），這個Solver實際是由一個BlockSolver組成的。這個BlockSolver有兩個部分，一個是SparseBlockMatrix ，用於計算稀疏的雅可比和Hessian矩陣；一個是線性方程的求解器（LinearSolver），它用於計算迭代過程當中最關鍵的一步HΔx=−b，LinearSolver有幾種方法能夠選擇：PCG, CSparse, Choldmod，具體定義後面會介紹

到此，就是上面圖的一個簡單理解。

一步步帶你看懂g2o編程流程

小白：師兄，看完了我也不知道編程時具體怎麼編呢！

師兄：我正好要說這個。首先這裏須要說一下，咱們梳理是從頂層到底層，可是編程實現時須要反過來，像建房子同樣，從底層開始搭建框架一直到頂層。g2o的整個框架就是按照下圖中我標的這個順序來寫的。

高博在十四講中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::LinearSolverDense<Block::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_sigma*w_sigma) ); // 信息矩陣：協方差矩陣之逆
  optimizer.addEdge( edge );
}

// 第6步：設置優化參數，開始執行優化
optimizer.initializeOptimization();
optimizer.optimize(100);

結合上面的流程圖和代碼。下面一步步解釋具體步驟。

一、建立一個線性求解器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

二、建立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維

之後遇到了知道這些數字是什麼意思就好了

三、建立總求解器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

四、建立終極大boss 稀疏優化器（SparseOptimizer），並用已定義求解器做爲求解方法。

建立稀疏優化器

g2o::SparseOptimizer    optimizer;

用前面定義好的求解器做爲求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是設置優化過程輸出信息用的

SparseOptimizer::setVerbose(bool verbose)

不信咱們來看一下它的定義

五、定義圖的頂點和邊。並添加到SparseOptimizer中。

這部分比較複雜，咱們下一次再介紹。

六、設置優化參數，開始執行優化。

設置SparseOptimizer的初始化、迭代次數、保存結果等。

初始化

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

設置迭代次數，而後就開始執行圖優化了。

SparseOptimizer::optimize(int iterations, bool online)

小白：終於搞明白g2o流程了！謝謝師兄！必須給你個「好看」啊！

注：以上內容部分參考了以下文章，感謝原做者：

https://www.jianshu.com/p/e16ffb5b265d

https://blog.csdn.net/heyijia0327/article/details/47686523

討論

咱們知道（不知道的話，去查一下十四講）用g2o和ceres庫都能用來進行BA優化，這二者在使用過程當中有什麼不一樣？

歡迎留言討論，更多學習視頻、文檔資料、參考答案等關注計算機視覺life公衆號，，菜單欄點擊「知識星球」查看「從零開始學習SLAM」星球介紹，快來和其餘小夥伴一塊兒學習交流~

推薦閱讀

從零開始一塊兒學習SLAM | 爲何要學SLAM？ 從零開始一塊兒學習SLAM | 學習SLAM到底須要學什麼？ 從零開始一塊兒學習SLAM | SLAM有什麼用？ 從零開始一塊兒學習SLAM | C++新特性要不要學？ 從零開始一塊兒學習SLAM | 爲何要用齊次座標？ 從零開始一塊兒學習SLAM | 三維空間剛體的旋轉 從零開始一塊兒學習SLAM | 爲啥須要李羣與李代數？ 從零開始一塊兒學習SLAM | 相機成像模型 從零開始一塊兒學習SLAM | 不推公式，如何真正理解對極約束? 從零開始一塊兒學習SLAM | 神奇的單應矩陣 從零開始一塊兒學習SLAM | 你好，點雲 從零開始一塊兒學習SLAM | 給點雲加個濾網 從零開始一塊兒學習SLAM | 點雲平滑法線估計 零基礎小白，如何入門計算機視覺？ SLAM領域牛人、牛實驗室、牛研究成果梳理 我用MATLAB擼了一個2D LiDAR SLAM 可視化理解四元數，願你再也不掉頭髮 最近一年語義SLAM有哪些表明性工做？ 視覺SLAM技術綜述