G2o的顶点与边属于 HyperGraph 抽象类的继承OptimizableGraph 的继承。
BaseVertex<D,T> D是预测点的维度（在流形空间的最小表示）T是待估计vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion 类型

// 顶点的定义
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);  // 输入有数据返回True
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

• read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

• setToOriginImpl：顶点重置函数，设定被优化变量的原始值。

• oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

// 顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual 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 {}
};

/**\* \brief SE3 Vertex parameterized internally with a transformation matrixand externally with its exponential map*/class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();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());        //更新方式}
};

SE3Quat是相机位姿类型，它内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作，相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，这是因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示（使用罗德里格斯公式），就可以把位姿估计变成无约束的优化问题，求解难度降低。

添加顶点

CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );

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