slam学习笔记4--后端之图优化

最新推荐文章于 2024-05-17 20:10:18 发布

少年心事当拂云ง ง

最新推荐文章于 2024-05-17 20:10:18 发布

阅读量1.3k

点赞数 1

分类专栏： slam 文章标签：学习算法

本文链接：https://blog.csdn.net/weixin_49239046/article/details/129107536

版权

slam 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言
一、图优化是什么？
二、G2O
- 1.G2O结构
总结

前言

提示：这里可以添加本文要记录的大概内容：

这篇主要是记录我针对于slam后端的学习，通过学习了解到，对于slam后端主要可以分为两种解决办法，一种是基于滤波，一种是通过图优化的方式，本篇记录的是针对于图优化的学习。
提示：以下是本篇文章正文内容，下面案例可供参考

一、图优化是什么？

图优化里的图表示的是数据结构里的图，这个图中包含有若干个顶点(vertex)，以及连接这些顶点的边(edge)组成，在slam后端中位姿是顶点而两个位姿间的变换关系就是边。
当一个机器人在房间里移动，在某一时刻的位姿就是一个顶点，同时也是待优化的变量，而位姿间的关系就是一个边，t时刻与t+1时刻间的相对位姿变换关系就是图中的边，而边通常就是误差项。
在slam的后端中，图优化一般可以分解成两个大的步骤：
1.构建图。机器人位姿作为顶点，位姿间关系作为边。
2.优化图。调整机器人的位姿(顶点)来尽量满足边的约束，使得误差最小。
后端建立图优化时，也是有现成的库可以调用，主要可以使用的库有g2o，ceres，GTSAM，这里主要记录下前两个的基本理论和使用步骤。

二、G2O

1.G2O结构

在这里插入图片描述 G2O的框架组成如上图所示，从图中可以看到核心是SparseOptimizer，
首先看图的上方，可以看到SparseOptimizer是一个HyperGraph，在HyperGraph包含许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge，之后看图下方图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个（我们常用的是GN和LM），这就是整个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(100);

1、创建一个线性求解器LinearSolver
要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当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有两种定义方式
一种是指定的固定变量的solver，来看一下定义

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

其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度
另一种是可变尺寸的solver，定义如下

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

预定义了比较常用的几种类型，如下所示：

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::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg 
g2o::OptimizationAlgorithmDogleg

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

g2o::SparseOptimizer    optimizer;

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

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

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

SparseOptimizer::setVerbose(bool verbose)

5、添加图的顶点和边到SparseOptimizer中
在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>

有的时候想使用的顶点可能G2O中没有定义，那么我们可以自己定义，自己定义一般是要重写下面的函数：

virtual bool read(std::istream& is);//读盘
virtual bool write(std::ostream& os) const;//存盘
virtual void oplusImpl(const number_t* update);//顶点更新函数。非常重要的函数。
virtual void setToOriginImpl();///顶点重置函数，设定被优化变量的原始值

在自己定义顶点时，一般只是使用2，3。opluslmpl主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的。
// 曲线模型的顶点，模板参数：优化变量维度和数据类型

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，更新时直接把更新量update加上去。
当定义完顶点后那么我们可以向图中加入顶点了。

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

setEstimate(type) 函数来设定初始值；setId(int) 定义节点编号

边的定义与添加
G2O中一般使用的就三种边，分别是BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个（3个以上）顶点。
用边表示三维点投影到图像平面的重投影误差，就可以设置输入参数如下：

 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

首先这个是个二元边。第1个2是说测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap
定义边：

virtual bool read(std::istream& is);//读盘
virtual bool write(std::ostream& os) const;//存盘
virtual void computeError();//使用当前顶点的值计算的测量值与真实的测量值之间的误差
virtual void linearizeOplus();//在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian

还有几个可能会使用到的成员变量和函数：

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

定义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()两个函数。
定义1元边的例子：

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

当定义完边就可以向图中添加边了，如下：

// 往图中增加边
    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、设置优化参数，开始执行优化。
设置SparseOptimizer的初始化、迭代次数、保存结果等。
初始化

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

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

SparseOptimizer::optimize(int iterations, bool online)

总结

图优化基本的理论和使用就是这样的了，但是目前也仅仅是大概能看懂，不太懂其奥义。
参考：
https://zhuanlan.zhihu.com/p/58521241
https://blog.csdn.net/electech6/article/details/88018481
https://blog.csdn.net/electech6/article/details/86534426
https://blog.csdn.net/tiancailx/article/details/121266020?spm=1001.2014.3001.5502
高翔《视觉SLAM十四讲》