g2o入门：曲线拟合代码分析

今天第三边跟着slam十四讲敲了g2o的曲线拟合代码，对g2o的认识也更进一步
首先，附上参考网址，最通俗易懂的教程没有之一：http://www.cnblogs.com/CV-life/p/10286037.html
这是ros上关于g2o库的文档，和g2o差别不大：http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html

进入正题，学习如何使用g2o库，重点是理解下面这张图：
在这里插入图片描述注意里面的箭头，实线表示类的继承关系，一般我们只用最上层的类，虚线表示箭头所指的东西属于箭头尾部的东西的对象（即我们可以用前者去初始化后者），编程时，由底部向顶部开始构造求解器：先构造线性求解器，再构造优化算法，最后构造优化器
附上代码：

#include<iostream>
#include<g2o/core/base_vertex.h>
#include<g2o/core/base_unary_edge.h>
#include<g2o/core/block_solver.h>
#include<g2o/core/optimization_algorithm_levenberg.h>
#include<g2o/core/optimization_algorithm_gauss_newton.h>
#include<g2o/core/optimization_algorithm_dogleg.h>
#include<g2o/solvers/dense/linear_solver_dense.h>
#include<Eigen/Core>
#include<opencv2/core/core.hpp>
#include<cmath>
#include<chrono>
using namespace std;

class CurveFittingVertex:public g2o::BaseVertex<3,Eigen::Vector3d>
{
public:
 // EIGEN_MAKE_ALIGEND_OPERATOR_NEW
  virtual void setToOriginImpl()
  {
    _estimate<<0,0,0;
  }
  
  virtual void oplusImpl(const double*update)
  {
    _estimate=_estimate+Eigen::Vector3d(update);
  }
  
virtual bool read(istream& in){}
virtual bool write(ostream& out) const {}
};

class CurveFittingEdge:public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>{
public:
  //EIGEN_MAKE_ALIGEND_OPERATOR_NEW
  //构造函数：
  CurveFittingEdge(double x):BaseUnaryEdge(),_x(x) {}
  
  //计算误差
  void computeError()
  {
    const CurveFittingVertex *v=static_cast<CurveFittingVertex*>(_vertices[0]); //初始化节点
    //typedef std::vector<Vextex*> VertexVector
    //VertexVector _vertices
    
    const Eigen::Vector3d abc=v->estimate(); //得到估计量
    _error(0,0)=_measurement-std::exp(abc(0,0)*_x*_x+abc(1,0)*_x+abc(2,0)); //y值为_measurement
     //typedef Eigen::Matrix<double, D, 1, Eigen::ColMajor> ErrorVector;
    //ErrorVector _error;
  }
  virtual bool read(istream& in) {}
  virtual bool write(ostream& out) const{}
public:
  double _x;  //_x用来传入估计点的x值
};

int main(int argc,char** argv)
{
  double a=1,b=2,c=3;
  int N=100;
  double w_sigma=1.0;
  cv::RNG rng;
  double abc[3]={0,0,0};
  
  vector<double> x_data,y_data;
  //产生观测数据
  for(int i=0;i<N;i++)
  {
    double x=i/100.0;
    x_data.push_back(x);
    double y=rng.gaussian(w_sigma)+std::exp(a*x*x+b*x+c);
    y_data.push_back(y);
  }
    //矩阵块：优化变量维度为3,误差值维度为1
   typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>> Block;
   
     //初始化线性求解器
   Block::LinearSolverType *linearSolver= new g2o::LinearSolverDense<Block::PoseMatrixType>();
   Block* solver_ptr=new Block(linearSolver);
   
   g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
   g2o::SparseOptimizer optimizer;
   optimizer.setAlgorithm(solver);
    //g2o::OptimizationAlgorithmLevenberg solver(solver_ptr);
    //optimizer.setAlgorithm(&solver);
   optimizer.setVerbose(true);
   
   //往图中增加顶点
   CurveFittingVertex *v=new CurveFittingVertex();
   v->setEstimate(Eigen::Vector3d(0,0,0));
   v->setId(0);
   optimizer.addVertex(v);
   
   //往图中增加边
   for(int i=0;i<N;i++)
   {
     CurveFittingEdge *edge=new CurveFittingEdge(x_data[i]);
     edge->setId(i);
     edge->setVertex(0,v);
     //void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); 
//_vertices[i]=v;}
     edge->setMeasurement(y_data[i]);
     //信息矩阵：协方差矩阵之逆
     edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
     optimizer.addEdge(edge);
   }
//执行优化
std::cout<<"start optimization"<<std::endl;
optimizer.initializeOptimization();
optimizer.optimize(100);
Eigen::Vector3d abc_estimate=v->estimate();
cout<<abc_estimate.transpose()<<endl;
return 0;
}

g2o入门：曲线拟合代码分析

猜你喜欢