g2o使用总结关于边和顶点

图优化是非线性最小二乘问题的另一种表达而已，使用g2o求解和Ceres很多地方都相似。

最小二乘问题的关键在于：已知观测数据（输入值，观测值），待估计参数及其初始值，误差项，雅克比矩阵，求解增量更新待估计参数，在不同的方法中去寻找对应，对于方法的理解即可迎刃而解。

定义顶点和边的类
1、构建顶点Vertex类：
顶点就是待优化的参数，重要的成员函数有：
setToOriginImpl() 将节点的值进行重置
oplusImpl（）表达顶点的更新（待优化变量的更新）

2、构建边Edge类:
边就是误差项，重要的函数有：
构造函数：用于接收已知数据的输入值，从而在computeError() 中书写
computeError() 计算误差项（其中_measurement用于接收观测值）
linearizeOplus() 求解雅克比矩阵

添加顶点和边（其他构建图优化问题部分略，重点展示添加）

1、添加顶点：
创建顶点（指针）访问成员函数：
setEstimate(type) ：设定初始值（迭代初始值）
setId(int) ：定义顶点的节点编号

optimizer.addVertex( )：添加顶点

2、添加边：（注意边一般添加很多个，因为已知数据有很多组）

通过构造函数创建边（指针）访问成员函数，（同时传入了已知数据的输入值）：
setId(i)：定义边的编号，决定在H矩阵中的位置
setVertex(int, vertex)：将顶点通过编号传入_vertices[]储存，并用于Class边中计算误差
setMeasurement（）传入观测值
setInformation（）信息矩阵

optimizer.addEdge( edge )：添加边

例子
slambook2/ch6/ceresCurveFitting.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 {}
    
//添加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
};

边：

//创建边类
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>//误差项维度、类型、顶点类型
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

//添加边
  for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );//创建指针并传入已知的输入值
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 传入观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆//identity matrix 信息矩阵
        optimizer.addEdge( edge );
    }