g2o内部实现初探

研究SLAM的同学应该对g2o并不陌生。用了一段时间之后，一直对其内部实现方式不太清楚，今天打算仔细研究一下。

首先祭出官方提供的g2o类层次结构图。

g2o类层次结构图

这个图需要分块来看。

左上角HyperGraph提供了顶点和边构成的拓扑图，它只关注图的连接关系，不负责优化相关的工作。其派生类OptimizableGraph为顶点和边提供了可优化的功能。再往下是OptimizableGraph的派生类SparseOptimizer，显然，对于构建的图优化问题，SparseOptimizer提供了稀疏求解的方案，这也是SLAM能够达到实时性所依赖的关键技术。以上，是优化器相关的部分。

再往下，优化器包含了一个OptimizationAlgorithm，这是优化算法的基类，优化器会调用该优化算法实现优化。右边给出了优化算法的具体实现，OptimizationWithHessian包含了一系列基于Hessian矩阵求解增量方程的优化算法，包括OptimizationAlgorithmGaussNewton、OptimizationAlgorithmLevenberg和OptimizationAlgorithmDogleg。不同的优化算法给出了不同的梯度下降策略，也就是说，用不同的方式找出增量的方向和大小，但迭代求解的本质是一样的。

再往右，优化算法包含一个Solver，也就是求解器。不论使用了什么优化算法，每次迭代都需要求解一个 H∆x=g 的增量方程，其中H是Hessian矩阵或其变体，∆x是待求的优化变量的更新量。那么如何求解这个方程，就是Solver的工作了。在g2o中，只提供了一个实现类BlockSolver<>。所谓块求解器，就是利用A矩阵的稀疏性，每个优化变量和误差项都体现为固定大小的矩阵块，可以利用它的一些性质加速计算。在块求解器中，包含了一个SparseBlockMatrix和LinearSolver，其实前者用来存放H矩阵的数据，后者用来指定具体的线性求解器。之所以叫线性求解器，是因为增量方程是一个线性方程。g2o提供的线性求解器有三个，分别是LinearSolverCSparse<>、LinearSolverCholmod<>和LinearSolverPCG<>。它们之间的不同大概只是对矩阵求逆的方式不同，可能会有速度上的差异，但结果一定一致。

最后，右上角的一大部分是顶点和边，这些比较容易理解，也是我们编程中接触得最多的，这里不再详述。

分析完这个图，基本上g2o的优化流程也就差不多清晰了。但有几个关键的环节刚才并没有提到，而且我的理解也不敢保证没有偏差，写在下面和大家一起交流。

SLAM中有一个加速增量方程求解的方法，称为边缘化。边缘化是说，如果我们把待优化的相机位姿放在H矩阵的左上角，把待优化的路标点放在H矩阵的右下角，再把H矩阵分为四块，就可以对H的矩阵块进行高斯消元，使得对相机位姿的求解不依赖于路标点。这种方法奏效的原因是因为相机矩阵相比于路标点稀疏得多，因此相机矩阵块求逆更容易。当然，具体的分析建议阅读高翔的《视觉SLAM十四讲》。这里我们更关注g2o中的实现。边缘化部分的操作在BlockSolver<>中，块求解器会把对增量方程的求解分为两步，先求解相机位姿的增量，再求解路标点的增量。当然，每次求解增量仍然是调用内部的LinearSolver。思路很清晰，但g2o在这里的实现却有待商榷，块求解器中根据顶点是否被边缘化决定该顶点是位姿顶点还是路标点顶点，也就是说，它默认所有位姿顶点都不被边缘化，所有路标点顶点都被边缘化。假如你尝试不边缘化路标点顶点，或边缘化位姿顶点，求解器都会报错，这就限制了我们优化的灵活性。在我看来，这可能是g2o作者实现过程中的一个瑕疵。BlockSolver并没有体现出其应有的抽象，它应该根据实际的顶点类型来决定如何实现边缘化，而不是一股脑地认为只有路标点应该被边缘化。（注意，这里提到的边缘化只用于加速增量方程求解，不同于滑动窗口中的边缘化。）

对于上面的问题，其实并非不能解决。如果我们不用固定大小的BlockSolver_6_3，而是用动态大小的BlockSolverX，就不会出问题。因为前者默认维度为6的位姿顶点被边缘化，维度为3的路标点顶点不被边缘化，不符合维度要求的顶点会导致出错。而后者并不要求边缘化的顶点维度为6，也不要求不边缘化的顶点维度为3，允许同时存在维度为6和维度为3的顶点被边缘化。但问题解决并不意味着g2o的设计没有问题，BlockSolver中把顶点维度和是否边缘化在语义层面绑定了起来，很容易造成误解。举个例子，如果我想边缘化所有位姿顶点，不边缘化路标点顶点，最简单的解决方案是使用BlockSolverX。但一般认为，固定矩阵大小可以把一部分运行时时间转移到编译期。所以我可能需要typedef g2o::BlockSolver> BlockSolver_3_6;，相当于把BlockSolverTraits的第一个模板参数_PoseDim设为3，第二个模板参数_LandmarkDim设为6，也就是把路标点当成位姿，把位姿当成路标点。虽然可以用，但实在太过别扭。

总体上看，我认为g2o是一个结构良好的图优化框架，它的类层次结构提供了很高的可扩展性。但遗憾的是，实际实现的功能并不多，很多类的派生类都只有一个，比如只有SparseOptimizer而没有DenseOptimizer，只有OptimizationWithHessian而没有OptimizationWithOthers，只有BlockSolver<>而没有PlainSolver。以至于g2o只能用于求解视觉SLAM问题，应用范围有些狭隘。但毕竟g2o没有企业在背后支持，不像Ceres有谷歌撑腰，搞不好以后做SLAM的标配会变成Ceres吧，感觉有些可惜。

文末给出了一些详细的参考资料，比本文更有价值，大家可以参考。

参考资料

《视觉SLAM十四讲》 高翔
g2o学习——g2o整体框架 无人的回忆
g2o学习——顶点和边之外的solver 无人的回忆
A General Framework for Graph Optimization R Kümmerle