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