新年第一篇,就来聊聊g2o吧。

g2o (general graph optimization) 是SLAM中常用的后端优化库。之前我在文章g2o小结里也大概总结了下g2o在《十四讲》中的使用。

与ceres相比,g2o没有详细的文档,只有两篇论文可以参考。这里我们深入g2o的源代码(core文件夹里的部分),来一窥这个大名鼎鼎的后端优化库的虚实。

这篇文章是我在看g2o源代码是大概做的一个笔记,不像之前文章、或者类似于“ORB-SLAM解析”这样的文章那么详细。希望你阅读之前浏览过、或者正准备看看g2o的源码,这样更有助于你理解。


这张图是g2o的一个类关系图。由这张图可以看出,整个g2o的架构分为了三层:

  1. 外层的图优化架构:包含了图、节点和边等等。这也是最主要的和用户进行交互的一层,用于构建整个优化问题;
  2. 中间的优化算法:这是具体的最小二乘优化算法,包括了GN、LM和狗腿算法。用户一般只要选择某个优化算法即可(根据问题性质),并不用太关心内部实现。而且,在g2o中,这些优化算法只专注于本次迭代中的求解,至于说何时停止、如何更新节点都是外层的图优化来维护的;
  3. 最里层的线性求解器:这是所有优化算法的核心,即如何高效求解一个大型(稀疏或稠密)的线性系统 (Hx=b),也是最复杂的。除非对效率由极致追求(必须选择最合适的计算方式),否则用户一般不关心。

我们就由上至下,一层层地看g2o是如何进行图优化的。

首先是HyperGraph类。HyperGraph是一个高度抽象的虚基类,Vertex和Edge是通过内类 (inner class) 的方式实现的。二者是相互关联的,例如,Vertex类中存有一个Edge类实例的set,用来保存与这个节点相关联的边;而Edge类中有一个存有Vertex实例的vector,保留这条边链接的节点。这两个类都只提供了很简单的成员函数,如设置ID,获得对应的边set,获得某个或整组节点,设置边的节点等等。

由于一条超边可以连接多个节点,因此边的内部用vector来存储所连接的节点,并用nullptr来代表未初始化的节点。

而HyperGraph类中除了简单的获得/设置某个节点和获得边set之外,还有一些稍微复杂一点的函数,如去除、添加某个节点和边等,实现也很直观,都先要求这个节点或边已经被初始化过,且尚未添加到当前的图中。添加边时也会同时把这个边同步到对应节点的EdgeSet中。删除节点和删除边也是同理:删除节点要把对应的边删除;但删除边的时候不必删除节点,但要同步到对应节点的EdgeSet中。这是因为去除边只是去除了一个约束(测量),对于观测没有影响;但是取出一个观测(节点)的话,其所关联的测量都要被去除。

此外,HyperGraph还有合并节点和将某个节点和边detach的功能。合并节点时要保证两个节点都在当前图中,然后将被合并节点的关联边的信息给替换掉。而把某个节点和边detach的功能和将某个节点和边链接起来的功能其实是一样的:前者不过是将边和空节点连接起来。该功能常用在删除节点之前。


OptimizableGraph继承自HyperGraph,但显然就复杂很多。同样是通过内类实现Vertex和Edge。

此时,节点和边就更加具体了,基本完成了所需所有函数的声明(作为interface),但仍然是虚基类。

Vertex开始有Oplus函数调用OplusImpl的实现、setToOriginImpl虚函数、setEstimateData等,还有是否fix、是否marginalized等设置。此外,还有海塞矩阵的相关操作,如获得海塞矩阵的某个元素,获得、设置在整个parameter blocks中的位置(海塞矩阵中的位置),获得b向量等。最后,还支持通过pop、push等类似于栈 (stack) 的操作来获得对应节点信息。

同样地,Edge类中有computeError、linearOplus虚函数,可以设置鲁棒核函数等。当然也有获得、设置measurement等函数。Edge类还支持获得、设置信息矩阵 (information matrix)、海塞矩阵。调用computeError函数后,还可以用errorData函数来获得误差信息,用chi2函数来获得(加权)最小二乘误差等,这些可以用来判断某条边的误差是否过大。最后,边允许获得/设置level,将边设置为不同的level,在优化时就可以分开优化不同level的边(orb-slam2的优化就采用了这种multi-level optimization)。

OptimizableGraph类对应的添加、删除节点或边的函数功能相同,但多了一些校验:必须继承自OptimizableGraph,没有被添加过,不在另一张图中等,其余操作调用HyperGraph的对应函数即可。该类已经有optimize函数以供继承、优化。并且,因为操纵一堆节点,因此提供借口来操作一个节点set,如push, pop, setFixed等。

总体而言,节点、边和优化器的最主要内容如下:


SparseOptimizer继承自OptimizableGraph,是优化时的具体操作对象。到了这一步,SparseOptimizer更加具体,并且不再包含具体的Vertex或者Edge,而是作为图优化的interface,实现具体的功能。

SparseOptimizer提供的第一大类别函数是优化初始化函数 initializeOptimization. 最简单的就是通过设置level来优化不同层次的图。此外,还可以通过设置特定的节点或边来初始化要优化的对象(一部分图)。以设置节点为例,函数遍历所有节点,和该节点对应的边,把相关节点和被设置为level(默认是0)的边添加到_activeVertices和_activeEdges,供后续算法优化。这样,就实现了mulit-level optimization。还有setAlgorithm来设置对应的优化算法。

另一大类别函数为辅助函数。如findGauge和gaugeFreedom函数可以找到维度最大的节点,或返回、或将之fix住来减小自由度,从而使得优化算法可行;或是update函数、computeActiveError函数等来单次更新节点、计算误差;或是findActiveEdge等来获得激活边等变量信息。

最后就是优化函数optimize。函数直接按照当前图的configure和设置的优化算法来进行迭代计算,算法在这里判断是否停止,内部使用一个OptimizationAlgorithm类型的变量_algorithm来进行具体的优化计算(见第一张图)。并根据之前的用户设置,如setVerbose, setComputeBatchStatistics来决定是否记录中间结果和统计信息等。


节点BaseVertex类直接继承OptimizableGraph中的Vertex类。两个模版参数为估计值(待优化变量)的维度D和类型T。

由于之前在OptimizableGraph::Vertex中,Vertex所需的大部分函数已经被实现了,因此,BaseVertex类只根据需要实现或重载了一部分成员函数。

类中包含变量_hessian和_b来记录这个节点的海塞矩阵和b,节点当前的估计值则存储在_estimate中。此外,类还提供了一个stack类型的_backup变量。可以调用push来备份某次_estimate和调用pop来获得上次的_estimate。这其实是为了进行数值求导而准备的。

最后,相比于基类,BaseVertex实现了大部分set, get函数。如获得海塞矩阵或b向量等。但更主要的,如OplusImpl需要用户自己去实现。

这里,g2o实现了solveDirect函数,由用户提供lambda,然后利用Cholesky分解来对这个节点进行求解: [公式] ,并调用oplus(dx)来对节点进行更新。


BaseEdge类继承了OptimizableGraph中的Edge类。后面的三种边:BaseUnaryEdge, BaseBinaryEdge和BaseMultiEdge都是继承的BaseEdge类。

由于链接的节点数量未知,所以BaseEdge类只有两个模版参数:观测值的维度D和及其类型E。

BaseEdge类主要包含三个成员变量:观测_measurement,信息矩阵_information和误差_error。其主要实现了关于这三个成员变量的set, get函数,还有一个计算加权最小二乘误差的chi2函数。其他的如setVertex, setMeasurement等函数已经在父类中实现。

另一个比较重要的函数就是robustInformation函数。这是用来计算鲁棒核函数所对应的信息矩阵的。在这里,g2o并没有按照我们上一章非线性优化(二):徒手实现LM算法中的公式 [公式] 计算,而是类似ceres-solver,直接丢弃了rho'',只计算 [公式],这是为了保证信息矩阵的正定性

每种edge都需要用户来实现computeError函数。

由于只链接一个节点,因此BaseUnaryEdge只多了一个模版参数:节点类型。并且多了一个雅可比矩阵的成员变量:_jacobianOplusXi。此外,由于HyperGraph::Edge类中用了vector<Vertex *>来存储一条边所连接的节点,因此,BaseUnaryEdge类在初始化的时候会将这个数组resize为1(对应地,BaseBinaryEdge类初始化时会将之resize为2)。

其最主要的函数是计算雅可比矩阵的linearizeOplus函数。可以由用户提供雅可比矩阵的解析解,而默认是g2o自己进行数值求导:g2o使用中值求导方法,其步长delta为常数值。函数对节点的_estimate加上和减去对应步长,再计算对应的误差值,再除以2*delta来计算误差函数对于特定误差项的导数。此时,为了保存节点原来的值,就会使用push/pop函数往_backup变量中保持修改前的节点估计值。(数值求导部分我们后面再细说。)

另一个比较主要的函数是constructQuadraticForm函数。该函数用来计算该误差项的系数矩阵H和对应的b向量。以BaseUnaryEdge为例,函数先调用chi2函数获得误差 ( [公式] ),如果没有使用鲁棒核函数,直接使用 [公式] 来计算系数矩阵H,用 [公式] 来计算b向量;否则,调用对应函数得到鲁棒核函数的值,及其一阶导、二阶导(rho,见后面的鲁棒核函数)。b向量可以直接通过 [公式] 来计算,但系数矩阵H则调用robustInformation函数来重新计算加权信息矩阵 (weightedOmega)。

BaseBinaryEdge链接两个节点,因此比BaseEdge多两个模版参数。对应的,自然也会有两个雅可比矩阵:_jacobianOplusXi和_jacobianOplusXj。其余本质上和BaseUnaryEdge并没有差别。需要注意的是,BaseBinaryEdge类也定义了一个_hessian变量,但这个海塞矩阵并不是残差对于观测(节点)进行求导得到的雅可比J得到的 [公式] ,而是通过 [公式] 得到的,显然,这是_Hpl矩阵,即位姿和路标点的协方差矩阵(或位姿和位姿间的协方差矩阵)。

BaseMultiEdge由于链接的节点数目是不固定的,因此其直接继承BaseEdge类而不在需要额外的模版参数。相关的雅可比矩阵也变成了一个vector变量:_jacobianOplus。链接多个节点并没有带来多大的不同。一些变量如_hessian和_jacobianOplus定义成了vector。而在计算雅可比矩阵(linearizeOplus)时,BaseUnaryEdge只计算一个雅可比矩阵,BaseBinaryEdge计算两个,而BaseMulitEdge利用循环计算了多个雅可比矩阵,分别存储_jacobianOplus[I]中;constructQuadraticForm函数也是同理。


现在,可以关注下一些细节的实现。先来看鲁棒核函数。

g2o实现了一个RobustKernel的虚基类。这个虚基类提供了设置和获得核函数参数lambda的接口和一个robustify虚函数,用 [公式] 来计算鲁棒核函数的值、一阶导数和二阶导数,存储在一个Vector3D变量rho中。

鲁棒核函数的真正实现在robust_kernel_impl文件中。g2o实现了多个鲁棒核函数,常见的Huber核、Cauchy核等都有。


前面提到的OptimizationAlgorithm类其实是一个虚基类,它定义了一个优化器的大部分接口,如init, computeMarginals和solve等,这些都是虚函数,需要对应的派生类来进行实现。

OptimizationAlgorithmWithHessian就是这个对应的派生类,但由于没有实现solve这个最重要的虚函数,因此它仍然是一个虚基类。它最重要的功能有两点。一个是定义了一个线性求解器_solver(Solver类,即我们刚开始说的第三层),后续派生类中,具体的矩阵求解等就通过这个线性求解器来完成。第二个就是这个虚派生类实现了OptimizationAlgorighm中的一部分虚函数,如init, computeMarginals(其实也是调用_solver来实现)等。以init函数为例,算法会遍历当前的activeVertices,如果有需要进行边缘化的节点,并且对应的线性求解器也支持Schur补,则会进行边缘化,并调用_solver的初始化函数来进行初始化。

g2o提供了三种优化算法:GN,LM和dog-leg,各是一个类,最主要的函数都是solve函数。但由于优化算法的不同,因此内部实现都不一样。要注意的是,优化算法的迭代和停止判断都是在外部SparseOptimizer类中的optimize函数中判断,这三种优化算法只负责解当前的迭代步骤。

GN算法最简单,因此类中(主要)只有一个solve函数:先通过_solver来完成一些初始化工作,如buildStructure, buildSystem等,然后就直接调用_solver->solve()来进行求解了,然后更新状态向量(_optimizer的update函数),并且判断_solver求解是否成功。这是最简单的。

LM算法则比较复杂了,有computeLambdaInit函数来初始化拉格朗日算子Lambda,方式也和前述笔记中描绘的是一样的。并且有一个max_trials变量来控制求解次数:如果当前求解结果不好(gain ratio [公式] <0),那么LM算法会增加拉格朗日算子的值,然后再次求解,而max_trails则控制这样的重复求解的次数。solve函数中除了和GN算法一样,通过_solver来完成一些初始化工作之外,还要初始化LM算法的相关值,如会计算当前网络的误差,以便后续计算F(x) - F(x+h)等(求解gain ratio用)。内部迭代求解时,算法调用对应优化网络_optimizer的push函数,来保存当前结果,以防当前迭代的结果不好,可以进行恢复。然后就调用_solver->solve()来进行求解。最后就是LM算法的典型操作,计算gain ratio来判断是否接受当前结果等。整个的具体实现流程其实和我们在非线性优化(一):优化方法中利用MATLAB实现的流程是一样的。

实现最复杂的就是dog-leg算法。在其solve函数中,通过_solver完成初始化并计算当前网络的误差后,算法先计算出h_sd(这个比较好计算,过程和之前非线性优化(一):优化方法一样);然后再计算h_gn,这里,为了防止出现海塞矩阵非正定的情况,g2o引入了一个damping factor lambda,并用一个简单的策略来更新lambda(类似于LM算法,但比前述的LM算法简单很多)。在这里,g2o给lambda设置了上下界,因为h_gn计算的成功与否,除了依靠_solver->solve()的返回值外,还看对应的lambda值:如果lambda的值过大,那么显然h_gn即使计算出来了也是失败的,算法返回fail。有了h_sd和h_gn,就可以根据dog-leg算法的流程来计算h_dl了。显然,算法也需要计算grain ratio [公式] 来判断当前结果的好坏并更新信任区域Dleta的大小。和LM算法一样,dog-leg算法也有一个max_trails变量来限制内部迭代的最大次数。


最后就是线性求解器Solver类。Solver类是一个虚基类,实现了大部分所需要的函数用作交互,包括用于求解器初始化的init, buildStructure和buildSystem函数等;求解Ax=b的solve()函数;和一些辅助函数:_x和_b的get,图_optimizer的set和get,和levenberg法相关的设置lambda等函数。


Solver的主要派生类是BlockSolver类。这是一个模板类,其模板参数是另一个模板类BlockSolverTraits。这是一个很常见的用法,BlockSolverTraits的两个模板参数分别是位姿的维度_poseDim和路标点的维度_landmarkDim,里面主要是定义了各种类型,如下:

template <int _PoseDim, int _LandmarkDim>
  struct BlockSolverTraits
  {
    static const int PoseDim = _PoseDim;
    static const int LandmarkDim = _LandmarkDim;
    typedef Eigen::Matrix<double, PoseDim, PoseDim, Eigen::ColMajor> PoseMatrixType;
    typedef Eigen::Matrix<double, LandmarkDim, LandmarkDim, Eigen::ColMajor> LandmarkMatrixType;
    typedef Eigen::Matrix<double, PoseDim, LandmarkDim, Eigen::ColMajor> PoseLandmarkMatrixType;
    typedef Eigen::Matrix<double, PoseDim, 1, Eigen::ColMajor> PoseVectorType;
    typedef Eigen::Matrix<double, LandmarkDim, 1, Eigen::ColMajor> LandmarkVectorType;




    typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
    typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
    typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
    typedef LinearSolver<PoseMatrixType> LinearSolverType;
  };

在ORB-SLAM中,常常用g2o已经定义好的BlockSolver类型,如下:

  // solver for BA/3D SLAM
  typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;  
  // solver fo BA with scale
  typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;  
  // 2Dof landmarks 3Dof poses
  typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;

由此可见g2o和ceres不同,不能说是一个很“通用”的优化库,而是聚焦在SLAM的后端优化。

此外,BlockSolver类中定义的线性求解器为LinearSolver<PoseMatrixType> *_linearSolver,所以在使用g2o时,每次初始化模板LinearSolver的时候都要初始化为PoseMatrixType类型。

--------------------------------------具体函数说明---------------------------------------

  1. buildStructure函数

中间层的优化算法调用线性求解器时,如果是第一次迭代,那么要调用buildStructure来构建Hx=b这样一个线性系统。通过这个函数我们可以发现,g2o默认边缘化掉landmark,且它们的顺序(即id)在机器人位姿之后,所以前面必须初始化为PoseMatrixType。buildStructure通过遍历多次图来构建这个线性问题:

  1. 第一次遍历整张图(所有节点)来得到机器人位姿和路标点的个数,并且按顺序确定每个变量在(各自)海塞矩阵(_Hpp, _Hll) 中的位置和占据的大小;
  2. 第二次遍历整张图来获得每个节点的海塞矩阵,按之前遍历得到的信息将之放入_Hpp或_Hll中恰当的位置;
  3. 第三次遍历所有待优化的边,这一次,是为了获得N(=2)个节点间的协方差矩阵,来构建_Hpl矩阵。

如果不进行schur补,那么到此就结束了。否则,我们要再一次遍历整张图,来完成schur补的辅助变量的构建。这时,我们只处理要被边缘化的节点。(g2o只支持marg路标点。)一个路标点要被marg点,则会在观测到它的n个相机位姿之间引入fill-in. 因此,要遍历被marg的路标点连接的边和这些边所连接的其他节点,在这些节点之间记录后面会有fill-in,相当于建立一个索引表,方便后续操作。然后根据这个索引表事先建立好_Hschur矩阵。

此外,g2o默认路标点的vertex都在位姿的vertex之后(状态向量中的下标)。所以可以看到不论是《十四讲》还是ORB-SLAM,都是先添加机器人位姿的vertex,再添加路标点的vertex。

2. buildSystem函数

buildStructure只有在还没开始迭代的时候先调用一次。然后,每次迭代求解之前,都需要调用buildSystem函数来初始化。

因为通过buildStructure函数,整个系统的H矩阵的结构我们已经构建好了。现在,经过之前的迭代求解,观测量的值发生了变化(或者没经过求解,但也要把初始化的值添加到构建好的结构里),因此要对整个系统(的值)进行更新。

先遍历所有节点,对每个结点,清空之前的H和b(clearQuadraticForm函数),并且清空_Hpp矩阵。如果要进行舒尔补,还要清空_Hll和_Hpl矩阵。然后遍历所有边,重新进行线性化操作(lineaizeOplus函数)并构建H和b(constructQuadraticForm函数)。此时,大的H矩阵已经完成更新,还需要遍历所有节点来更新b向量(因为BaseVertex中,_hessian是Eigen::Map类型,而_b是Eigen::Matrix类型,因此虽然没有显式地更新系统的H矩阵,但在遍历边时,更新的H就已经更新在_Hpp等变量里面了)。


3. solve函数

最后,我们就可以调用solve函数来求解了。如果我们没有进行schur补操作,那么没啥说的,直接调用_linearSolver的solve函数来求解Hx=b就可以了(调用_linearSolver)。

如果我们要进行schur补操作,那就稍微有点复杂了,但也只是有点,整体也是按照舒尔补的公式来进行schur补进行求解。由于路标点对应的H矩阵显然是个稀疏的block diagonal matrix,因此我们遍历路标点,单独对其_Hll矩阵进行求逆(即单独求一个3*3矩阵的逆)。然后按照公式进行schur补相关矩阵的计算。这里先求出的机器人位姿,因此先计算其相关的矩阵(_Hschur, _bschur等);然后调用_linearSolver,输入相关矩阵进行求解。如果机器人位姿求解成功了,就按照公式将之反代回schur补公式中,对路标点的位置进行求解。

4. 其他函数

  • 要么没怎么用到,如updateStructure函数,这是外部传入一组节点和边,以此来构建大的H矩阵。这个函数不支持schur补,因此比buildStructure函数简单多了,主要就是完成buildStruct函数的那三个步骤。而且由于不进行舒尔补,所以步骤1是不需要的,就当所有节点都是机器人位置,然后挨个放进去就可以。
  • 要么很简单。如setLambda和restoreDiagonal这一对函数,就是将H矩阵备份,然后在主对角元素上加上lambda或者从备份中恢复原来的H矩阵。init函数就是清空各个矩阵块,然后调用_linearSolver的init函数即可。

最后,线性求解器LinearSolver是一个模板虚基类。它是solvers文件夹下各个线性求解器的interface. 它还有一个派生类LinearSolverCCS,主要是利用迭代的方式(如PCG)来对线性系统进行求解。

这里就不详细介绍了。比较简单的求解器有LinearSolverDense,大概介绍下让大家对这个先行求解器有所了解。

顾名思义,LinearSolverDense是用来求解一个稠密矩阵的,比如舒尔补之后的机器人位姿矩阵。的solve函数中,先准备好一个矩阵_H(传给_linearSolver的海塞矩阵H在solve函数的形参名称为A,可以认为是H的CRS存储形式(一种矩阵存储方式),所以这里面是在求Ax=b的解),这里假设传入的A矩阵是一个稀疏矩阵,并且按照CRS的方式存储。因此需要将A矩阵恢复成一个真正的m*n大小的矩阵,存放在_H中。最后,调用Eigen中的Cholesky分解来求解Ax=b,并返回最后的结果。


最后,附上我做的、与文章开头的类关系图对应的、更详细的g2o类关系图。类之间的连接线的关系和第一张图中是一样的,我就不在添加了。

g2o: 图优化框架
g2o: 优化算法与线性求解器

如果觉得有用,还请点个赞: )