小白：师兄师兄，我最近在钻研 SLAM 中的优化算法时，遇到了一个叫“图优化”的方法。回想以前还学过“凸优化”，这两个名字听起来好像，应该不是同一个概念吧？

师兄：哈哈，你这个问题问得真有水平！虽然中文读音一模一样，但含义天差地别。我们来看英文表达——图优化是 graph optimization 或 graph-based optimization，这里的“图”指的是数据结构中的 graph；而凸优化是 convex optimization，这里的“凸”指的是凸函数。所以光看英文就能轻松区分开。

小白：原来如此！我看 SLAM 里图优化用得非常广泛，也翻过高博的书，但还是一头雾水，求师兄科普一下啊。

师兄：图优化确实是个重要工具，概念其实并不复杂，主要是编程实现时稍微有点绕。

小白：太同意了……那个代码看得我简直怀疑人生。

图优化有什么优势？

师兄：按照老规矩，先聊聊图优化的由来。SLAM 后端通常有两条技术路线：一条是以扩展卡尔曼滤波（EKF）为代表的滤波方法，另一条是以图优化为代表的非线性优化方法。目前 SLAM 研究的主流热点几乎都围绕图优化展开。

小白：滤波方法很早就被提出来了，前人的研究也很深入。那为什么现在图优化成了主流呢？

师兄：你说得没错。滤波方法（尤其 EKF）在 SLAM 发展的很长一段历史里一直占据主导地位，早期大牛们设计了各种滤波器来提升效果，那时候入门 SLAM，EKF 是必修课。简单总结一下滤波方法的优缺点：

优点：在当年计算资源有限、待估计量比较简单的情况下，EKF 等滤波方法相当有效，经常被用在激光 SLAM 中。

缺点：一个明显的短板是存储量会随着状态量呈平方增长——因为要存储协方差矩阵，所以不适用于大型场景。而如今的视觉 SLAM 会涉及大量路标点（特征点），数据量极大，滤波方法根本扛不住，效率极低。

小白：原来是这样。那图优化在视觉 SLAM 中的效率真的很高吗？

师兄：说来话长。不到十年前（感觉还挺久的），大家仍普遍使用滤波方法，因为图优化里的 Bundle Adjustment（以下简称 BA）虽然起到了核心作用，但那时包含大量特征点和相机位姿的 BA 计算量巨大，根本无法实时运行。

小白：啊？那后来发生了什么？（聚精会神听故事中）

师兄：后来研究者们发现，视觉 SLAM 尽管包含大量特征点和相机位姿，但 BA 实际上是稀疏的——稀疏就有办法加速啊！比较有代表性的是 2009 年，几位大神发表了《SBA：A software package for generic sparse bundle adjustment》，再加上计算机硬件发展迅猛，基于图优化的视觉 SLAM 终于可以实现实时运算了！

小白：太厉害了！向大牛们致敬！

图优化是什么？

小白：既然图优化是主流，那我能不能跳过滤波方法直接学图优化呢？反正滤波方法也看不太懂……

师兄：嗯，图优化确实是主流，以后有需要再回去看滤波方法也行。今天咱们就专注讲图优化好啦。

小白：好嘞，那问题来了：到底什么是图优化？

师兄：图优化里的“图”就是数据结构中的图——一个图由若干个顶点（vertex）以及连接这些顶点的边（edge）组成。举个例子：

一个机器人在房间里移动，它在某个时刻 t 的位姿（pose）就是一个顶点，这也是待优化的变量。位姿之间关系就构成一条边，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是一条边，边通常代表误差项。

在 SLAM 中，图优化一般分解成两个任务：

1. 构建图：机器人位姿作为顶点，位姿间的约束关系作为边。

2. 优化图：调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小化。

下面是一个直观的例子。根据机器人位姿（来自编码器或 ICP 匹配）作为图的顶点，图的边就是位姿之间的关系。由于误差存在，机器人建立的地图其实是不准确的（如下图左）。通过设置边的约束，图优化会朝着满足边约束的方向进行优化，最终得到一个优化后的地图（如下图中间），它与真实地图（下图右）非常接近。

小白：哇塞，这个效果也太明显了！刚开始误差那么大，最后全都被校正过来了。

师兄：是啊，所以图优化在 SLAM 中举足轻重，必须掌握！

小白：好，感觉动力满满！咱们开始进入编程模式吧！

先了解 g2o 框架

师兄：前面简单介绍了图优化，你也看到了它的强大能力。那具体怎么编程实现呢？

小白：对啊，有没有现成的库啊？我还只是个“调包侠”。

师兄：这个必须有！在 SLAM 领域，基于图优化的一个非常流行的框架就是 g2o，它是 General Graphic Optimization 的缩写，是一个用来优化非线性误差函数的 C++ 框架。这个库足以满足你调包侠的梦想~

小白：哈哈，太好了，不然打死我也写不出来啊！那这个 g2o 具体怎么用？

师兄：先说安装。g2o 安装很简单，参考 GitHub 官网：https://github.com/RainerKuemmerle/g2o，按步骤操作即可。注意安装前要确认电脑上已经装好了第三方依赖。

小白：好，看起来挺好装的。不过问题是，我看相关的代码感觉特别复杂，不知从何下手。

师兄：别急，第一次接触 g2o 确实会有这种感觉，而且官方文档写得也不算“通俗易懂”。但如果你能理清它的框架，再去看代码，应该很快就能上手。

小白：没错，必须先把框架了然于胸，不然就算勉强看懂别人的代码，自己也不会写啊！

师兄：嗯嗯。其实 g2o 已经帮我们实现了许多内部算法，只是在构建时需要遵循一些规则。这完全可以接受——毕竟一个程序不可能满足所有需求，所以以后用 g2o 时还需要多看多记，才能更好地驾驭这个库。

小白：记住了。养成记笔记的好习惯，还要多练习。

师兄：好，那先看下面这张图，这是 g2o 的基本框架结构。在查资料时很多地方都能看到。看图时要注意箭头类型。

1. 图的核心

小白：师兄，这个图该从哪里开始看？感觉东西好多……

师兄：想知道哪个最重要，就看看箭头源头在哪里。

小白：我看看……好像是最左侧的 SparseOptimizer？

师兄：对了。SparseOptimizer 是整个图的核心。注意右上角的 is-a 实心箭头——这个 SparseOptimizer 是一个 Optimizable Graph，从而也是一个超图（HyperGraph）。

小白：我去，师兄，怎么突然冒出这么多奇怪的术语，都是啥意思啊？

师兄：不需要一个个全都搞懂，不然黄花菜都凉了。暂时只了解名字就好，有些以后用不上，有些以后用到再回来看。遇到重要的我会详细解释。

小白：好。那下一步看哪里？

2. 顶点和边

师兄：先看上面的结构。注意 has-many 箭头——超图包含许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。这些顶点继承自 Base Vertex，也就是 OptimizableGraph::Vertex；边可以继承自 BaseUnaryEdge（单边）、BaseBinaryEdge（双边）或 BaseMultiEdge（多边），它们都叫做 OptimizableGraph::Edge。

小白：头有点晕了，师兄。

师兄：哈哈，不用一个个记，现阶段了解这些就行。顶点和边在编程中很重要，关于它们的定义以后会详细说。下面看底部的结构。

小白：嗯嗯，知道啦！

3. 配置 SparseOptimizer 的优化算法和求解器

师兄：看下面——整个图的核心 SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm 通过 OptimizationWithHessian 来实现，其中迭代策略可以从 Gauss-Newton（高斯牛顿法，GN）、Levenberg-Marquardt（LM 法）、Powell's dogleg 三者中选一个（常用的是 GN 和 LM）。

小白：GN 和 LM 就是我们以前讲过的非线性优化方法中常用的那两种吧。

4. 如何求解

师兄：那么如何求解？OptimizationWithHessian 内部包含一个求解器（Solver），这个 Solver 实际由一个 BlockSolver 组成。BlockSolver 有两个部分：一个是 SparseBlockMatrix，用于计算稀疏的雅可比和 Hessian 矩阵；一个是线性方程的求解器（LinearSolver），用于计算迭代过程中最关键的一步 HΔx=−b。LinearSolver 有几种方法可选：PCG、CSparse、Cholmod，具体定义后面会介绍。

到此，就是上面那张图的简单理解。

一步步带你看懂 g2o 编程流程

小白：师兄，看完了我还是不知道编程时具体该怎么写！

师兄：我正要讲这个。首先需要说明：梳理顺序是从顶层到底层，但编程实现时需要反过来——就像建房子一样，从底层开始搭建框架一直到顶层。g2o 的整个框架就是按下图中标的顺序来写的。

高博在十四讲中用 g2o 求解曲线参数的例子来说明，源代码地址：https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp

为了方便理解，我重新加了注释，如下所示：

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
// 第1步：创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );
// 第3步：创建总求解器solver。从GN, LM, DogLeg中选一个，再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步：创建终极大boss 稀疏优化器（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; isetId(i);
  edge->setVertex( 0, v );
  edge->setMeasurement( y_data[i] );
  edge->setInformation( Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) );
  optimizer.addEdge( edge );
}
// 第6步：设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);

结合上面的流程图和代码，下面一步步解释具体步骤。

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

我们要求解的增量方程形式是：H△X=-b。通常情况下，直接求逆就是 △X=-H.inv*b。看起来很简单，但这有个前提——H 的维度较小，矩阵求逆就能解决问题。但当 H 的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。

小白：那有什么办法吗？

师兄：办法肯定是有的。此时我们需要一些特殊方法对矩阵进行求逆。下面这张图是 GitHub 上 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 内部包含 LinearSolver，用上面定义的线性求解器来初始化。它的定义在如下文件夹内：g2o/g2o/core/block_solver.h

点进去会发现 BlockSolver 有两种定义方式：

一种是指定固定变量的 solver，来看定义：using BlockSolverPL = BlockSolver< BlockSolverTraits >; 其中 p 代表 pose 的维度（注意一定是流形 manifold 下的最小表示），l 表示 landmark 的维度。

另一种是可变尺寸的 solver，定义如下：using BlockSolverX = BlockSolverPL;

小白：为何会有可变尺寸的 solver 呢？

师兄：因为某些场景下，Pose 和 Landmark 在程序开始时并不能确定，此时块状求解器没法固定变量，使用可变尺寸的 solver，所有参数都在中间过程中被确定。

另外，block_solver.h 的最后预定义了比较常用的几种类型：

• BlockSolver_6_3：表示 pose 是6维，观测点是3维，用于3D SLAM中的BA
• BlockSolver_7_3：在 BlockSolver_6_3 基础上多了一个 scale
• BlockSolver_3_2：表示 pose 是3维，观测点是2维

以后遇到，知道这些数字是什么意思就行了。

3. 创建总求解器 solver，从 GN、LM、DogLeg 中选一个，再用块求解器 BlockSolver 初始化

看 g2o/g2o/core/ 目录，发现求解器的优化方法有三种：高斯牛顿（GaussNewton）法、LM（Levenberg–Marquardt）法、Dogleg 法，如下图所示，也和前面的图匹配。

小白：师兄，上图最后那个 OptimizationAlgorithmWithHessian 是干嘛的？

师兄：点进 GN、LM、Doglet 算法内部，会发现它们都继承自同一个类：OptimizationWithHessian，如下图所示，也和我们最前面那张图相符。

然后点进 OptimizationAlgorithmWithHessian，发现它又继承自 OptimizationAlgorithm，也和前面相符。

总之，在该阶段可选的三种方法：

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

4. 创建终极大 boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法

创建稀疏优化器：g2o::SparseOptimizer optimizer;

用前面定义好的求解器作为求解方法：SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中 setVerbose 是设置优化过程输出信息用的：SparseOptimizer::setVerbose(bool verbose)

来看它的定义：

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

这部分比较复杂，下一次再介绍。

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

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

初始化：SparseOptimizer::EdgeSet& eset

设置迭代次数，然后就开始执行图优化：SparseOptimizer::optimize(int iterations, bool online)

小白：终于搞明白 g2o 流程了！谢谢师兄！必须给你个「好看」啊！