G2O：顶点

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

vertex有关的第①个类： HyperGraph::Vertex，它在这个路径
g2o/core/hyper_graph.h
这个 HyperGraph::Vertex 是个abstract vertex，必须通过派生来使用。

第②个类：HyperGraph::Vertex 是通过类OptimizableGraph 来继承的， 而OptimizableGraph的定义在
g2o/core/optimizable_graph.h
我们找到vertex定义，发现果然，OptimizableGraph 继承自 HyperGraph

第③个类：**BaseVertex<D, T>**那么它在哪里呢？ 在这个路径：
g2o/core/base_vertex.h

g2o的顶点（Vertex) 参数如何理解？

//模板参数 D 和 T
static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
typedef T EstimateType;
EstimateType _estimate;

D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示
T就是顶点（状态变量）的类型

如何自己定义顶点

因为在不同的应用场景（二维空间，三维空间），有不同的待优化变量（位姿，空间点），还涉及不同的优化类型（李代数位姿、李群位姿）
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：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
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();  //举例：_estimate << 0,0,0;
      }
      virtual void oplusImpl(const double* update) override
      {
          _estimate += /*update*/;  //**
      }
  }

**：因为顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新的。但是但是有些例子就不行，比如下面这个复杂点例子：李代数表示位姿VertexSE3Expmap

virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());		//更新方式
  }

如何向图中添加顶点

1. 初始化一个顶点
2. 给该顶点添加ID：setId（）
3. 给该顶点添加估计值：setEstimate（）
4. 设置该顶点是否被边缘化：setMarginalized （）
5. 增加该顶点到优化器：optimizer.addVertex （）

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