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

点“计算机视觉life”关注，置顶更快接收消息！

小白：师兄，上一次将的g2o框架《》真的很清晰，我现在再去看g2o的那些优化的部分，基本都能看懂了呢！

师兄：那太好啦，以后多练习练习，加深理解

小白：嗯，我开始编程时，发现g2o的顶点和边的定义也非常复杂，光看十四讲里面，就有好几种不同的定义，完全懵圈状态。。。师兄，能否帮我捋捋思路啊

师兄：嗯，你说的没错，入门的时候确实感觉很乱，我最初也是花了些时间才搞懂的，下面分享一下。

g2o的顶点（Vertex) 从哪里来的？

师兄：在《g2o: A general Framework for (Hyper) Graph Optimization》这篇文档里，我们找到那张经典的类结构图。也就是上次讲框架用到的那张结构图。其中涉及到顶点 （vertex） 的就是下面 加了序号的3个东东了。

小白：记得呢，这个图很关键，帮助我理清了很多思路，原来来自这篇文章啊

师兄：对，下面我们一步步来看吧。先来看看上图中和vertex有关的第①个类： HyperGraph::Vertex，在g2o的GitHub上（），它在这个路径

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