G2O框架及编程方法

最新推荐文章于 2024-10-24 18:16:51 发布

YWL0720

最新推荐文章于 2024-10-24 18:16:51 发布

阅读量938

点赞数 2

CC 4.0 BY-SA版权

分类专栏： SLAM 文章标签：算法图论 slam c++

本文链接：https://blog.csdn.net/weixin_44911075/article/details/120906342

SLAM 专栏收录该内容

9 篇文章

订阅专栏

一、G2O的基本框架

在这里插入图片描述

1.图的核心

如图所示，在G2O整个框架中最左侧的SparseOptimizer(稀疏优化器)是最核心的部分，其中SparseOptimizer是一个OptimizableGraph也是一个HyperGraph(超图)

2.顶点和边

最上方的超图(HyperGraph)包含了许多个顶点(HyperGraph::Vertex)和边(HyperGraph::Edge)。实际问题中的顶点继承自BaseVertex也就是OptimizableGraph::Vertex，而边则有三种不同的形式，单边BaseUnaryEdge,双边BaseBinaryEdge或者多边BaseMultiEdge,他们都是OptimizableGraph::Edge。

3.配置SparseOptimizer的优化算法和求解器

核心SparseOptimizer包含一个优化算法对象OptimizationAlgorithm，他是通过OptimizationWithHessian来实现的，其中迭代策略可以从Gauss-Newton,Levenberg-Marquardt和Powell’s dogleg三者中选择。

4.求解

OptimizationWithHessian内部包含一个求解器Solver，他由BlockSolver组成，而BlockSolver有两部分，一个是SparseBlockMatrix用于计算稀疏的雅可比和Hessian矩阵，一个是线性方程求解器LinearSolver，用于计算迭代过程中最关键的一步 $H\Delta x = -b$ ，LinearSolver有几种方法可以选择：PCG,CSparse,Choldmod

二、G2O编程流程

在这里插入图片描述

在实际编程时，框架由底层开始搭起。
给出一个实例源自于高博的十四讲，其中一部分由于G2O更新的缘故，编译时会报错，这里也做了进一步的更改

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(1  00);

三、G2O顶点编程方法

在这里插入图片描述

第一个类HyperGraph::Vertex是一个abstract vertex，必须通过派生来使用。
而第二个类OptimizableGraph::Vertex继承于上一个类HyperGraph::Vertex,这个类也比较底层，因此G2O提供了一个比较通用的适合大部分情况的模版，也就是第三个类BaseVertex<D,T>。
第三个类是一个模版类，接受两个参数D,和T。其中D并非是顶点(更确切的说是状态变量)的维度，而是其在流形空间的最小表示。T指顶点(状态变量)的类型。

1.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 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>

2.自定义顶点的方法

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

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()是顶点更新函数，主要用于优化过程中 $\Delta x$ 的计算，我们根据增量方程计算出增量后，就是通过这个函数对估计值进行调整的。
自定义顶点的格式：

  class myVertex: public g2::BaseVertex<Dim, Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      
      myVertex(){}
      
      virtual void read(std::istream& is) {}
      virtual void write(std::ostream& os) const {}
      
      virtual void setOriginImpl()
      {
          _estimate = Type();
      }
      virtual void oplusImpl(const double* update) override
      {
          _estimate += /*update*/;
      }
  }

实例，来自高博的第六讲

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 {}
};

3.向图中添加顶点

向图中添加顶点主要用到两个函数：
setEstimate()函数，用来设定初始值
setId()定义顶点编号
接着上面的例子：

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

四、G2O边的编程方法

1.G2O中边的类型

边有三种类型，一元边BaseUnaryEdge,二元边BaseBinaryEdge,多云边BaseMultiEdge
一元边只连接一个顶点，二元边连接两个顶点，多元边一条边可以连接多个顶点。

边的主要参数，G2O中边的定义主要有D,E,VertexXi,VertesXj几个参数，其中D是int型，表示测量值的维度，E表示测量值的数据类型，VertexXi,VertexXj分别表示不同顶点的类型。举个例子：
用边来表示三维点投影到图像平面的重投影误差

 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

这里定义了一个Binary二元边，参数2表示测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点，也就是优化变量分别是三维点VertexSBAPointXYZ，和李群位姿VertexSE3Expmap。

2.边的主要函数

定义一个边也要复写几个函数，如下所示

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

前两个读写函数和顶点中的类似。
computeError()函数是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus()函数是在当前顶点的值下，该误差对优化变量的偏导数，也就是雅可比。
除此之外，还有：

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

3.自定义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
  }

举个例子，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
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      // 顶点v2
      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;
};

其中

_error = obs - cam->cam_map(v1->estimate().map(v2->estimate()));

这里cam_map函数将相机坐标系下三维点(输入)用内参转换为图像坐标(输出)
而.map函数将世界坐标系下三维点转换到相机坐标系下。
因此

v1->estimate().map(v2->estimate())

就是用v1估计的位姿，把v2代表的三维点变换到相机坐标系下。

4.向图中添加边

一元边的例子

    // 往图中增加边
    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 );
    }

二元边的例子

    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        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++;
    }