一、前言
在上一篇博客中，讲解了运行该示例代码环境的搭建，同时对 g2o 进行了简单的讲解。其实对于g2o(图优化)来说，只要理解了顶点(Vertex) 与边(Edge) ，那么接下下来的构图以及编程都是十分的简单，所以在进行代码讲解之前，先来看看 顶点 (Vertex) 与 边(Edge) 的介绍。该篇文章主要介绍顶点 Vertex，Edge 下篇文章中进行介绍。 主要分为以下几个部分：1、g2o的顶点（Vertex) 从哪里来的；2、g2o的顶点（Vertex) 参数如何理解；3、如何自己定义顶点；4、如何向图中添加顶点。这里先粘贴一下类的结构图：

二、g2o的顶点（Vertex) 从哪里来的
在源码 https://github.com/RainerKuemmerle/g2o 的 doc 目录中，可以看到一下 g2o.pdf 文件，该为教材文件，想深入了解的朋友可以认真查看。其上，上图也是来自于该文档。我们先来看其中关于顶点的这一部分，如下：

首先看到上面的类①HyperGraph::Vertex，其位于源码的 g2o/core/hyper_graph.h 文件中：

  class G2O_CORE_API HyperGraph
  {
    public:
        ......
        ......
        //! abstract Vertex, your types must derive from that one
        class G2O_CORE_API Vertex : public HyperGraphElement {
            ......
            ......
        }; 

上的的代码表示 class HyperGraph 包含了 class Vertex ，并且 Vertex 继承于 HyperGraphElement。另外通过注释，可以知道 class Vertex 是一个抽象类，上图②OptimizableGraph::Vertex 由其派生而来，或者说 ②OptimizableGraph::Vertex 继承于 HyperGraph::Vertex。如下所示，在 core/optimizable_graph.h 文件中，可以看到如下代码：

struct G2O_CORE_API OptimizableGraph : public HyperGraph {
    ......
    ......
    /**
     * \brief A general case Vertex for optimization
     */
    class G2O_CORE_API Vertex : public HyperGraph::Vertex, public HyperGraph::DataContainer {
        ......
        ......

这里很明显可以看出来 OptimizableGraph::Vertex 继承于 HyperGraph::Vertex。不过，这个OptimizableGraph::Vertex 是比较底层的类，具体使用时一般会进行扩展，因此g2o中提供了一个比较通用的适合大部分情况的模板。也就是上图中的③BaseVertex<d, t="">，其在 g2o/core/base_vertex.h 中实现，代码如下：

/**
* \brief Templatized BaseVertex
*
* Templatized BaseVertex
* D  : minimal dimension of the vertex, e.g., 3 for rotation in 3D
* T  : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
*/
template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
    public:
    typedef T EstimateType;
    typedef std::stack<EstimateType, 
                       std::vector<EstimateType,  Eigen::aligned_allocator<EstimateType> > >
    BackupStackType;

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

这样就弄清楚了 Vertex 的来源，但是这个模板类 BaseVertex 如何理解呢？

三、g2o的顶点（Vertex) 参数如何理解？
因为常用的就是 class BaseVertex，所以我们对其进行讲解，其模板的参数分别为D 和 T，

1、D是int 类型的，表示vertex的最小维度，比如3D空间中旋转是3维的，那么这里 D = 3
2、T是待估计vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion 类型

class BaseVertex 源码中 D 是这样进行注释的：

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

可以看到这个D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示，这里一定要区别开，另外，源码里面也给出了T的作用

typedef T EstimateType;
typedef std::stack<EstimateType, 

可以看到，这里T就是顶点（状态变量）的类型，跟前面一样。

四、g2o的顶点（Vertex) 参数如何理解？
通过上面的了解，就可以写顶点的定义了，已知顶点的基本类型是 BaseVertex，那么下一步关心的就是如何使用了，因为在不同的应用场景(二维空间，三维空间)，有不同的待优化变量(位姿，空间点)，还涉及不同的优化类型(李代数位姿、李群位姿)。有这么多，是不是都要根据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();

每次定义都要重写这几个函数，来看一下他们都是什么意义：

(1)read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
(2)setToOriginImpl： 顶点重置函数，设定被优化变量的原始值。
(3)oplusImpl： 顶点更新函数。非常重要的一个函数，主要用于优化过程中增量 △ x △x△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*/;
      }
  } 

先来看一个简单的例子，来自于
https://github.com/gaoxiang12/slambook2/blob/master/ch6/g2oCurveFitting.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 {}
};

该为曲线类型的顶点(后续有详细的代码讲解)，模板参数→待优化变量的维度，以及数据类型。根据 _estimate ‹‹ 0,0,0; 可以知道，顶点初始值为0，更新逻辑 _estimate += Eigen::Vector3d(update); 也比较简单，就是把更新量 update 加上去就行了。

对于这里例子来说是可以直接相加的，因为顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新的。但是但是有些例子就不行，比如下面这个复杂点例子，如李代数表示位姿VertexSE3Expmap，来自于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 分别是什么意思？其实在 slam十四讲 中都有提及到，书中是这样介绍的：

第一个参数 6 → \color{blue} 第一个参数6→第一个参数6→ 表示内部存储的优化变量维度，这是个6维的李代数
第二个参数 → \color{blue} 第二个参数→第二个参数→ 表示优化变量的类型，这里使用了g2o定义的相机位姿类型：SE3Quat(三维旋转，三维平移)

在g2o/types/slam3d/se3quat.h可以具体了解SE3Quat()，其内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作。那么这里更新时没有为什么像上面那样直接加上去？

对于位姿来说，是不能直接加的，原因是变换矩阵不满足加法封闭。为什么相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，而不是使用旋转矩阵和平移矩阵？其实也是上述原因的拓展：这是因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示，就可以把位姿估计变成无约束的优化问题，求解难度降低。继续来看一个例子：

 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;
    }
}; 

下面是三维点的例子，空间点位置 VertexPointXYZ，维度为3，类型是Eigen的Vector3，比较简单，就不解释了。

五、如何向图中添加顶点？
理解顶点 (Vertex)之后，对于把顶点 (Vertex)添加到图中是比较简单的，如 ch6/g2oCurveFitting.cpp 中的代码如下：

 // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v ); 

其中的setId(int) 定义节点编号，这个主要是后续获取数据，以及顶点与边连接时需要使用到。下面是添加 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 );
    } 

六、结语
通过该篇博客，对于1、g2o的顶点（Vertex) 从哪里来的；2、g2o的顶点（Vertex) 参数如何理解；3、如何自己定义顶点；4、如何向图中添加顶点。四个环节已经讲解完成。下面来看看边(Edge)应该如何去理解与使用。