g2o使用详情

g2o使用详情

转载:
1、计算机视觉life | 从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码
2、计算机视觉life |从零开始一起学习SLAM | 掌握g2o顶点编程套路
3、计算机视觉life |从零开始一起学习SLAM | 掌握g2o边的代码套路
4、CSDN | g2o使用总结

g2o使用方便记忆的话可以大致分为以下两步:

		 1. 配置优化算法和求解器
		 2. 配置顶点和边

详细步骤用图表示如下:
在这里插入图片描述
步骤演示代码案例,求解曲线参数 :SLAM十四讲 /ch6/g2o_curve_fitting/main.cpp

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3,误差值维度为1

// 第1步:创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 
	/*	除了使用  LinearSolverDense (使用dense cholesky分解法。继承自LinearSolver)之外还有如下选择
		LinearSolverCholmod :使用sparse cholesky分解法。继承自LinearSolverCCS
		LinearSolverCSparse:使用CSparse法。继承自LinearSolverCCS
		LinearSolverPCG :使用preconditioned conjugate gradient 法,继承自LinearSolver
		LinearSolverEigen: 依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多。继承自LinearSolver
	*/
// 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );      

// 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
	/*	总共有3种可选:
		g2o::OptimizationAlgorithmGaussNewton
		g2o::OptimizationAlgorithmLevenberg 
		g2o::OptimizationAlgorithmDogleg 
	*/
// 第4步:创建终极大boss 稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 设置求解器
optimizer.setVerbose( true );       // 打开调试输出

// 第5步:定义图的顶点和边。并添加到SparseOptimizer中
	//往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();  //此处是自定义的顶点类型,非g2o自带
v->setEstimate( Eigen::Vector3d(0,0,0) );  //设置待优化位姿
v->setId(0);                               //设置Id号
optimizer.addVertex( v );
	// 往图中增加边
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) ); // 信息矩阵:协方差矩阵之逆
  optimizer.addEdge( edge );
}

// 第6步:设置优化参数,开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);

g2o本身定义可直接使用对的顶点:

/*
 以下所有数据类型均继承自 g2o::BaseVertex<Dim, Type> 模板类
 Dim含义:是int 类型的,表示vertex的最小维度,比如3D空间中旋转是3维的,那么这里 D = 3
 Type:  是待估计vertex的数据类型,比如用四元数表达三维旋转的话,T就是Quaternion 类型
*/
VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>

如果g2o自带的顶点类型不能满足需要,可自定义顶点类型,重新定义顶点一般需要考虑重写如下函数:

virtual bool read(std::istream& is); //读盘函数,一般只申明即可
virtual bool write(std::ostream& os) const;  //写盘函数,一般只申明即可
virtual void setToOriginImpl(); //顶点重置函数,设定被优化变量的原始值。
virtual void oplusImpl(const number_t* update); //顶点更新函数。非常重要的一个函数,主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后,就是通过这个函数对估计值进行调整的,因此这个函数的内容一定要重视。

自定义顶点例子

//例子 1: ch6/g2o_curve_fitting/main.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 {}
};





//例子2:g2o/types/sba/types_six_dof_expmap.h

class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexSE3Expmap();
  bool read(std::istream& is);
  bool write(std::ostream& os) const;
  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());        //更新方式
  }
};

边的详细使用参见转载 3、4

展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 深蓝海洋 设计师: CSDN官方博客
应支付0元
点击重新获取
扫码支付

支付成功即可阅读