g2o学习

其他 2018-11-26 22:57:47 阅读次数: 0

写在前面

跟着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的使用中还是应该多看多记，这样才能更好的使用这个库。

猜你喜欢

转载自blog.csdn.net/hai008007/article/details/80947974
g2o
今日推荐
开源日报 | Chrome内置Gemini的意义不在于Gemini；中国AI追随之路的五大误区；ECharts创始人“下海”养鱼；谷歌I/O开发者大会什么都有，只是没有惊喜
微软回应中国区AI团队“打包赴美”传闻
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
《2024 年一季度互联网投融资运行情况》研究报告
报告：Django 仍然是 74% 开发者的首选
周排行
laravle中orm简单的增删改查
文本分类 特征选取之CHI开方检验
Spark核心编程-WordCount
大数据开发实战系列之电信客服(1)
读书笔记 - 把时间当作朋友 by 李笑来
python 笔记--if else
SpringBoot/Mybatis/Druid, 多数据源MultiDataSource配置思路
排序三个整数
redis集群搭建【2】-Windows中Redis集群搭建
STM32F030驱动TM1650点亮4联数码管
每日归档
更多
2024-05-16(6)
2024-05-15(24)
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022