從零開始一起學習SLAM | 掌握g2o邊的代碼套路


點“計算機視覺life”關注,置頂更快接收消息!


小白:師兄,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進行優化,並繪制結果(繪制函數已經寫好)。

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

預期結果.png

參考:

高翔《視覺 SLAM十四講》
https://blog.csdn.net/try_again_later/article/details/81813639

代碼框架、數據、窗口值具體順序、優化目標函數、預期輸出結果已經為你准備好了,公眾號「計算機視覺life」后台回復:,即可獲得。

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

img

推薦閱讀

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


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM