g2o中的顶点

g2o中的顶点

自定义顶点

注意误差模型的雅克比矩阵的求导方式，和顶点的更新方式的关系，比如SE3，Sim3中的左右扰动模型，需要和顶点的更新方式保持一致。

//重新定义边和点
//顶点：继承自。。参数：优化变量维度、数据类型
class MyVertex :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){   //更新顶点的数组 X = X + dx
        _estimate+=Eigen::Vector3d(update);
    }

    virtual  bool  read(istream &in){}
    virtual  bool  write(ostream &out) const{}
};