g2o的简介、安装、使用说明和曲线拟合例程

• g2o的论文：g2o: A General Framework for Graph Optimization
  (在源码文件中…g2o/doc/g2o.pdf，有更详细版本)
• g2o的github地址：https://github.com/RainerKuemmerle/g2o

g2o(General Graphic Optimization)是基于图优化的库。图优化是把优化问题表现成图的一种方式。一个图由若干个顶点(Vertex)，以及连接这这些顶点的边(Edge)组成。用顶点表示优化变量，用边表示误差项。

对任意非线性最小二乘问题都可以构建一个与之对应的图。也可以用概率图里的定义，称之为贝叶斯图或因子图。

以相机为例，相机不同时刻的位姿和路标点可以表示为顶点，相机的运动模型可以表示相机之间的边，相机的观测模型可以表示相机与路标之间的边。如下关系图：

安装依赖项：

安装下列命令依次执行安装：

安装完成后在目录/usr/local/includ 下能找到g2o目录，在/usr/local/lib 下能找到libg2o_**.so的文件。

g2o使用的大致步骤：

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

如果g2o的标准顶点和边不能满足我们的需求，一般需要对它们进行重写。标准顶点和边为：

• 顶点部分说明：

1. oplusImpl(计算下一次的估计值，相当于一次setEstimate)：函数处理的是 xk+1 = xk + ∆x 的过程；
2. setToOriginImpl(清零：_estimate)：估计值清零；
3. setEstimate(设置：_estimate)：设置估计值，一般会有一个初始值；(在曲线拟合中有了估计值，也就可以算出y的估计值。)。
4. setId（设置顶点的id：_id）
5. estimate（ 返回： _estimate）：优化的结果。
6. addVertex（插入到_vertices，vertices()函数可以返回_vertices的值。）：添加顶点
7. setFixed：要优化的变量设置为false。
8. setMarginalized ():设置该顶点是否被边缘化，_marginalized默认为false

• 边的说明

1. computeError(返回：_error)：边的误差项，观测值与估计值的差距；(曲线拟合中y的测量值与估计值的差)。
2. setVertex（设置：_vertices）：设置连接的顶点
3. setMeasurement(设置：_measurement)：观测数据的输入，在曲线拟合中也就是y真实值(x值会在构建边的时候输入)；
4. setId：边的id
5. addEdge：v->edges().insert(e);添加边的数据，edges函数可以返回_edges的值。
6. setInformation：设置信息矩阵(_information)

• 优化器说明

1. 可以选择LM、GN或者DogLeg方法

设置方式(_algorithm)：

• 设置停止优化标志

• 设置是否打开调试器

• （1）李代数顶点

• 参数6 ：SE3Quat类型为六维，三维旋转，三维平移
• 参数SE3Quat ：该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数

该顶点需要设置的参数：

• （2）空间点位置

该顶点需要设置的参数：

• （1）Point-Pose 二元边（PointXYZ-SE3边）

即要优化MapPoints的位置，又要优化相机的位姿

需要设置的模板参数：

• 参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度
• 参数Vector ：观测值类型，piexl.x，piexl.y
• 参数VertexSBAPointXYZ：第一个顶点类型
• 参数VertexSE3Expmap ：第二个顶点类型

该边需要设置的参数：

• （2） Pose 一元边（SE3）

仅优化相机位姿，为了构造出投影方程，需要按下面的方式把MapPoints的位置作为常量加入

该继承于BaseUnaryEdge这个一元边模板类，需要设置的模板参数如上。

该边需要设置的参数：

• （3） Pose-Pose 二元边（SE3-SE3边）

优化变量是相机相邻两个关键帧位姿，约束来自对这两个关键帧位姿变换的测量（里程计、IMU等）

需要设置的参数如下：

上面的比较麻烦的协方差的计算，准确的协方差计算可以参考论文《Odometry-Vision-Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses》中的处理.

g2o中经常使用的BlockSolver_6_3、BlockSolver_7_3 即是上面程序中【1】所作的事，如下：

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化。

第二次优化完成后，会对连接偏差比较大的边，在关键帧中剔除对该MapPoint的观测，在MapPoint中剔除对该关键帧的观测，具体实现参考orb-slam源码：Optimizer::LocalBundleAdjustment

如果不想优化相机内参，则内参按照4.2中二元边中的程序demo中设置。

参考《视觉slam十四讲》的曲线拟合代码。

使用cmake进行编译，cmake的具体使用步骤可以参考链接。

使用步骤：

代码说明：

在这个程序中，我们从 g2o 派生出了用于曲线拟合的图优化顶点和边：CurveFittingVertex 和 CurveFittingEdge，这实质上是扩展了 g2o 的使用方式。在这两个派生类中，我们重写了重要的虚函数：

• 顶点的更新函数：oplusImpl。我们知道优化过程最重要的是增量 ∆x 的计算，而该函数处理的是 xk+1 = xk + ∆x 的过程。每个优化的加法运算不一致，可能还包括向量，所以需要重写。
• 顶点的重置函数：setToOriginImpl。这是平凡的，我们把估计值置零即可。
• 边的误差计算函数：computeError。该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
• 存盘和读盘函数：read, write。由于我们并不想进行读写操作，就留空了。

---

续着本文第3节内容，记录g2o内部的结构。g2o的整体结构如下图所示。

• 实线箭头:is - a关系，表示从派生类指向基类；
• 虚线箭头：has - a关系，表示成员变量；
• 多虚线箭头：has - many关系，多个成员变量。Vertex和Edge都是class，在HyperGraph中分别定义了它们的集合set。

• g2o结构说明如下：

1. 整个g2o框架可以分为上下两部分，两部分中间的连接点：SparseOptimizer 就是整个g2o的核心部分。
2. 往上看，SparseOptimizer是一个派生类，其实是一个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中选择。
5. 对优化算法部分进行求解的时求解器solver，它实际由BlockSolver组成。BlockSolver由两部分组成：一个是SparseBlockMatrix，它由于求解稀疏矩阵(雅克比和海塞)；另一个部分是LinearSolver，它用来求解线性方程 得到待求增量，因此这一部分是非常重要的，它可以从PCG/CSparse/Choldmod选择求解方法。

• 把g2o步骤再写一遍：

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

依次说明具体步骤：

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

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

1. LinearSolverCholmod ：使用sparse cholesky分解法，继承自LinearSolverCCS。
2. LinearSolverCSparse：使用CSparse法，继承自LinearSolverCCS。
3. LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver。
4. LinearSolverDense ：使用dense cholesky分解法，继承自LinearSolver。
5. LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多，继承自LinearSolver。

_batchStatistics：全局统计信息，包括顶点和边的数量、时间、cholesky因子等信息。
_ivMap：初始化之后的顶点。

2. 创建BlockSolver，并用定义的线性求解器初始化

BlockSolver有两种定义方式：

此外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. 创建图优化的核心：稀疏优化器

创建稀疏优化器：

设置求解方法：

设置优化过程输出信息：

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

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

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

初始化：

设置迭代次数：

---

参考：
《视觉SLAM十四讲》

g2o使用总结：https://blog.csdn.net/hzwwpgmwy/article/details/79884070