SLAM小白日记——g2o学习笔记

最新推荐文章于 2022-08-14 22:43:34 发布

每天都在努力学习SLAM的小黑

最新推荐文章于 2022-08-14 22:43:34 发布

阅读量607

点赞数 5

本文链接：https://blog.csdn.net/weixin_45626706/article/details/104295732

版权

初学SLAM，因为ubuntu中不能使用onenote，所以决定用CSDN来记录自己的学习笔记，发表的目的是想和大家一起分享交流进步，笔记内容多来自各大佬的心得，我也会附上链接，如果有侵权烦请告知。

在这里插入图片描述

g2o的创建步骤主要有：

1、创建一个线性求解器LinearSolver

我们要求的增量的方程的形式是： $H △ X = - b$ ，通常情况下想到的方法就是直接求逆，也就是 $X=-H^{-1}*b$ 。但是当 $H$ 的维度较大时，矩阵求逆变得很困难，此时我们就需要一些特殊的方法对矩阵进行求逆。

LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen：依赖项只有eigen，使用eigen中sparse Cholesky 求解，性能和CSparse差不多。继承自LinearSolver

2、创建BlockSolver。并用上面定义的线性求解器初始化

BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内：g2o/g2o/core/block_solver.h

BlockSolver有两种定义方式

一种是指定的固定变量的solver，定义如下:
其中 $p$ 代表pose的维度（注意一定是流形manifold下的最小表示）， $l$ 表示landmark的维度

	 using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

另一种是可变尺寸的solver，定义如下:

	using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

block_solver.h的最后，预定义了比较常用的几种类型，如下所示：

BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2：表示pose 是3维，观测点是2维

3、创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

在g2o/g2o/core/ 目录下，Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下图所示，也和前面的图相匹配在这里插入图片描述
而GN、 LM、 Doglet算法内部，都继承自同一个类：OptimizationWithHessian。

而 OptimizationAlgorithmWithHessian，又继承自OptimizationAlgorithm。

总之，在该阶段，我们可以选则三种方法：

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg

4、创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法。

创建稀疏优化器

	g2o::SparseOptimizer  optimizer;

用前面定义好的求解器作为求解方法：

	SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是设置优化过程输出信息用的

	SparseOptimizer::setVerbose(bool verbose)

5、定义图的顶点和边。并添加到SparseOptimizer中

在这里插入图片描述
先来看看上图中和vertex有关的第①个类： HyperGraph::Vertex，它在路径为g2o/core/hyper_graph.h

这个 HyperGraph::Vertex 是个abstract vertex，必须通过派生来使用。如下图所示
在这里插入图片描述
然后我们看g2o 类结构图中第②个类，我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的，而OptimizableGraph的定义在g2o/core/optimizable_graph.h

我们找到vertex定义，发现果然，OptimizableGraph 继承自 HyperGraph，如下图所示
在这里插入图片描述
不过，这个OptimizableGraph::Vertex 也非常底层，具体使用时一般都会进行扩展，因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中对应的第③个类：BaseVertex<D, T>

它的路径为：g2o/core/base_vertex.h

在这里插入图片描述

D是int 类型的，表示vertex的最小维度，比如3D空间中旋转是3维的，那么这里 D = 3

T是待估计vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion 类型

如何自己定义顶点？

g2o本身内部定义了一些常用的顶点类型:

	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顶点的内参数化与外参数化及其指数映射
	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为正值，否则qx，qy，qz中存在一个不明确的旋转
	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>

我们也可以自己定义顶点,前面的名字可以由你自己定义的,主要是后面的父类public BaseVertex<维度,类型>

重新定义顶点一般需要考虑重写如下函数：

	virtual bool read(std::istream& is);
	virtual bool write(std::ostream& os) const;
	virtual void oplusImpl(const number_t* update);
	virtual void setToOriginImpl();

read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
setToOriginImpl：顶点重置函数，设定被优化变量的原始值。
oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

先看一个简单例子，来自十四讲中的曲线拟合，来源如下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 {}
};

代码中顶点初值设置为0，更新时由 $x + △ x$ 直接把更新量 update 加上去的（因为顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新）

但是有些例子就不行，比如下面这个复杂点例子：李代数表示位姿VertexSE3Expmap

来自g2o官网g2o/types/sba/types_six_dof_expmap.h

//brief SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
	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());		// 李代数左乘微小扰动更新
  }
};

这个里面的6, SE3Quat 分别为：

第一个参数6 表示内部存储的优化变量维度，这是个6维的李代数
第二个参数是优化变量的类型，这里使用了g2o定义的相机位姿类型：SE3Quat。

在这里可以具体查看g2o/types/slam3d/se3quat.h

它内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作

如何向图中添加顶点？

往图中增加顶点比较简单，我们还是先看看第一个曲线拟合的例子:
setEstimate(type) 函数来设定初始值；setId(int) 定义节点编号

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    // 设定初始值
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    // 设定顶点的编号
    v->setId(0);
    optimizer.addVertex( v );

这个是添加 VertexSBAPointXYZ 的例子，都很容易看懂/ch7/pose_estimation_3d2d.cpp

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); 
        optimizer.addVertex ( point );
    }

初步认识g2o的边

与边有关的头文件：
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。
一元边可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，多元边理解为一条边可以连接多个（3个以上）顶点
在这里插入图片描述
它们的主要参数为：D, E, VertexXi, VertexXj，他们的分别代表：

D 是 int 型，表示测量值的维度（dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型

比如我们用边表示三维点投影到图像平面的重投影误差，就可以设置输入参数如下：

	 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

2:是说测量值是2维的，也就是图像像素坐标x,y的差值
Vector2D: 边对应测量值的类型是Vector2D
VertexSBAPointXYZ, VertexSE3Expmap:两个顶点(优化变量)分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap

定义边我们通常需要复写一些重要的成员函数，边主要有以下几个重要的成员函数

	virtual bool read(std::istream& is);
	virtual bool write(std::ostream& os) const;
	virtual void computeError();
	virtual void linearizeOplus();

下面简单解释一下

read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian
除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

	_measurement：存储观测值
	_error：存储computeError() 函数计算的误差
	_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
	setId(int)：来定义边的编号（决定了在H矩阵中的位置）
	setMeasurement(type) 函数来定义观测值
	setVertex(int, vertex) 来定义顶点
	setInformation() 来定义协方差矩阵的逆

如何自定义g2o的边？

基本上定义g2o中的边，就是如下套路：

class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
      myEdge(){}     
      virtual bool read(istream& in) {}
      virtual bool write(ostream& out) const {}      
      virtual void computeError() override
      {
          // ...
          _error = _measurement - Something;
      }      
      virtual void linearizeOplus() override
      {
          _jacobianOplusXi(pos, pos) = something;
          // ...         
          /*
          _jocobianOplusXj(pos, pos) = something;
          ...
          */
      }      
      private:
      // data
  }

我们可以发现，最重要的就是computeError()，linearizeOplus()两个函数了.

我们先来看一个简单例子，地址在https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp
这个是个一元边，主要是定义误差函数了，如下所示，你可以发现这个例子基本就是上面例子的一丢丢扩展

// 误差模型 模板参数：几元边<观测值维度，类型，连接顶点类型>
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // 初始化
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

下面是一个复杂一点例子，3D-2D点的PnP 问题，也就是最小化重投影误差问题，这个问题非常常见，使用最常见的二元边，弄懂了这个基本跟边相关的代码也差不多都一通百通了

//继承了BaseBinaryEdge类，二元边<观测值是2维，类型Vector2D,顶点分别是三维点、李群位姿>
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    //1. 默认初始化
    EdgeProjectXYZ2UV();
    //2. 计算误差
    void computeError()  {
      //李群相机位姿v1 ,在_vertices存储的顶点编号为1
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      // 顶点v2,在_vertices存储的顶点编号为0
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      //相机参数
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
     //误差计算，测量值减去估计值，也就是重投影误差obs-cam
     //估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。
      Vector2D obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
      //误差 = 观测 - 投影
    }
    //3. 线性增量函数，也就是雅克比矩阵J的计算方法
    virtual void linearizeOplus();
    //4. 相机参数
    CameraParameters * _cam; 
    bool read(std::istream& is);
    bool write(std::ostream& os) const;
};

补充:如何定义多元边:

class G2O_CORE_API EdgeSE3MultiCamProjectXYZ : public  BaseMultiEdge<2, Vector2d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3MultiCamProjectXYZ() {
        resize(3);
    };

    Vector2d panoramic_project(const Vector3d &trans_xyz) const
    {
        float  theta,phi;
        theta = atan2(trans_xyz(2),trans_xyz(0));
        if(theta<-PI/2) theta = 2*PI+theta;
        phi = atan(-cos(theta)*trans_xyz(1)/trans_xyz(0));
        Vector2d res;
        res[0]  =  mcoef*(-theta + PI);
        res[1]  =  mcoef*(-phi + PI/2.0)-IMAGELOWERBOUND;
        return res;
    }

    void computeError(){
        const VertexSE3Expmap* v0 = static_cast<const VertexSE3Expmap *> (_vertices[0]);
        const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap *> (_vertices[1]);
        const VertexSBAPointXYZ* V2 = static_cast<const  VertexSBAPointXYZ *> (_vertices[2]);
        Vector2d obs(_measurement);
        _error = obs- panoramic_project(v1->estimate().map( v0->estimate().map(V2->estimate())));
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}

private:
    int IMAGEWIDTH= 640;
    float IMAGELOWERBOUND =130;
    float mcoef = (IMAGEWIDTH*1.0)/PI;
};

如何向图中添加边？

我们还是先从最简单的例子说起：一元边的添加方法

下面代码来自GitHub上，仍然是前面曲线拟合的例子
slambook/ch6/g2o_curve_fitting/main.cpp

// 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        // 新建边
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);							// 设置边的id
        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 );              // 把边添加到优化器中
    }

对于这个曲线拟合，观测值就是实际观测到的数据点。对于视觉SLAM来说，通常就是我们我们观测到的特征点坐标。

下面一个例子比这个的复杂一点，因为它是二元边，需要用边连接两个顶点。
代码来自GitHub上slambook/ch7/pose_estimation_3d2d.cpp

index = 1;
    for ( const Point2f p:points_2d )
    {
    	// 新建边
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        // 设置边的id
        edge->setId ( index );
        // 设置连接的顶点
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        // 向边中添加观测数据
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        // 信息矩阵,用作权重
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        // 把边添加到优化器中
        optimizer.addEdge ( edge );
        index++;
    }

setMeasurement函数里的p来自向量points_2d，也就是特征点的图像坐标(x,y)。
_vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点，也就是三维点，_vertices[1] 对应的是VertexSE3Expmap 类型的顶点，也就是位姿pose

6、设置优化参数，开始执行优化。

设置SparseOptimizer的初始化、迭代次数、保存结果等。

初始化

	SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数，然后就开始执行图优化了。

	SparseOptimizer::optimize(int iterations, bool online)

——————————————————————————————

代码总览

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

	// 第1步：创建一个线性求解器LinearSolver
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 

	// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
	Block* solver_ptr = new Block( linearSolver );      

	// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

	// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm( solver );   // 设置求解器
	optimizer.setVerbose( true );       // 打开调试输出

	// 第5步：定义图的顶点和边。并添加到SparseOptimizer中
	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 );                // 设置连接的顶点
 	  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);

参考资料：高翔《视觉SLAM十四讲》
https://blog.csdn.net/electech6/article/details/88018481
https://www.cnblogs.com/CV-life/p/10286037.html
https://www.cnblogs.com/CV-life/archive/2019/03/13/10525579.html

每天都在努力学习SLAM的小黑

关注

5
点赞
踩
11

收藏

觉得还不错? 一键收藏
1
评论
SLAM小白日记——g2o学习笔记

初学SLAM，因为ubuntu中不能使用onenote，所以决定用CSDN来记录自己的学习笔记，发表的目的是想和大家一起分享交流进步，笔记内容多来自各大佬的心得，我也会附上链接，如果有侵权烦请告知。1、创建一个线性求解器LinearSolver我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H^-1*b。但是当H的维度较大时，矩阵求逆变得很困难，此时...
复制链接

扫一扫