专栏首页计算机视觉life从零开始一起学习SLAM | 掌握g2o边的代码套路

从零开始一起学习SLAM | 掌握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对应的 应该就是三维点。

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 太極:MIT华人学神开源的计算机图形库

    太極(Taichi)是一个开源的计算机图形库,它旨在提供易于使用的计算机图形学基础架构,并提供了 40 多份计算机图形学重要研究的实现。出于效率的考虑,太極本身...

    机器之心
  • 网易杭州游戏研发实习面经

    在牛客混了一段时间,看了很多大佬的面经,学习了很多,国内面试套路跟美国不大一样,写点东西也回馈下牛友

    牛客网
  • NOIP学习的进阶大约需要的几个阶段。

    C++入门后,我们就需要狂做题,训练程序思想。简单的说,数学思想是你会做题,程序思想是你得讲清楚去让别人做题,这个区别还是很大的。这个时候很多程序设计的思想不断...

    用户5325900
  • SpringBoot【静态资源】

      SpringBoot中的静态资源的存放路径和我们前面的web项目还是有些区别的,本文我们来介绍下SpringBoot中的静态资源。   springboo...

    用户4919348
  • 为什么在招聘网站上看到招聘JAVA的很多,而C语言和C++相对很少?

    这个标准是国内的标准看,如果放在全球的范围内这三种编程语言还依然还是排名前三名,但现在编程语言有个大趋势已经不可逆了,就是集成化编程语言的流行,符合这种特质的j...

    程序员互动联盟
  • Java 学习笔记(4)——面向对象

    现在一般的语言都支持面向对象,而java更是将其做到很过分的地步,java是强制使用面向对象的写法,简单的写一个Hello Word都必须使用面向对象,这也是当...

    Masimaro
  • 人人都应该懂Python的时代,再不学习就晚了

    Python是一门计算机编程语言,类似Java,PHP,C++等编程语言。Python本身面向对象语言,具有丰富和强大的库,轻松地使用C语言、C++、Cytho...

    腾讯NEXT学位
  • cssjshtml css之display:inline-block布局

    1.解释一下display的几个常用的属性值,inline , block, inline-block

    葫芦
  • golang基础之初识

    很久以前,有一个IT公司,这公司有个传统,允许员工拥有20%自由时间来开发实验性项目。在2007的某一天,公司的几个大牛,正在用c++开发一些比较繁琐但是核心的...

    程序员同行者
  • Python在手,天下我有!

    说来也奇怪,N妹周围互联网同事最近纷纷开始学Python,N妹没跟上这波步伐,感觉就要被时代抛弃了?

    腾讯NEXT学位

扫码关注云+社区

领取腾讯云代金券