从零开始一起学习SLAM | 掌握g2o顶点编程套路

点“计算机视觉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相关论文分类集锦

原文地址:https://www.cnblogs.com/CV-life/p/10449028.html

时间: 2024-11-10 08:20:09

从零开始一起学习SLAM | 掌握g2o顶点编程套路的相关文章

从零开始一起学习SLAM | 神奇的单应矩阵

小白最近在看文献时总是碰到一个奇怪的词叫"homography matrix",查看了翻译,一般都称作"单应矩阵",更迷糊了.正所谓:"每个字都认识,连在一块却不认识"就是小白的内心独白.查了一下书上的推导,总感觉有种"硬凑"的意味,于是又找到了师兄... 神奇的单应矩阵小白:师兄~单应矩阵是什么鬼啊?我看书上的推导,每一步勉强能看懂,但还是不太理解其背后的物理意义,感觉不能转化为自己理解的方式啊师兄:哦,我第一次看的时候也是

从零开始系统学习C/C++,C++系统学习路线

C++常用在后端服务器,移动互联网后端,经典Windows界面开发,移动互联网端界面开发,跨平台界面开发,图形分析,系统架构,应用设计,还有现在最为流行的分布架构等一些新的技术领域,C/C++工程师必备知识的思维导图. 从零开始系统学习C/C++,C++系统学习路线一.初级入门阶段 在入门之初,首先要做的是通过C/C++语言的学习培养编程思维和动手能力,深刻理解面向过程和面向对象的思想方法.这个阶段你需要学习: ① C语言 数据类型.变量.内存布局.指针基础: 字符串.一维数组.二维数组: 一级

Linux 程序设计学习笔记----终端及串口编程基础之概念详解

转载请注明出处,谢谢! linux下的终端及串口的相关概念有: tty,控制台,虚拟终端,串口,console(控制台终端)详解 部分内容整理于网络. 终端/控制台 终端和控制台都不是个人电脑的概念,而是多人共用的小型中型大型计算机上的概念. 1.终端 一台主机,连很多终端,终端为主机提供了人机接口,每个人都通过终端使用主机的资源. 终端有字符哑终端和图形终端两种. 控制台是另一种人机接口, 不通过终端与主机相连, 而是通过显示卡-显示器和键盘接口分别与主机相连, 这是人控制主机的第一人机接口.

java学习之第五章编程题示例(初学篇)

1 /* 2 Animal.java 3 */ 4 package animal; 5 6 public abstract class Animal { 7 public abstract void cry(); 8 public abstract String getanimalName(); 9 } 1 //Dog.java 2 package animal; 3 4 public class Dog extends Animal 5 { 6 7 String aa="旺旺"; 8

【OpenGL 学习笔记04】顶点数组

通过之前的学习,我们知道,如果要绘制一个几何图形,那就要不断的调用绘制函数,比如绘制一个20条边的多边形,起码要调用22条函数(包含glBegin和glEnd). 所以OpenGL提供了一系列的顶点数组函数减少函数调用的次数来提高性能.而且使用顶点还可以避免顶点共享的冗余处理. 1.简单示例 先来回顾一下之前我们是怎么画直线的: void drawOneLine(GLfloat x1,GLfloat y1,GLfloat x2,GLfloat y2) { glBegin(GL_LINES); g

ucore lab0的学习: 2.6 了解 ucore 编程方法

今天开始学习ucore的lab. 按照lab0的文档,显示介绍了80386的4个工作模式:real-address mode, protected mode, SMM mode, virtual 8086 mode. ia-32-architectures-software-developer-system-programming-manual-VOL3_Feb2014中的Figure 2-3. Transitions Among the Processor's Operating Modes是

Linux学习记录--文件管理相关系统编程

文件管理相关系统编程 重要文件标识 打开文件标识 O_RDONLY:只读方式打开 O_WRONLY:只写方式打开 O_RDWR:可读写方式打开 打开文件操作副标识 O_CREAT:若路径中文件不存在则创建,使用Open函数时需同时指定文件权限 O_EXCL:若与O_CREAT连用,检查文件是否已经存在,若不存在则建立文件存在则返回错误,这使创建和测试成为一个原子操作 O_APPEND:读写文件从文件尾部开始移动,所有写入数据都加入文件尾部 O_TRUNC:若文件存在并且可以写入,此标识会将源文件

JavaSE入门学习47:Socket网络通信编程(一)

今天既然我们学习Java的Socket网络通信编程,那么在我们的现实生活中那些是Socket编程的应用呢?可以想象 我们最常用的聊天工具软件QQ就是,当然MSN也是啦. 一网络基础知识 两台计算机通过网络进行通信的必要条件: 1)IP地址 2)网络协议 3)端口号 两台计算机网络通信示意图: (1)TCP/IP协议 1)TCP/IP协议是目前世界上应用最为广泛的协议,是以TCP和IP为基础的不同层次上多个协议的集合,也称 TCP/IP协议族或TCP/IP协议栈. 2)TCP:Transmissi

Linux 程序设计学习笔记----终端及串口编程及实例应用

转载请注明出处,http://blog.csdn.net/suool/article/details/38385355. 部分内容类源于网络. 终端属性详解及设置 属性 为了控制终端正常工作,终端的属性包括输入属性.输出属性.控制属性.本地属性.线路规程属性以及控制字符. 其在系统源代码的termios.h中定义(具体的说明文档http://pubs.opengroup.org/onlinepubs/7908799/xsh/termios.h.html),其结构体成员主要是 Thetermios