图优化g2o学习

**概念：**图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成。

**举例：**一个机器人在房屋里移动，它在某个时刻 t 的位姿（pose）就是一个顶点，这个也是待优化的变量。而位姿之间的关系就构成了一个边，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表示误差项。

在SLAM里，图优化一般分解为两个任务：

1、构建图。机器人位姿作为顶点，位姿间关系作为边。

2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

下面看一张整体图:

以高博在Slam十四讲中的代码为例：

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

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

// 第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; isetId(i);
  edge->setVertex( 0, v );                // 设置连接的顶点
  edge->setMeasurement( y_data[i] );      // 观测数值
  edge->setInformation( Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge( edge );
}

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

编写一个图优化的程序需要从底层到顶层逐渐搭建，，步骤可以分为6步：

1.创建一个线性求解器LinearSolver。

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。 p代表pose的维度（是流形manifold下的最小表示），l表示landmark的维度
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

// 可变尺寸的solver。Pose和Landmark在程序开始时并不能确定，所有参数都在中间过程中被确定。
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

此外g2o还预定义了以下几种常用类型：

• 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初始化。

在该阶段可以选择GN/LM/DogLeg 这三种方法。

4.创建图优化的核心：稀疏优化器（SparseOptimizer）。

创建稀疏优化器

g2o::SparseOptimizer    optimizer;

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

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

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

SparseOptimizer::setVerbose(bool verbose)

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

顶点

一：先来看看g2o整体图中和vertex有关的第①个类： HyperGraph::Vertex。它在这个路径g2o/core/hyper_graph.h。 HyperGraph::Vertex 是个abstract vertex，必须通过派生来使用。

二：图中第②个类，我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的， 而OptimizableGraph的定义在g2o/core/optimizable_graph.h。我们找到vertex定义，发现果然，OptimizableGraph 继承自 HyperGraph。

三：OptimizableGraph::Vertex 非常底层，具体使用时一般都会进行扩展，因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中 对应的第③个类：BaseVertex 路径：g2o/core/base_vertex.h

BaseVertex

参数

static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space

**D：**并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示，这里一定要区别开.

typedef T EstimateType;
EstimateType _estimate;

**T：**从这里看到就是顶点（状态变量）的类型.

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>

如果内置的顶点中没有我们需要的，那么需要进行自定义，自定义顶点一般需要考虑重写如下函数：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

virtual void oplusImpl(const number_t* update);
//顶点更新函数

virtual void setToOriginImpl();
//顶点重置函数，设定被优化变量的原始值。

自定义的顶点实例代码（高博Slam十四讲）：

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

边

图优化中的边：BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

一元边可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点（常见），多元边理解为一条边可以连接多个（3个以上）顶点。

参数：

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

实例：

 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

这是个二元边。测量值是2维的，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap

自定义边：

自定义边需要重写以下的函数：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

virtual void computeError();
// 非常重要，是使用当前顶点值计算的测量值与真实测量值之间的误差

virtual void linearizeOplus();
// 非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian矩阵

自定义边的实例代码（高博Slam十四讲课）：

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // 计算曲线模型误差
  virtual void computeError() override {
    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 void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

对边中重要的成员变量和函数进行简单介绍：

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

在设置边对应顶点的时候可以

edge->vertices()[0]=v;

也可以

edge->setVertex(0, v); 

参数中表示顶点的编号。

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

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

初始化

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

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

SparseOptimizer::optimize(int iterations, bool online)

参考：

• https://zhuanlan.zhihu.com/p/121628349，
• https://blog.csdn.net/qq_37221466/article/details/102588687，
• https://zhuanlan.zhihu.com/p/55133492
• https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486992&idx=1&sn=ecb7c3ef9bd968e51914c2f5b767428d&chksm=97d7eb87a0a062912a9db9fb16a08129f373791fd3918952342d5db46c0bc4880326a7933671&scene=21#wechat_redirect
• https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247487082&idx=1&sn=d4a27e4c9a76760fffb571f57f4f7719&chksm=97d7ebfda0a062eba412877e9ecf5933f2051f0210c0d56f03267985512d97f2db434ab7356c&cur_album_id=1361700104461467649&scene=189#wechat_redirect