g2o库的使用



，与并列为目前视觉SLAM中应用最广泛的优化算法库。它最大的特点就是基于图优化设计，这对于视觉SLAM来说是异常适配的。在很多的SLAM库的中都使用到它来进行优化操作，比如ORB-SLAM。

代码仓库：https://github.com/RainerKuemmerle/g2o。

本文所对照的源码版本是2023.9.1日期，因此和网上其他的一些教程版本有些出入，但是总体相差不大。总体来说，差别在BlockSolver的接口由裸指针变成了unique_ptr、VertexSBAPointXYZ类被删除等。具体可以看：SLAM十四讲ch7代码调整（undefined reference to symbol）。

G2O的全称为：，通用图优化。为何叫通用呢？

G2O的核里带有各种各样的求解器，而它的顶点、边的类型则多种多样。通过自定义顶点和边，事实上，只要一个优化问题能够表达成图，那么就可以用G2O去求解它。常见的，比如bundle adjustment、ICP、数据拟合，都可以用G2O来做。

所谓图优化，是把优化问题表现成图的一种方式，这里的图是图论意义上的图。一个图由若干个顶点，以及连着这些顶点的边组成。在这里，我们用顶点表示优化变量，而用边表示残差项。

下图展示的就是一些优化问题的图的表现形式，左图是诸如曲线拟合等一元边，右图是诸如BA等二元边的情况：

下图是G2O的基础框架结构，它几乎包含了所有重要的类之间的关系：

其中：

• SparseOptimizer：，它有基类OptimizableGraph（可优化图），又有基类HyperGraph（超图）
• Vertex：，它是图的数据成员，会有很多定制化的子类
• Edge：，它是图的数据成员，会有很多定制化的子类
• OptimizationAlgorithm：，它是图的数据成员，可以指定Gauss-Newton、Levernberg-Marquardt、Powell’s dogleg等迭代算法
• Solver：，它是优化算法的数据成员，它有两个部分，计算稀疏（Schur舒尔补）的雅可比和Hessian矩阵（SparseBlockMatrix），计算迭代过程中最关键的一步 （LinearSolver）

从顶层到底层梳理完整个的基础框架结构，在编程实现时需要反过来，从底层开始搭建框架一直到顶层：

1. 【STEP1】构建一个优化求解器LinearSolver、BlockSolver、OptimizationAlgorithm
2. 【STEP2】构建稀疏优化器SparseOptimizer
3. 【STEP3】定义并添加图的顶点Vertex、边Edge
4. 【STEP4】设置优化参数，开始执行优化

一个简单的代码实现是：

下面将会对其中的重要的几个STEP进行详细讲解。

求解器的有三种：LinearSolver、BlockSolver、OptimizationAlgorithm。

线性求解器LinearSolver

由于在迭代的过程中，基本都需要求解近似于 的增量方程。

通常情况下想到的方法就是直接求逆，也就是 。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。

因此，G2O提供了各种求解该线性方程的线性求解器，这里就直接总结一下几种预设：

当然，它们的源码在g2o/g2o/solver目录下，感兴趣的可以自行查看一下。

BlockSolver

BlockSolver内部包含LinearSolver，这就需要用到上面定义的线性求解器LinearSolver来对它进行初始化。在该类中，会对稀疏（Schur舒尔补）的雅可比和Hessian矩阵进行计算，这部分由SparseBlockMatrix负责。但由于SparseBlockMatrix部分比较固定，一般不做什么修改或自定义，因此，初始化的时候仅有LinearSolver。

在源码g2o/g2o/core/block_solver.h中，可以发现它主要有两种形式：

为何会有可变尺寸的BlockSolver呢？

这是因为在某些应用场景，我们的Pose和Landmark在程序开始时并不能确定，那么此时这个块状求解器就没办法固定变量，此时使用这个可变尺寸的BlockSolver，所有的参数都在中间过程中被确定。

当然，G2O也预定义了比较常用的几种类型：

OptimizationAlgorithm

总优化求解器OptimizationAlgorithm用于指定优化算法，主要有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法。它们都定义在源码g2o/g2o/core目录下。

这三种优化算法分别对应下面的三个类：

顶点表示待优化变量。如果需要估计3D位置、3D位姿等，直接就定义/创建该内容对应的顶点类型对象即可。

定义顶点

顶点Vertex的主要继承关系为：HyperGraph::Vertex（最基类）、OptimizableGraph::Vertex（较基类）、BaseVertex、BaseDynamicVertex（派生类）。其中，后两者是比较通用的适合大部分情况的模板，另外两个类都比较底层，通常不会直接使用。

BaseVertex、BaseDynamicVertex的源码定义在g2o/g2o/core文件夹内：

其中：

• D：int类型的数字，表示Vertex的最小维度，更准确的说，是其在流形空间（manifold）的最小表示，比如3D空间中旋转是3维的，那么这里D=3
• T：待估计的Vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion类型

我们可以看到，G2O的顶点分为两种类型的顶点，固定长度的顶点（BaseVertex）、可变长度的顶点（BaseDynamicVertex）。

因此，我们就需要根据不同的场景实现特定的Vertex就行了。这里的不同场景包括：不同的应用场景（二维空间、三维空间），不同的待优化变量（位姿、空间点），不同的优化类型（李代数位姿、李群位姿）等。

G2O也预设了一些比较常用的顶点类型，可以直接使用：

当然我们可以直接用这些，但是有时候我们需要的顶点类型这里面没有，就得自己定义了。重新定义顶点一般需要考虑重写如下函数（有时也需要重写其他函数）：

除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

就以官方源码中预设的VertexPointXYZ顶点类型为例：

再以官方源码中较复杂的预设的VertexSE3顶点类型为例：

添加顶点

往优化器中增加顶点比较简单，一般来说直接调用接口就行了：

边表示残差项。每条边对应着若干个顶点（）和一个测量值，依靠对应的顶点可以计算出一个和测量值意义一致的估计值，而这个估计值和测量值之间的误差则表示对应的残差。

定义边

边Edge的主要继承关系为：HyperGraph::Edge（最基类）、OptimizableGraph::Edge、BaseEdge（较基类）、BaseUnaryEdge、BaseBinaryEdge、BaseMultiEdge（派生类）。其中，后两者是比较通用的适合大部分情况的模板，另外三个类都比较底层，通常不会直接使用。

BaseUnaryEdge、BaseBinaryEdge、BaseMultiEdge的源码定义在g2o/g2o/core文件夹下：

其中：

• D：int类型的数字，表示测量值的最小维度
• T：测量值的数据类型
• VertexTypes：该边对应的顶点的数据类型

我们可以看到，G2O的边分为两种类型的边，固定长度的边（BaseFixedSizedEdge）、可变长度的边（BaseVariableSizedEdge）。其中，固定长度的边又分为BaseUnaryEdge（一元边）、BaseBinaryEdge（二元边），可变长度的边又引申出了BaseMultiEdge（多元边）。

因此，我们就需要根据不同的场景实现特定的Edge就行了。这里的不同场景包括：不同的测量值数据类型，不同的顶点类型等。假设，我们用边来表示三维点投影到图像平面的重投影残差：

其中：

• 2：测量值的维度
• Vector2D：测量值的数据类型
• VertexPointXYZ：顶点类型，空间三维点
• VertexSE3：顶点类型，3D位姿

也就是说，此时该边连接的是两个顶点，空间三维点VertexPointXYZ、3D位姿VertexSE3，根据这两个点再结合已知的相机内参可以计算出该空间三维点在图像平面的坐标。而此时该边还有一个测量值Vector2D，它的维度为2，也是一个图像平面的坐标。此时重投影误差就可以通过两个坐标计算出来了。

G2O也预设了一些比较常用的边类型，可以直接使用：

当然我们可以直接用这些，但是有时候我们需要的边类型这里面没有，就得自己定义了。重新定义边一般需要考虑重写如下函数（有时也需要重写其他函数）：

需要注意的是，一旦指定了linearizeOplus()，就会使用该雅可比矩阵进行优化；如果不对该函数进行重写，就会使用自动求导的方式进行优化。

除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

需要注意的是，_vertices内存放了该边对应的顶点信息，它可以通过方括号[]运算符进行取顶点操作，它的顺序完全由setVertex()的第一个size_t类型的参数决定的。

就以官方源码中预设的EdgeSE3边类型为例（边对应的两个顶点都是3D位姿，对应的观测值是两个顶点之间的相对3D位姿测量）：

再以官方源码中较复杂的预设的EdgeProjectXYZ2UV边类型为例（边对应的两个顶点一个是空间3D点，一个是此时对应的3D位姿，对应的观测值是该空间3D点对应的图像2D坐标，其实就是重投影误差模型）：

这里的雅可比计算需要一些BA的基本功，不太清楚的可以查看文章：BA优化中Jacobian矩阵的计算。这不是本文的重点，就不展开讲述了。

添加边

往优化器中增加边比较简单，一般来说直接调用接口就行了：

G2O执行优化的代码很简单：

如果想要了解一下这两个函数的内部实现流程，这部分代码的实现在源码：g2o/sparse_optimizer.cpp。

有一篇博客中讲解了其中的一些内容，可以参考博文：g2o学习笔记。

CmakeLists.txt配置

示例：曲线拟合

拟合非线性函数的曲线（其中，、、，待拟合数据点：之间均匀生成的100个数据点，加上方差为1的白噪声）。代码如下：

示例：3D与2D的PNP问题

已知一批空间点世界坐标系的3D坐标、它们对应到图像坐标系的像素级2D坐标、相机的内参、已通过其他方法诸如PNP求解出位姿，此时需要通过优化的方法求解出更精准的空间3D点坐标、相机的位姿。

完整代码位置：pose_estimation_3d2d.cpp。关键代码如下所示：

示例：3D与3D的ICP问题

已知一批空间点两个坐标系的3D坐标、已通过其他方法诸如ICP求解出的两个坐标系之间的相对位姿，此时需要通过优化的方法求解出更精准的相对位姿（这里没有优化3D空间点，实际上也是可以进行优化的）。

由于G2O并没有预先设定该类型的边，因此需要手动创建该边的类型。

完整代码位置：pose_estimation_3d3d.cpp。关键代码如下所示：

• 从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
• 从零开始一起学习SLAM | 掌握g2o顶点编程套路
• 从零开始一起学习SLAM | 掌握g2o边的代码套路
• [代码实践] G2O 学习记录（一）：2D 位姿图优化
• [代码实践] G2O 学习记录（二）：3D 位姿图优化

版权声明：

本文来源网络，所有图片文章版权属于原作者，如有侵权，联系删除。

本文网址：https://www.mushiming.com/mjsbk/10177.html