2. 程式人生 > >g2o學習

g2o學習

阿新 • • 發佈：2018-11-26

寫在前面

跟著g2o的slam2d_tutorial進行了學習，發現自己對於頂點和邊的理解還是不太夠，覺得有必要把頂點和邊的一些東西再給總結一下，主要參考的就是如下網站：
http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html
這個網站裡面有較為全面的g2o的類以及函式的講解，很方便。

g2o的頂點（Vertex）

首先我們來看一下頂點的繼承關係：
頂點的繼承關係
可以看到比較“成熟”的型別就是BaseVertex了，由於我們一般在派生的時候就是繼承自這個類的，下面主要是對這個類進行分析，其中個人認為還是有很多東西要注意的。

g2o::BaseVertex< D, T >

int D, typename T

首先記錄一下定義模板的兩個引數D和T，兩個型別分別是int和typename的型別，D表示的是維度，g2o原始碼裡面是這個註釋的

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

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

typedef T EstimateType;
EstimateType _estimate;
  • 1
  • 2

可以看到，這裡T就是頂點（狀態變數）的型別。
在頂點的繼承中，這兩個引數是直接面向我們的，所以務必要定義妥當。

_hessian矩陣和_b向量

看到這兩個名字就感覺很激動，畢竟增量方程美名遠揚，而其中很重要的就是H和b兩個引數，這裡的_hessian和_b是增量方程中H和b的一部分，更確切的說是對應於該頂點的部分，下面簡單的說一下這兩個引數的作用。

_hessian矩陣

它的型別是Eigen中的Map型別，也就是這個引數只是一個對映，把一塊記憶體區域對映為Eigen中的資料型別，具體這裡就不贅述了。它的作用也比較簡單，就是拿到邊中算出的jacobian，之後根據公式

H=JTWJH=JTWJ計算出整個大H中該處的資料。
相關的程式程式碼這裡貼出來：
block_solver.hpp

  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    if (! v->marginalized()){
      //assert(poseIdx == v->hessianIndex());
      PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
      if (zeroBlocks)
        m->setZero();
      v->mapHessianMemory(m->data());
      ++poseIdx;
    } else {
      LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
      if (zeroBlocks)
        m->setZero();
      v->mapHessianMemory(m->data());
      ++landmarkIdx;
    }
  }
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

base_binary_edge.hpp

if (this->robustKernel() == 0) {
   if (fromNotFixed) {
     Eigen::Matrix<number_t, VertexXiType::Dimension, D, Eigen::ColMajor> AtO = A.transpose() * omega;
     from->b().noalias() += A.transpose() * omega_r;
     from->A().noalias() += AtO*A;
     if (toNotFixed ) {
       if (_hessianRowMajor) // we have to write to the block as transposed
         _hessianTransposed.noalias() += B.transpose() * AtO.transpose();
       else
         _hessian.noalias() += AtO * B;
     }
   } 
   if (toNotFixed) {
     to->b().noalias() += B.transpose() * omega_r;
     to->A().noalias() += B.transpose() * omega * B;
   }
 } else { // robust (weighted) error according to some kernel
   number_t error = this->chi2();
   Vector3 rho;
   this->robustKernel()->robustify(error, rho);
   InformationType weightedOmega = this->robustInformation(rho);
   //std::cout << PVAR(rho.transpose()) << std::endl;
   //std::cout << PVAR(weightedOmega) << std::endl;

   omega_r *= rho[1];
   if (fromNotFixed) {
     from->b().noalias() += A.transpose() * omega_r;
     from->A().noalias() += A.transpose() * weightedOmega * A;
     if (toNotFixed ) {
       if (_hessianRowMajor) // we have to write to the block as transposed
         _hessianTransposed.noalias() += B.transpose() * weightedOmega * A;
       else
         _hessian.noalias() += A.transpose() * weightedOmega * B;
     }
   } 
   if (toNotFixed) {
     to->b().noalias() += B.transpose() * omega_r;
     to->A().noalias() += B.transpose() * weightedOmega * B;
   }
 }
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40

_b矩陣

它的型別就是簡單的Eigen::Vector，這裡不是用的對映關係，但是他的作用和上述的類似，只不過最後是通過拷貝把b=JTerrorb=JTerror給整個的b。
相關程式碼如下（上面的base_binary_edge.hpp程式碼有意部分）：
base_vertex.h

virtual int copyB(number_t* b_) const {
  memcpy(b_, _b.data(), Dimension * sizeof(number_t));
  return Dimension; 
}
  • 1
  • 2
  • 3
  • 4

總結

g2o的頂點的繼承還是比較容易的，但是一定要搞清楚D和E表示什麼，不要填寫錯誤（這裡的D對應BlockSolver中的PoseMatrixTpye的維度）。

g2o的邊（Edge）

首先我們也是先給出邊的繼承關係
g2o二元邊的繼承關係
這裡擷取的是二元邊的繼承圖，下面的分析也是以該二元邊為例的。

g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >

int D, typename E

首先還是介紹這兩個引數，還是從原始碼上來看

static const int Dimension = D;
typedef E Measurement;
typedef Eigen::Matrix<number_t, D, 1, Eigen::ColMajor> ErrorVector;
  • 1
  • 2
  • 3

可以看到，D決定了誤差的維度，從對映的角度講，三維情況下就是2維的，二維的情況下是1維的；然後E是measurement的型別，也就是測量值是什麼型別的，這裡E就是什麼型別的（一般都是Eigen::VectorN表示的，N是自然數）。
和前面一樣，這兩個引數是直接面向我們的，一定要定義妥當。

typename VertexXi, typename VertexXj

這兩個引數就是邊連線的兩個頂點的型別，這裡特別注意一下，這兩個必須一定是頂點的型別，也就是繼承自BaseVertex等基礎類的類！不是頂點的資料類！例如必須是VertexSE3Expmap而不是VertexSE3Expmap的資料型別類SE3Quat。原因的話原始碼裡面也很清楚，因為後面會用到一系列頂點的維度等等的屬性，這些屬性是資料型別類裡面沒有的。

_jacobianOplusXi，_jacobianOplusXj和_hessian

這幾個引數是一些內部的引數，_jacobianOplusXi和_jacobianOplusXj也算是在繼承的時候需要面向的引數，不算特別內部，但是這裡還是記錄說明一下。

_jacobianOplusXi，_jacobianOplusXj

這兩個變數本質上是Eigen::Matrix型別的，具體定義在這裡：

typedef typename Eigen::Matrix<number_t, D, Di, D==1?Eigen::RowMajor:Eigen::ColMajor>::AlignedMapType JacobianXiOplusType;
typedef typename Eigen::Matrix<number_t, D, Dj, D==1?Eigen::RowMajor:Eigen::ColMajor>::AlignedMapType JacobianXjOplusType;
JacobianXiOplusType _jacobianOplusXi;
JacobianXjOplusType _jacobianOplusXj;
  • 1
  • 2
  • 3
  • 4

然後這裡給出一個base_binary_edge類中的jacobian的程式：
base_binary_edge.hpp

template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus()
{
  VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
  VertexXjType* vj = static_cast<VertexXjType*>(_vertices[1]);

  bool iNotFixed = !(vi->fixed());
  bool jNotFixed = !(vj->fixed());

  if (!iNotFixed && !jNotFixed)
    return;

#ifdef G2O_OPENMP
  vi->lockQuadraticForm();
  vj->lockQuadraticForm();
#endif

  const number_t delta = cst(1e-9);
  const number_t scalar = 1 / (2*delta);
  ErrorVector errorBak;
  ErrorVector errorBeforeNumeric = _error;

  if (iNotFixed) {
    //Xi - estimate the jacobian numerically
    number_t add_vi[VertexXiType::Dimension] = {};

    // add small step along the unit vector in each dimension
    for (int d = 0; d < VertexXiType::Dimension; ++d) {
      vi->push();
      add_vi[d] = delta;
      vi->oplus(add_vi);
      computeError();
      errorBak = _error;
      vi->pop();
      vi->push();
      add_vi[d] = -delta;
      vi->oplus(add_vi);
      computeError();
      errorBak -= _error;
      vi->pop();
      add_vi[d] = 0.0;

      _jacobianOplusXi.col(d) = scalar * errorBak;
    } // end dimension
  }

  if (jNotFixed) {
    //Xj - estimate the jacobian numerically
    number_t add_vj[VertexXjType::Dimension] = {};

    // add small step along the unit vector in each dimension
    for (int d = 0; d < VertexXjType::Dimension; ++d) {
      vj->push();
      add_vj[d] = delta;
      vj->oplus(add_vj);
      computeError();
      errorBak = _error;
      vj->pop();
      vj->push();
      add_vj[d] = -delta;
      vj->oplus(add_vj);
      computeError();
      errorBak -= _error;
      vj->pop();
      add_vj[d] = 0.0;

      _jacobianOplusXj.col(d) = scalar * errorBak;
    }
  } // end dimension

  _error = errorBeforeNumeric;
#ifdef G2O_OPENMP
  vj->unlockQuadraticForm();
  vi->unlockQuadraticForm();
#endif
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76

由程式可以看出，這裡的求解演算法是從定義出發的一種求解，數學上的推導類似於：
假定有函式f(x⃗) x⃗=(a,b,c)Tf(x→) x→=(a,b,c)T，此時我們要求解該函式的偏導數，自然按照定義就行求解：

limΔx→0f(x⃗+Δx→)−f(x⃗)Δx→limΔx→0f(x→+Δx→)−f(x→)Δx→
對於假定的函式，我們可以進行如下的推導：
J=[f(x⃗+Δa)−f(x⃗)Δa,f(x⃗+Δb)−f(x⃗)Δb,f(x⃗+Δc)−f(x⃗)Δc]J=[f(x→+Δa)−f(x→)Δa,f(x→+Δb)−f(x→)Δb,f(x→+Δc)−f(x→)Δc]
上面的程式碼基本上就是按照這個思路進行的，只不過程式碼取了 ±Δx±Δx進行求取。
這裡，一個問題出現在我的腦海裡，既然這個方法是如此的正確（都按照定義來了哇），為什麼我們還要在繼承的時候把這個方法覆寫呢？想了段時間，我覺得可能是因為該段程式碼比較費時吧，如果我們有幾個乘法就能求出jacobian，那麼勢必不會用這樣的迴圈去做，特別是狀態變數多的時候！當然這裡僅僅是個人的見解， 有大神知道的話還煩請留言說一下。

_hessian矩陣

這裡hessian矩陣的作用和頂點裡面的無二，都是一個Map的對映，旨在成小塊的構建大的H矩陣，這裡千萬不要以為邊構建的是Hpl和HlpHpl和Hlp，主要還是取決於邊連線的頂點的型別，在g2o裡面，Hpl和HlpHpl和Hlp主要是靠頂點的marginalization標誌進行區分的，標誌為false，則將會歸在HppHpp中，標誌為true則會歸在HllHll中，可不是我們認為這是pose就歸在HppHpp，這是point就歸在HllHll中的！下面給出的是整個大H矩陣的塊狀結構。

H=[HppHlpHplHll]H=[HppHplHlpHll]
主要的程式碼這裡也給出來：
block_solver.h 

for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
  OptimizableGraph::Edge* e = *it;

  for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
    OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
    int ind1 = v1->hessianIndex();
    if (ind1 == -1)
      continue;
    int indexV1Bak = ind1;
    for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
      OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
      int ind2 = v2->hessianIndex();
      if (ind2 == -1)
        continue;
      ind1 = indexV1Bak;
      bool transposedBlock = ind1 > ind2;
      if (transposedBlock){ // make sure, we allocate the upper triangle block
        std::swap(ind1, ind2);
      }
      if (! v1->marginalized() && !v2->marginalized()){
        PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
        if (zeroBlocks)
          m->setZero();
        e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
        if (_Hschur) {// assume this is only needed in case we solve with the schur complement
          schurMatrixLookup->addBlock(ind1, ind2);
        }
      } else if (v1->marginalized() && v2->marginalized()){
        // RAINER hmm.... should we ever reach this here????
        LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
        if (zeroBlocks)
          m->setZero();
        e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
      } else { 
        if (v1->marginalized()){ 
          PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
        } else {
          PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
        }
      }
    }
  }
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49

總結

  1. 在定義自己的邊和頂點的時候，務必要弄明白D和E所代表的含義，在定義邊的時候還要注意後面兩個引數一定是一個頂點的類而不是資料型別；
  2. 在邊的linearizeOplus函式定義時，如果我們有更簡潔的數學表達，那麼可以轉化為程式語言進行求解，如果沒有，也可以使用父類的求解方法，但是這種方法由於使用了迴圈，甚至中間還求了多次對映誤差，因此較為耗時；
  3. 千萬不要以為g2o會幫你區分什麼是pose，什麼是point，從而對號入座在HppHpp和HllHll中，這塊必須由使用者設定marginalization告訴g2o什麼頂點歸在那一塊裡面；
  4. 綜合看來，g2o幫助我們實現了很多內部的演算法，但是在進行構造的時候，也需要遵循一些規則，在我看來這是可以接收的，畢竟一個程式不可能滿足所有的要求，因此在以後g2o的使用中還是應該多看多記，這樣才能更好的使用這個庫。

相關推薦

g2o學習

寫在前面 跟著g2o的slam2d_tutorial進行了學習，發現自己對於頂點和邊的理解還是不太夠，覺得有必要把頂點和邊的一些東西再給總結一下，主要參考的就是如下網站： http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html

g2o學習——g2o整體框架

寫在前面進來對g2o優化庫進行了學習，雖然才模仿著寫了兩個例程，但是對於整個g2o的理解和使用方面還是多了不少的感觸，特此寫下部落格，對這些天的學習進行記錄。g2o的整體結構說到整體的結構，不得不用一張比較概括的圖來說明：這張圖最好跟著畫一下，這樣能更好的理解和掌握，例如我第

《一起學習rgbdSLAM》中g2o部分報錯的問題

《一起學習rgbdSLAM》中g2o部分報錯的問題 在高博的《一起學習rgbdSLAM》第六講中使用到了g2o工具。但是由於版本問題，按照高博的寫法來編寫程式碼在編譯過程中會報錯。所以需要對該部分程式碼稍作修改。 我使用的系統是ubuntu16.04。 教程中的原始碼

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

視覺slam 必須 hat 一個 point 公眾 pmap plus 存儲 點“計算機視覺life”關註，置頂更快接收消息！ ## 小白：師兄，上一次將的g2o框架《從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼》真的很清晰，我現在再去看g2o的

6、自學——Linux的學習進度與任務【FHS】

include 同名 med 可選 第三方 安裝 lin 三方 引導 FHS：文件層次標準 FHS:文件層次標準 　　 / : 代表根目錄 　　 /bin: 二進制文件，可執行程序，所有用戶都能用。 　　/sbin: 只有管理員執行的，二進制可執行程序。

Robot Operating System (ROS)學習筆記4---語音控制

sla 語音 出現 tput http 學習 process 輸入 ubun 搭建環境：XMWare Ubuntu14.04 ROS（indigo） 轉載自古月居 轉載連接：http://www.guyuehome.com/260 一、語音識別包 1、安裝

2017.5.3上午學習內容

技術 images logs log -1 alt cnblogs 學習內容 image 聽刑法第十一章2017.5.3上午學習內容

設計模式學習總結（八）策略模式(Strategy)

isp 筆記本 override div ont 角色 write stat 通過 　　策略模式，主要是針對不同的情況采用不同的處理方式。如商場的打折季，不同種類的商品的打折幅度不一，所以針對不同的商品我們就要采用不同的計算方式即策略來進行處理。 　　一、示例展示： 　　以

設計模式學習總結（七）適配器模式(Adapter)

實現接口 國外 手機 額外 sed ges program ebe 通過 　　適配器模式主要是通過適配器來實現接口的統一，如要實現國內手機在國外充電，則需要在不同的國家采用不同的適配器來進行兼容！ 　　一、示例展示： 　　以下例子主要通過給筆記本電腦添加類似手機打電話和發短

5月3日學習收獲

rate add 破解 mdk ext visio 不能 區別 .exe keil uvision4 破解：MDK412安裝包→一路next→安裝好了之後，用管理員模式打開軟件→File→License Management→Computer ID→復制CID→keilli

python學習之路（四）

[1] size class dex epc uri msu 語句 這就是 繼續昨天的學習，學到了數組。 首先有兩個數組，name1和name2.我們可以將兩個數組合並 name1=[1,2,3,4] name2=[5,6,7,8] names=name1.extend(

5月3日上午學習日誌

能力 程序 系統調用 logs 學習日誌 執行 shel 管理軟件 -1 2017年5月3日上午把昨天記憶的英語單詞的多種詞意用自己組句的方法聯系起來再記憶一遍，然後再學10個考研高頻詞匯，完成英語app的打卡和看通信工程視頻。今天基本上完成了通信工程教學視頻中對操作系統組

MySQL學習筆記（六）—— MySQL自連接

概念 cor 子查詢 ron 表操作 例子 質量 _id order by 有的時候我們需要對同一表中的數據進行多次檢索,這個時候我們可以使用之前學習過的子查詢,先查詢出需要的數據,再進行一次檢索。 例如:一張products表,有產品id,供應商id(vend_

PHP學習路線[轉]

訪問 驗證 開發技術 adc 階段 javascrip 性能 導航 段落 第一階段第一講，WEB基礎 1.1 網站基本知識； 1.2 網絡協議介紹； 1.3 B/S與C/S結構的區別； 1.4 WEB編程、網站開發技術介紹。 第二講，網頁設計

學習過程遇到問題的解決方法

_exit 變量 prot 預定義變量 void col malloc() lee 項目組 1.select.sh 用sh -x 和bash -x的進行腳本調試時，前者會報錯誤，這是內置shell的原因。 2.awk內置函數使用時，定義變量需要用-v ；awk -v s

Linux學習134 Unit 8

windows linux 客戶端 記錄 認證 Unit8 ldap網絡帳號1.ldap是什麽ldap目錄服務認證，和windows活動目錄類似，就是記錄數據的一種方式 2.ldap客戶端所須軟件yum sssd krb5-workstation -y 3.如何開啟ldap用戶認證auth

Linux學習134 Unit 11

系統恢復unit11系統恢復1.系統啟動流程通電 ||bios(主板上的只讀存儲中，basic input or output system)作用，硬件檢測，激活硬件||grub系統引導（grub引導分為兩個階段）1）階段1 mbr（主引導記錄）主引導記錄在硬盤上的0磁道，一扇區，446個字節*）dd if=

nodejs學習 之 安裝

c盤 信息 如果 安裝過程 ima 打開 nodejs https x64 1. 官網找最新適合自己電腦的版本 下載 https://nodejs.org/en/download/ 2.我的是win7 x64選擇了msi的安裝包，安裝過程修改安裝的目標目錄，最好不要

Redis的安裝及學習

mongod 無效 擴展 管理 數據持久化 windows string類型 實例 配置 最近因為做Chatbot項目需要對於NoSQL數據庫進行研究，調研範圍包括MongoDB和Redis。本文將介紹Redis在Windows環境的安裝及如何利用python來操作Redi

Struts2學習三----------Action搜索順序

round col uil 1-1 earch sch 預覽 image pin ? 版權聲明：本文為博主原創文章，轉載請註明出處 Struts2的Action的搜索順序 　　http://localhost:8080/path1/path2/student.action