圖優化系列-g2o

尊重別人的勞動成果就是對自己的尊重——聲明至上：轉載來源：計算機視覺life公眾號

理解圖優化，一步步帶你看懂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里，圖優化一般分解為兩個任務：

1、構建圖。機器人位姿作為頂點，位姿間關系作為邊。

2、優化圖。調整機器人的位姿（頂點）來盡量滿足邊的約束，使得誤差最小。

下面就是一個直觀的例子。我們根據機器人位姿來作為圖的頂點，這個位姿可以來自機器人的編碼器，也可以是ICP匹配得到的，圖的邊就是位姿之間的關系。由於誤差的存在，實際上機器人建立的地圖是不准的，如下圖左。我們通過設置邊的約束，使得圖優化向着滿足邊約束的方向優化，最后得到了一個優化后的地圖（如下圖中所示），它和真正的地圖（下圖右）非常接近。

 

 

 

小白：哇塞，這個圖優化效果這么明顯啊！剛開始誤差那么大，最后都校正過來了

師兄：是啊，所以圖優化在SLAM中舉足輕重啊，一定得掌握！

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

先了解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的整個框架就是按照下圖中我標的這個順序來寫的。

 

 

高博在十四講中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);

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

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)

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

---

掌握g2o頂點編程套路

小白：師兄，上一次將的g2o框架《從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼》真的很清晰，我現在再去看g2o的那些優化的部分，基本都能看懂了呢！

師兄：那太好啦，以后多練習練習，加深理解

小白：嗯，我開始編程時，發現g2o的頂點和邊的定義也非常復雜，光看十四講里面，就有好幾種不同的定義，完全懵圈狀態。。。師兄，能否幫我捋捋思路啊

師兄：嗯，你說的沒錯，入門的時候確實感覺很亂，我最初也是花了些時間才搞懂的，下面分享一下。

g2o的頂點（Vertex) 從哪里來的？

師兄：在《g2o: A general Framework for (Hyper) Graph Optimization》這篇文檔里，我們找到那張經典的類結構圖。也就是上次講框架用到的那張結構圖。其中涉及到頂點 （vertex） 的就是下面 加了序號的3個東東了。

小白：記得呢，這個圖很關鍵，幫助我理清了很多思路，原來來自這篇文章啊

師兄：對，下面我們一步步來看吧。先來看看上圖中和vertex有關的第①個類： HyperGraph::Vertex，在g2o的GitHub上（https://github.com/RainerKuemmerle/g2o），它在這個路徑

g2o/core/hyper_graph.h

這個 HyperGraph::Vertex 是個abstract vertex，必須通過派生來使用。如下圖所示

然后我們看g2o 類結構圖中第②個類，我們看到HyperGraph::Vertex 是通過類OptimizableGraph 來繼承的， 而OptimizableGraph的定義在

g2o/core/optimizable_graph.h

我們找到vertex定義，發現果然，OptimizableGraph 繼承自 HyperGraph，如下圖所示

不過，這個OptimizableGraph::Vertex 也非常底層，具體使用時一般都會進行擴展，因此g2o中提供了一個比較通用的適合大部分情況的模板。就是g2o 類結構圖中 對應的第③個類：

BaseVertex<D, T>

那么它在哪里呢？ 在這個路徑：

g2o/core/base_vertex.h

小白：哇塞，原來是這樣抽絲剝繭的呀，學習了，授人以魚不如授人以漁啊！

師兄：嗯，其實就是根據那張圖結合g2o GitHub代碼就行了

g2o的頂點（Vertex) 參數如何理解？

小白：那是不是就可以開始用了？

師兄：別急，我們來看看參數吧，這個很關鍵。

我們來看一下模板參數 D 和 T，翻譯一下上圖紅框：

D是int 類型的，表示vertex的最小維度，比如3D空間中旋轉是3維的，那么這里 D = 3

T是待估計vertex的數據類型，比如用四元數表達三維旋轉的話，T就是Quaternion 類型

小白：哦哦，大概理解了，但還是有點模糊

師兄：我們進一步來細看一下D, T。這里的D 在源碼里面是這樣注釋的

static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space

可以看到這個D並非是頂點（更確切的說是狀態變量）的維度，而是其在流形空間（manifold）的最小表示，這里一定要區別開，另外，源碼里面也給出了T的作用

typedef T EstimateType;
EstimateType _estimate;

可以看到，這里T就是頂點（狀態變量）的類型，跟前面一樣。

小白：Got it!

如何自己定義頂點？

小白：師兄，我們是不是可以開始寫頂點定義了？

師兄：嗯，我們知道了頂點的基本類型是 BaseVertex<D, T>，那么下一步關心的就是如何使用了，因為在不同的應用場景（二維空間，三維空間），有不同的待優化變量（位姿，空間點），還涉及不同的優化類型（李代數位姿、李群位姿）

小白：這么多啊，那要自己根據 BaseVertex 一個個實現嗎？

師兄：那不需要！g2o本身內部定義了一些常用的頂點類型，我給找出來了，大概這些：

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>

// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>

// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>

小白：好全啊，我們可以直接用啦！

師兄：當然我們可以直接用這些，但是有時候我們需要的頂點類型這里面沒有，就得自己定義了。

重新定義頂點一般需要考慮重寫如下函數：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

小白：這些函數啥意思啊，我也就能看懂 read 和 write（/尷尬臉），還有每次定義都要重新寫這幾個函數嗎？

師兄：是的，這幾個是主要要改的地方。我們來看一下他們都是什么意義：

read，write：分別是讀盤、存盤函數，一般情況下不需要進行讀/寫操作的話，僅僅聲明一下就可以

setToOriginImpl：頂點重置函數，設定被優化變量的原始值。

oplusImpl：頂點更新函數。非常重要的一個函數，主要用於優化過程中增量△x 的計算。我們根據增量方程計算出增量之后，就是通過這個函數對估計值進行調整的，因此這個函數的內容一定要重視。

自己定義 頂點一般是下面的格式：

class myVertex: public g2::BaseVertex<Dim, Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      
      myVertex(){}
      
      virtual void read(std::istream& is) {}
      virtual void write(std::ostream& os) const {}
      
      virtual void setOriginImpl()
      {
          _estimate = Type();
      }
      virtual void oplusImpl(const double* update) override
      {
          _estimate += /*update*/;
      }
  }

小白：看不太懂啊，師兄

師兄：沒事，我們看例子就知道了，先看一個簡單例子，來自十四講中的曲線擬合，來源如下

ch6/g2o_curve_fitting/main.cpp

// 曲線模型的頂點，模板參數：優化變量維度和數據類型

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盤和讀盤：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

我們可以看到下面代碼中頂點初值設置為0，更新時也是直接把更新量 update 加上去的，知道為什么嗎？

小白：更新不就是 x + △x 嗎，這是定義吧

師兄：嗯，對於這個例子是可以直接加，因為頂點類型是Eigen::Vector3d，屬於向量，是可以通過加法來更新的。但是但是有些例子就不行，比如下面這個復雜點例子：李代數表示位姿VertexSE3Expmap

來自g2o官網，在這里

g2o/types/sba/types_six_dof_expmap.h

/**

 \* \brief SE3 Vertex parameterized internally with a transformation matrix

 and externally with its exponential map

 */

class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexSE3Expmap();
  bool read(std::istream& is);
  bool write(std::ostream& os) const;
  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());        //更新方式
  }
};

小白：師兄，這個里面的6, SE3Quat 分別是什么意思？

師兄：書中都寫了，以下來自十四講的介紹：

第一個參數6 表示內部存儲的優化變量維度，這是個6維的李代數

第二個參數是優化變量的類型，這里使用了g2o定義的相機位姿類型：SE3Quat。

在這里可以具體查看g2o/types/slam3d/se3quat.h

它內部使用了四元數表達旋轉，然后加上位移來存儲位姿，同時支持李代數上的運算，比如對數映射（log函數）、李代數上增量（update函數）等操作

說完了，那我現在問你個問題，為啥這里更新時沒有像上面那樣直接加上去？

小白：這個表示位姿，好像是不能直接加的我記得，原因有點忘了

師兄：嗯，是不能直接加，原因是變換矩陣不滿足加法封閉。那我再問你，為什么相機位姿頂點類VertexSE3Expmap使用了李代數表示相機位姿，而不是使用旋轉矩陣和平移矩陣？

小白：不造啊。。

師兄：其實也是上述原因的拓展：這是因為旋轉矩陣是有約束的矩陣，它必須是正交矩陣且行列式為1。使用它作為優化變量就會引入額外的約束條件，從而增大優化的復雜度。而將旋轉矩陣通過李群-李代數之間的轉換關系轉換為李代數表示，就可以把位姿估計變成無約束的優化問題，求解難度降低。

小白：原來如此啊，以前學的東西都忘了。。

師兄：以前學的要多看，溫故而知新。我們繼續看例子，剛才是位姿的例子，下面是三維點的例子，空間點位置 VertexPointXYZ，維度為3，類型是Eigen的Vector3，比較簡單，就不解釋了

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const number_t* update)
    {
      Eigen::Map<const Vector3> v(update);
      _estimate += v;
    }
};

如何向圖中添加頂點？

師兄：往圖中增加頂點比較簡單，我們還是先看看第一個曲線擬合的例子，setEstimate(type) 函數來設定初始值；setId(int) 定義節點編號

    // 往圖中增加頂點
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

這個是添加 VertexSBAPointXYZ 的例子，都很容易看懂

/ch7/pose_estimation_3d2d.cpp

 int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); 
        optimizer.addVertex ( point );
    }

至此，我們講完了g2o 的頂點的來源，定義，自定義方法，添加方法，基本上你以后再看到頂點就不會陌生啦！

小白：太感謝啦！

---

 

掌握g2o邊的代碼套路

小白：師兄，g2o框架《從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼》，以及頂點《從零開始一起學習SLAM | 掌握g2o頂點編程套路》我都學完啦，今天給我講講g2o中的邊吧！是不是也有什么套路？

師兄：嗯，g2o的邊比頂點稍微復雜一些，不過前面你也了解了許多g2o的東西，有沒有發現g2o的編程基本都是固定的格式（套路）呢？

小白：是的，我現在按照師兄說的g2o框架和頂點設計方法，再去看g2o實現不同功能的代碼，發現都是一個模子出來的，只不過在某些地方稍微改改就行了啊

師兄：是這樣的。我們來看看g2o的邊到底是咋回事。

初步認識g2o的邊

師兄：在《g2o: A general Framework for (Hyper) Graph Optimization》這篇文檔里，我們找到那張經典的類結構圖，里面關於邊（edge）的部分是這樣的，重點是下圖中紅色框內。

上一次我們講頂點的時候，還專門去追根溯源查找頂點類之間的繼承關系，邊其實也是類似的，我們在g2o官方GitHub上這些

g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

頭文件下就能看到這些繼承關系了，我們就不像之前頂點那樣一個個去追根溯源了，如果有興趣你可以自己去試試看。我們主要關注一下上面紅框內的三種邊。

BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分別表示一元邊，兩元邊，多元邊。

小白：他們有啥區別啊？
師兄：一元邊你可以理解為一條邊只連接一個頂點，兩元邊理解為一條邊連接兩個頂點，也就是我們常見的邊啦，多元邊理解為一條邊可以連接多個（3個以上）頂點

一個比較丑的示例

下面我們來看看他們的參數有什么區別？你看主要就是 幾個參數：D, E, VertexXi, VertexXj，他們的分別代表：

D 是 int 型，表示測量值的維度 （dimension）
E 表示測量值的數據類型
VertexXi，VertexXj 分別表示不同頂點的類型

比如我們用邊表示三維點投影到圖像平面的重投影誤差，就可以設置輸入參數如下：

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

你說說看 這個定義是什么意思？
小白：首先這個是個二元邊。第1個2是說測量值是2維的，也就是圖像像素坐標x,y的差值，對應測量值的類型是Vector2D，兩個頂點也就是優化變量分別是三維點 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap？

師兄：對的，就是這樣~當然除了輸入參數外，定義邊我們通常需要復寫一些重要的成員函數
小白：聽着和頂點類似哦，也是復寫成員函數，頂點里主要復寫了頂點更新函數oplusImpl和頂點重置函數setToOriginImpl，邊的話是不是也差不多？
師兄：邊和頂點的成員函數還是差別比較大的，邊主要有以下幾個重要的成員函數

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();

下面簡單解釋一下

read，write：分別是讀盤、存盤函數，一般情況下不需要進行讀/寫操作的話，僅僅聲明一下就可以
computeError函數：非常重要，是使用當前頂點的值計算的測量值與真實的測量值之間的誤差
linearizeOplus函數：非常重要，是在當前頂點的值下，該誤差對優化變量的偏導數，也就是我們說的Jacobian

除了上面幾個成員函數，還有幾個重要的成員變量和函數也一並解釋一下：

_measurement：存儲觀測值
_error：存儲computeError() 函數計算的誤差
_vertices[]：存儲頂點信息，比如二元邊的話，_vertices[] 的大小為2，存儲順序和調用setVertex(int, vertex) 是設定的int 有關（0 或1）
setId(int)：來定義邊的編號（決定了在H矩陣中的位置）
setMeasurement(type) 函數來定義觀測值
setVertex(int, vertex) 來定義頂點
setInformation() 來定義協方差矩陣的逆

后面我們寫代碼的時候回經常遇到他們的。

如何自定義g2o的邊？

小白：前面你介紹了g2o中邊的基本類型、重要的成員變量和成員函數，那么如果我們要定義邊的話，具體如何編程呢？
師兄：我這里正好有個模板給你看看，基本上定義g2o中的邊，就是如下套路：

 class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
      myEdge(){}     
      virtual bool read(istream& in) {}
      virtual bool write(ostream& out) const {}      
      virtual void computeError() override
      {
          // ...
          _error = _measurement - Something;
      }      
      virtual void linearizeOplus() override
      {
          _jacobianOplusXi(pos, pos) = something;
          // ...         
          /*
          _jocobianOplusXj(pos, pos) = something;
          ...
          */
      }      
      private:
      // data
  }

我們可以發現，最重要的就是computeError()，linearizeOplus()兩個函數了

小白：嗯，看起來好像也不難啊
師兄：我們先來看一個簡單例子，地址在
https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp
這個是個一元邊，主要是定義誤差函數了，如下所示，你可以發現這個例子基本就是上面例子的一丟丟擴展，是不是感覺so easy？

// 誤差模型 模板參數：觀測值維度，類型，連接頂點類型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 計算曲線模型誤差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值為 _measurement
};

小白：嗯，這個能看懂
師兄：下面是一個復雜一點例子，3D-2D點的PnP 問題，也就是最小化重投影誤差問題，這個問題非常常見，使用最常見的二元邊，弄懂了這個基本跟邊相關的代碼也差不多都一通百通了

代碼在g2o的GitHub上這個地方可以看到
g2o/types/sba/types_six_dof_expmap.h
這里根據自己理解對代碼加了注釋，方便理解

//繼承了BaseBinaryEdge類，觀測值是2維，類型Vector2D,頂點分別是三維點、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    //1. 默認初始化
    EdgeProjectXYZ2UV();
    //2. 計算誤差
    void computeError()  {
      //李群相機位姿v1
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      // 頂點v2
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      //相機參數
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
     //誤差計算，測量值減去估計值，也就是重投影誤差obs-cam
     //估計值計算方法是T*p,得到相機坐標系下坐標，然后在利用camera2pixel()函數得到像素坐標。
      Vector2D obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }
    //3. 線性增量函數，也就是雅克比矩陣J的計算方法
    virtual void linearizeOplus();
    //4. 相機參數
    CameraParameters * _cam; 
    bool read(std::istream& is);
    bool write(std::ostream& os) const;
};

有一個地方比較難理解

_error = obs - cam->cam_map(v1->estimate().map(v2->estimate()));

小白：我確實看不懂這一句。。
師兄：其實就是：誤差 = 觀測 - 投影

下面我給你捋捋思路。我們先來看看cam_map 函數，它的定義在
g2o/types/sba/types_six_dof_expmap.cpp
cam_map 函數功能是把相機坐標系下三維點（輸入）用內參轉換為圖像坐標（輸出），具體代碼如下所示

Vector2  CameraParameters::cam_map(const Vector3 & trans_xyz) const {
  Vector2 proj = project2d(trans_xyz);
  Vector2 res;
  res[0] = proj[0]*focal_length + principle_point[0];
  res[1] = proj[1]*focal_length + principle_point[1];
  return res;
}

然后看 .map函數，它的功能是把世界坐標系下三維點變換到相機坐標系，函數在
g2o/types/sim3/sim3.h
具體定義是

  Vector3 map (const Vector3& xyz) const {
        return s*(r*xyz) + t;
      }

因此下面這個代碼

v1->estimate().map(v2->estimate())

 

就是用V1估計的pose把V2代表的三維點，變換到相機坐標系下。

小白：原來如此，以前我都忽視了這些東西了，沒想到里面是這樣的關聯的。
師兄：嗯，我們繼續，前面主要是對computeError() 的理解，還有一個很重要的函數就是linearizeOplus()，用來定義雅克比矩陣
我摘取了相關代碼（來自：g2o/g2o/types/sba/types_six_dof_expmap.cpp），並進行了標注，相信會更容易理解

十四講第169頁中的雅克比矩陣完全是按照書上 式子（7.45）、（7.47）來編程的，不難理解
小白：后面就是直接照抄書上就行，哈哈

如何向圖中添加邊？

師兄：前面我們講過如何往圖中增加頂點，可以說非常easy了，往圖中增加邊會稍微多一些內容，我們還是先從最簡單的 例子說起：一元邊的添加方法

下面代碼來自GitHub上，仍然是前面曲線擬合的例子
slambook/ch6/g2o_curve_fitting/main.cpp

    // 往圖中增加邊
    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 );
    }

小白：setMeasurement 函數的輸入的觀測值具體是指什么？
師兄：對於這個曲線擬合，觀測值就是實際觀測到的數據點。對於視覺SLAM來說，通常就是我們我們觀測到的特征點坐標，下面就是一個例子。這個例子比剛才的復雜一點，因為它是二元邊，需要用邊連接兩個頂點
代碼來自GitHub上
slambook/ch7/pose_estimation_3d2d.cpp

    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

小白：這里的setMeasurement函數里的p來自向量points_2d，也就是特征點的圖像坐標(x,y)了吧！
師兄：對，這正好呼應我剛才說的。另外，你看setVertex 有兩個一個是 0 和 VertexSBAPointXYZ 類型的頂點，一個是1 和pose。你覺得這里的0和1是什么意思？能否互換呢？

小白：0，1應該是分別指代哪個頂點吧，直覺告訴我不能互換，可能得去查查頂點定義部分的代碼
師兄：你的直覺沒錯！我幫你 查過啦，你看這個是setVertex在g2o官網的定義：

// set the ith vertex on the hyper-edge to the pointer supplied
void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}

這段代碼在
g2o/core/hyper_graph.h
里可以找到。你看 _vertices[i] 里的i就是我們這里的0和1，我們再去看看這里邊的類型： g2o::EdgeProjectXYZ2UV
的定義，前面我們也放出來了，就這兩句

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV 
.....
 //李群相機位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 頂點v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);

你看 _vertices[0] 對應的是 VertexSBAPointXYZ 類型的頂點，也就是三維點，_vertices[1] 對應的是VertexSE3Expmap 類型的頂點，也就是位姿pose。因此前面 1 對應的就應該是 pose，0對應的 應該就是三維點。

小白：原來如此，之前都沒注意這些，看來g2o不會幫我區分頂點的類型啊，以后這里編程要對應好，不然錯了都找不到原因呢！謝謝師兄，今天又是收獲滿滿的一天！

練習

題目：用直接法Bundle Adjustment 估計相機位姿。給定3張圖片，兩個txt文件，其中poses.txt中存儲3張圖片對應的相機初始位姿（Tcw），格式為：timestamp, tx, ty, tz, qx, qy, qz, qw ，分別對應時間戳、平移、旋轉（四元數），而points.txt中存儲的是3D點集合以及該點周圍 4x4 窗口的灰度值，記做 I(p)i，格式為：

x, y, z, 灰度1，灰度2...，灰度16

我們把每個3D點投影到對應圖像中，用投影后點周圍的灰度值與原始窗口的灰度值差異作為待優化誤差。

請使用g2o進行優化，並繪制結果（繪制函數已經寫好）。

代碼框架中需要你填寫頂點、邊的定義。如果正確，輸出結果如下圖所示：