SLAM从0到1之图优化g2o：从看懂代码到动手编写（长文）

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

本文由知乎作者yikang授权转载，不得擅自二次转载。原文链接：https://zhuanlan.zhihu.com/p/121628349

「本文主要介绍了对g2o代码框架的解读，以及如何自己设计g2o里的顶点和边（代码实现）」

g2o（General Graphic Optimization)，是一个通用图优化算法库。由于目前主流的SLAM研究基本都是基于图优化的，因此十分有必要掌握一下g2o方法。

关于g2o的基本理论并不高深，在SLAM中应用g2o的难点主要是在代码实现上（作为入门小白，那些代码看的我是一脸懵*）。

>> 特别需要记住的是：

图优化中的点是相机位姿，也就是优化变量（状态变量）
图优化中的边是指位姿之间的变换关系，通常表示误差项

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

下图为官方文档中经典的g2o框架：

对这个结构框图做一个简单介绍（注意图中三种箭头的含义（右上角注解））：

（1）整个g2o框架可以分为上下两部分，两部分中间的连接点：SparseOpyimizer 就是整个g2o的核心部分。

（2）往上看，SparseOpyimizer其实是一个Optimizable Graph，从而也是一个超图（HyperGraph）。

（3）超图有很多顶点和边。顶点继承自 Base Vertex，也即OptimizableGraph::Vertex；而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。

（4）往下看，SparseOptimizer包含一个优化算法部分OptimizationAlgorithm，它是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）、 Levernberg-Marquardt（简称LM法）,、Powell's dogleg 三者中间选择一个（常用的是GN和LM）。

（5）对优化算法部分进行求解的时求解器solver，它实际由BlockSolver组成。BlockSolver由两部分组成：一个是SparseBlockMatrix，它由于求解稀疏矩阵(雅克比和海塞)；另一个部分是LinearSolver，它用来求解线性方程得到待求增量，因此这一部分是非常重要的，它可以从PCG/CSparse/Choldmod选择求解方法。

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

我们再来看一下框架的搭建步骤，以高博在SLAM十四讲中使用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步：创建图优化的核心：稀疏优化器（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);    //设置迭代次数

如程序中所示，编写一个图优化的程序需要从底层到顶层逐渐搭建，参照g2o官方框架图（上方），步骤可以分为6步：

创建一个线性求解器LinearSolver。
创建BlockSolver，并用上面定义的线性求解器初始化。
创建总求解器solver，并从GN/LM/DogLeg 中选一个作为迭代策略，再用上述块求解器BlockSolver初始化。
创建图优化的核心：稀疏优化器（SparseOptimizer）。
定义图的顶点和边，并添加到SparseOptimizer中。
设置优化参数，开始执行优化。

分开说一下各个步骤：

（1）创建一个线性求解器LinearSolver

这一步中我们可以选择不同的求解方式来求解线性方程，g2o中提供的求解方式主要有：

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

注意看程序中只使用了一行代码进行创建：右侧是初始化；左侧含有我们选择的迭代策略，在这一部分，我们有三迭代策略可以选择：

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

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

根据程序中的代码示例，创建稀疏优化器：

g2o::SparseOptimizer  optimizer;

设置求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

设置优化过程输出信息：

SparseOptimizer::setVerbose(bool verbose)

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

看下面的具体讲解。

（6）设置优化参数，开始执行优化

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

初始化：

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

设置迭代次数：

SparseOptimizer::optimize(int iterations,bool online)

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

下面专门讲讲第5步：定义图的顶点和边。这一部分使比较重要且比较难的部分，但是如果要入门g2o，这又是必不可少的一部分。

1. 点 Vertex

在g2o中定义Vertex有一个通用的类模板：BaseVertex。在结构框图中可以看到它的位置就是HyperGraph继承的根源。

同时在图中我们注意到BaseVertex具有两个参数D/T，这两个参数非常重要，我们来看一下：

D 是int 类型，表示vertex的最小维度，例如3D空间中旋转是3维的，则 D = 3
T 是待估计vertex的数据类型，例如用四元数表达三维旋转，则 T 就是Quaternion 类型

static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold spacetypedef T EstimateType;EstimateType _estimate;

特别注意的是这个D不是顶点(状态变量)的维度，而是其在流形空间(manifold)的最小表示。

>>>如何自己定义Vertex

在我们动手定义自己的Vertex之前，可以先看下g2o本身已经定义了一些常用的顶点类型：

ertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)VertexSE3 : public BaseVertex<6, Isometry3> //Isometry3使欧式变换矩阵T，实质是4*4矩阵//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 mapVertexSE3Expmap : 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 rotationVertexCam : 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>

但是！如果在使用中发现没有我们可以直接使用的Vertex，那就需要自己来定义了。一般来说定义Vertex需要重写这几个函数（注意注释）：

virtual bool read(std::istream& is);virtual bool write(std::ostream& os) const;// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以virtual void oplusImpl(const number_t* update);//顶点更新函数virtual void setToOriginImpl();//顶点重置函数，设定被优化变量的原始值。

请注意里面的oplusImpl函数，是非常重要的函数，主要用于优化过程中增量△x 的计算。根据增量方程计算出增量后，通过这个函数对估计值进行调整，因此该函数的内容要重视。

根据上面四个函数可以得到定义顶点的基本格式：

class myVertex: public g2o::BaseVertex<Dim, Type>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWmyVertex(){}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);   //update强制类型转换为Vector3d    }// 存盘和读盘：留空    virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}};

另外值得注意的是，优化变量更新并不是所有时候都可以像上面两个一样直接 += 就可以，这要看优化变量使用的类型（是否对加法封闭）。

>>> 向图中添加顶点

接着上面定义完的顶点，我们把它添加到图中：

CurveFittingVertex* v = new CurveFittingVertex();v->setEstimate( Eigen::Vector3d(0,0,0) )；  // 设定初始值v->setId(0);                               // 定义节点编号optimizer.addVertex( v );                  // 把节点添加到图中

三个步骤对应三行代码，注释已经解释了作用。

2.边 Edge

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

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

以最常见的二元边为例分析一下他们的参数：D, E, VertexXi, VertexXj：

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

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

上面这行代码表示二元边，参数1是说测量值是2维的；参数2对应测量值的类型是Vector2D，参数3和4表示两个顶点也就是优化变量分别是三维点 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();  // 定义协方差矩阵的逆

有了上面那些重要的成员变量和成员函数，就可以用来定义一条边了：

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}

让我们继续看curveftting这个实例，这里定义的边是简单的一元边：

// （误差）边的模型    模板参数：观测值维度，类型，连接顶点类型class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( 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      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估计的pose把V2代表的三维点，变换到相机坐标系下。

>>>向图中添加边

和添加点有一点类似，下面是添加一元边：

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

但在SLAM中我们经常要使用的二元边（前后两个位姿），那么此时：

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

至此，就介绍完了g2o中的一些框架和实现，需要提醒的是在SLAM中我们常常用到的是二元边以及对应的点，他们都较为复杂，应当多次学习复习实践。

上述内容，如有侵犯版权，请联系作者，会自行删文。