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<CurveFittingVetex*>(_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"<<solver_property.name<<std::endl;
  std::cout<<"desc"<<solver_property.desc<<std::endl;
  std::cout<<"type"<<solver_property.type<<std::endl;
  std::cout<<"landmarkDim"<<solver_property.landmarkDim<<std::endl;
  std::cout<<"poseDim"<<solver_property.poseDim<<std::endl;
  std::cout<<"margina"<<solver_property.requiresMarginalize<<std::endl;
  solver_factory->listSolvers(std::cerr);

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

#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include<math.h>
#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 <g2o/stuff/macros.h>
#include <g2o/core/factory.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/linear_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d/edge_se3_pointxyz.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include "g2o/types/sba/types_six_dof_expmap.h"
#include <iostream>
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<CurveFittingVetex*>(_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<double> x(N),y(N);
  for(int i=0;i<N;i++)
  {
      x[i]=(double)i/100;
      y[i]=std::exp(a*x[i]*x[i]+b*x[i]+c)+rng.gaussian(sigma);
      std::cout<<x[i]<<" "<<y[i]<<std::endl;
  }

  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();

  g2o::OptimizationAlgorithmProperty solver_property;
  g2o::OptimizationAlgorithm* solver = solver_factory->construct("dl_var", solver_property);
  std::cout<<"name"<<solver_property.name<<std::endl;
  std::cout<<"desc"<<solver_property.desc<<std::endl;
  std::cout<<"type"<<solver_property.type<<std::endl;
  std::cout<<"landmarkDim"<<solver_property.landmarkDim<<std::endl;
  std::cout<<"poseDim"<<solver_property.poseDim<<std::endl;
  std::cout<<"margina"<<solver_property.requiresMarginalize<<std::endl;
  solver_factory->listSolvers(std::cerr);

  // some handy typedefs

  std::unique_ptr<g2o::SparseOptimizer> 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;i<N;i++)
  {
      EdgeCurveFitting *edge=new EdgeCurveFitting(x[i]);
      edge->setId(i);
      edge->setMeasurement(y[i]);
      edge->setVertex(0,vetex1);
      edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(sigma*sigma));
      optimazer->addEdge(edge);
  }
  optimazer->initializeOptimization();
  optimazer->optimize(100);
  Eigen::Vector3d e=vetex1->estimate();
  std::cout<<"estimate"<<e.transpose()<<std::endl;
  return 0;
}