點“計算機視覺life”關註，置頂更快接收消息！ ##

小白：師兄，上一次將的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 的頂點的來源，定義，自定義方法，添加方法，基本上你以後再看到頂點就不會陌生啦！

小白：太感謝啦！

編程練習

• 題目：給定一組世界坐標系下的3D點(p3d.txt)以及它在相機中對應的坐標(p2d.txt)，以及相機的內參矩陣。

  使用bundle adjustment 方法（g2o庫實現）來估計相機的位姿T。初始位姿T為單位矩陣。

• 本程序學習目標：熟悉g2o庫編寫流程，熟悉頂點定義方法。

代碼框架、數據及預期結果已經為你準備好了，公眾號「計算機視覺life」後臺回復：頂點，即可獲得。

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

本文參考：

高翔《視覺SLAM十四講》

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

推薦閱讀

從零開始一起學習SLAM | 為什麽要學SLAM？
從零開始一起學習SLAM | 學習SLAM到底需要學什麽？
從零開始一起學習SLAM | SLAM有什麽用？
從零開始一起學習SLAM | C++新特性要不要學？
從零開始一起學習SLAM | 為什麽要用齊次坐標？
從零開始一起學習SLAM | 三維空間剛體的旋轉
從零開始一起學習SLAM | 為啥需要李群與李代數？
從零開始一起學習SLAM | 相機成像模型
從零開始一起學習SLAM | 不推公式，如何真正理解對極約束?
從零開始一起學習SLAM | 神奇的單應矩陣
從零開始一起學習SLAM | 你好，點雲
從零開始一起學習SLAM | 給點雲加個濾網
從零開始一起學習SLAM | 點雲平滑法線估計
從零開始一起學習SLAM | 點雲到網格的進化
從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼
零基礎小白，如何入門計算機視覺？
SLAM領域牛人、牛實驗室、牛研究成果梳理
我用MATLAB擼了一個2D LiDAR SLAM
可視化理解四元數，願你不再掉頭發
最近一年語義SLAM有哪些代表性工作？
視覺SLAM技術綜述
匯總 | VIO、激光SLAM相關論文分類集錦

從零開始一起學習SLAM | 掌握g2o頂點編程套路