G2O的使用1-——构建一个简单的优化器

学习了一下G2O的使用，练习一个简单优化器来拟合曲线，参考了slam十四讲和g2o的一些例程，首先分别创建一个边和顶点类

//第一个参数3表示update指针指向的数组大小，Eigen::Vector3d为_estimate的类型
class CurveFittingVetex: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 {}
};
//1为_error的大小，double为观测值_measurement的大小，CurveFittingVetex为一元边上的一个顶点
class EdgeCurveFitting:public g2o::BaseUnaryEdge<1,double,CurveFittingVetex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeCurveFitting(double x):BaseUnaryEdge(),_x(x){}
    virtual void computeError()
    {
        CurveFittingVetex* v=static_cast(_vertices[0]);
        Eigen::Vector3d estimate=v->estimate();
        _error(0)=_measurement-std::exp(estimate(0)*_x*_x+estimate(1)*_x+estimate(2));
    }
    virtual bool read(istream& in){}
    virtual bool write(ostream &os) const{}
public:
    double _x;
};

//创建一个优化器，线建立一个优化器生成器，然后按照指定的优化器名生成对应的solver,可以指定lm_var,gn_var,dl_var等等多种类型。

g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
  g2o::OptimizationAlgorithmProperty solver_property;
  g2o::OptimizationAlgorithm* solver = solver_factory->construct("dl_var", solver_property);
  std::cout<<"name"<listSolvers(std::cerr);

然后添加边和顶点。全部代码见下：

#include 
#include 
#include 
#include
#include"vetex_curve_fitting.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/sparse_optimizer.h"
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "g2o/types/sba/types_six_dof_expmap.h"
#include 
using namespace std;
G2O_USE_OPTIMIZATION_LIBRARY(csparse)

class CurveFittingVetex: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 {}
};

class EdgeCurveFitting:public g2o::BaseUnaryEdge<1,double,CurveFittingVetex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeCurveFitting(double x):BaseUnaryEdge(),_x(x){}
    virtual void computeError()
    {
        CurveFittingVetex* v=static_cast(_vertices[0]);
        Eigen::Vector3d estimate=v->estimate();
        _error(0)=_measurement-std::exp(estimate(0)*_x*_x+estimate(1)*_x+estimate(2));
    }
    virtual bool read(istream& in){}
    virtual bool write(ostream &os) const{}
public:
    double _x;
};
int main()
{
  double a=1,b=2,c=1;
  int N=100;
  double sigma=1;
  cv::RNG rng;
  vector x(N),y(N);
  for(int i=0;iconstruct("dl_var", solver_property);
  std::cout<<"name"<listSolvers(std::cerr);

  // some handy typedefs

  std::unique_ptr optimazer;
  optimazer.reset(new g2o::SparseOptimizer);
  optimazer->setAlgorithm(solver);
  optimazer->setVerbose(true);
  CurveFittingVetex* vetex1(new CurveFittingVetex);
  vetex1->setEstimate(Eigen::Vector3d(0,0,0));
  vetex1->setId(0);
  optimazer->addVertex(vetex1);
  //optimazer.addVertex(vetex1);
  for(int i=0;isetId(i);
      edge->setMeasurement(y[i]);
      edge->setVertex(0,vetex1);
      edge->setInformation(Eigen::Matrix::Identity()*1/(sigma*sigma));
      optimazer->addEdge(edge);
  }
  optimazer->initializeOptimization();
  optimazer->optimize(100);
  Eigen::Vector3d e=vetex1->estimate();
  std::cout<<"estimate"<