从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
从零开始一起学习SLAM | 掌握g2o顶点编程套路
从零开始一起学习SLAM | 掌握g2o边的代码套路
什么是图？
图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成，给你举个例子
比如一个机器人在房屋里移动，它在某个时刻 t 的位姿（pose）就是一个顶点，这个也是待优化的变量。而位姿之间的关系就构成了一个边，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表示误差项。
在SLAM里，图优化一般分解为两个任务：
1、构建图。机器人位姿作为顶点，位姿间关系作为边。
1. 图的核心
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）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个（我们常用的是GN和LM）
4. 求解
OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod
采用十四讲中g2o求解曲线参数的例子来说明：
1. 创建线性求解器LinearSolver
我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。
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，用上面我们定义的线性求解器LinearSolver来初始化。
BlockSolver有两种定义方式
一种是指定的固定变量的solver，我们来看一下定义
其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度
另一种是可变尺寸的solver，定义如下
比较常用的几种类型
- 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初始化
Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法
GN、 LM、 Doglet算法内部，会发现他们都继承自同一个类：OptimizationWithHessian
OptimizationAlgorithmWithHessian，发现它又继承自OptimizationAlgorithm
总之，在该阶段，我们可以选则三种方法：
4. 创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法。
创建稀疏优化器
用前面定义好的求解器作为求解方法：
其中setVerbose是设置优化过程输出信息用的
5. 定义图的顶点和边。 添加到SparseOptimizer中 （最重要的一步）
顶点（Vertex) 从哪里来的？
一步步来看吧。先来看看上图中和vertex有关的第①个类： HyperGraph::Vertex，在g2o的GitHub上（https://github.com/RainerKuemmerle/g2o），它在这个路径
g2o/core/hyper_graph.h
这个 HyperGraph::Vertex 是个abstract vertex，必须通过派生来使用。如下图所示
然后我们看g2o 类结构图中第②个类，我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的， 而OptimizableGraph的定义在
g2o/core/optimizable_graph.h
我们找到vertex定义，发现果然，OptimizableGraph 继承自 HyperGraph，如下图所示
不过，这个OptimizableGraph::Vertex 也非常底层，具体使用时一般都会进行扩展，因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中 对应的第③个类：
BaseVertex
那么它在哪里呢？ 在这个路径：
g2o/core/base_vertex.h
BaseVertex->OptimizableGraph->HyperGraph::Vertex
继承关系是HyperGraph::Vertex继承OptimizableGraph继承BaseVertex
顶点（Vertex) 参数如何理解？
我们来看一下模板参数 D 和 T，翻译一下上图红框：
D是int 类型的，表示vertex的最小维度，比如3D空间中旋转是3维的，那么这里 D = 3
T是待估计vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion 类型
可以看到这个D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示，这里一定要区别开，另外，源码里面也给出了T的作用
可以看到，这里T就是顶点（状态变量）的类型
如何自己定义顶点？
顶点的基本类型是 BaseVertex，那么下一步关心的就是如何使用了，因为在不同的应用场景（二维空间，三维空间），有不同的待优化变量（位姿，空间点），还涉及不同的优化类型（李代数位姿、李群位姿）
g2o本身内部定义了一些常用的顶点类型
当然我们可以直接用这些，但是有时候我们需要的顶点类型这里面没有，就得自己定义了。
重新定义顶点一般需要考虑重写如下函数：
这几个是主要要改的地方。我们来看一下他们都是什么意义：
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
setToOriginImpl：顶点重置函数，设定被优化变量的原始值。
oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。
自己定义 顶点一般是下面的格式：
当我们使用Eigen时，在类中需要重载内存分配的new delete函数时，加上这句话即可：EIGEN_MAKE_ALIGNED_OPERATOR_NEW ，比如：使用g2o优化时，定义一个顶点，需要使用eigen中的一些底层函数
先看一个简单例子，来自十四讲中的曲线拟合，来源如下
ch6/g2o_curve_fitting/main.cpp
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
我们可以看到下面代码中顶点初值设置为0，更新时也是直接把更新量 update 加上去的，知道为什么吗？
小白：更新不就是 x + △x 吗，这是定义吧
师兄：嗯，对于这个例子是可以直接加，因为顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新的。但是但是有些例子就不行，比如下面这个复杂点例子：李代数表示位姿VertexSE3Expmap
来自g2o官网，在这里
g2o/types/sba/types_six_dof_expmap.h
第一个参数6 表示内部存储的优化变量维度，这是个6维的李代数
第二个参数是优化变量的类型，这里使用了g2o定义的相机位姿类型：SE3Quat。
在这里可以具体查看g2o/types/slam3d/se3quat.h
它内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作
说完了，那我现在问你个问题，为啥这里更新时没有像上面那样直接加上去？
小白：这个表示位姿，好像是不能直接加的我记得，原因有点忘了
师兄：嗯，是不能直接加，原因是变换矩阵不满足加法封闭。那我再问你，为什么相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，而不是使用旋转矩阵和平移矩阵？
其实也是上述原因的拓展：这是因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示，就可以把位姿估计变成无约束的优化问题，求解难度降低。
刚才是位姿的例子，下面是三维点的例子，空间点位置 VertexPointXYZ，维度为3，类型是Eigen的Vector3，比较简单，就不解释了
如何向图中添加顶点？
往图中增加顶点比较简单，我们还是先看看第一个曲线拟合的例子，setEstimate(type) 函数来设定初始值；setId(int) 定义节点编号
这个是添加 VertexSBAPointXYZ 的例子，都很容易看懂
+++
以上为设置顶点，以下为边
+++
初步认识g2o的边
上一次我们讲顶点的时候，还专门去追根溯源查找顶点类之间的继承关系，边其实也是类似的，我们在g2o官方GitHub上这些
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h
下面我们来看看他们的参数有什么区别？你看主要就是 几个参数：D, E, VertexXi, VertexXj，他们的分别代表：
D 是 int 型，表示测量值的维度 （dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型
比如我们用边表示三维点投影到图像平面的重投影误差，就可以设置输入参数如下：
首先这个是个二元边。第1个2是说测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap
除了输入参数外，定义边我们通常需要复写一些重要的成员函数
顶点里主要复写了顶点更新函数oplusImpl和顶点重置函数setToOriginImpl
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError函数：非常重要，是使用当前顶点的值计算优化的估计值与真实的测量值之间的误差
linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian矩阵
除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：
- _measurement：存储观测值
- _error：存储computeError() 函数计算的误差
- __vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
- setId(int)：来定义边的编号（决定了在H矩阵中的位置）
- setMeasurement(type) 函数来定义观测值
- setVertex(int, vertex) 来定义顶点
- setInformation() 来定义协方差矩阵的逆
如何自定义g2o的边？
先来看一个简单例子，地址在
https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp
这个是个一元边，主要是定义误差函数了，如下所示，你可以发现这个例子基本就是上面例子的一丢丢扩展
下面是一个复杂一点例子，3D-2D点的PnP 问题，也就是最小化重投影误差问题，这个问题非常常见，使用最常见的二元边，弄懂了这个基本跟边相关的代码也差不多都一通百通了
代码在g2o的GitHub上这个地方可以看到
g2o/types/sba/types_six_dof_expmap.h
这里根据自己理解对代码加了注释，方便理解
有一个地方比较难理解
小白：我确实看不懂这一句。。
师兄：其实就是：误差 = 观测 - 投影
捋捋思路。我们先来看看cam_map 函数，它的定义在
g2o/types/sba/types_six_dof_expmap.cpp
cam_map 函数功能是把相机坐标系下三维点（输入）用内参转换为图像坐标（输出），具体代码如下所示
然后看 .map函数，它的功能是把世界坐标系下三维点变换到相机坐标系，函数在
g2o/types/sim3/sim3.h
具体定义是
因此下面这个代码
就是用V1估计的pose把V2代表的三维点，变换到相机坐标系下。
前面主要是对computeError() 的理解，还有一个很重要的函数就是linearizeOplus()，用来定义雅克比矩阵
我摘取了相关代码（来自：g2o/g2o/types/sba/types_six_dof_expmap.cpp），并进行了标注，相信会更容易理解
如何向图中添加边
一元边的添加方法
下面代码来自GitHub上，仍然是前面曲线拟合的例子
slambook/ch6/g2o_curve_fitting/main.cpp
小白：setMeasurement 函数的输入的观测值具体是指什么？
师兄：对于这个曲线拟合，观测值就是实际观测到的数据点。对于视觉SLAM来说，通常就是我们我们观测到的特征点坐标，下面就是一个例子。这个例子比刚才的复杂一点，因为它是二元边，需要用边连接两个顶点
代码来自GitHub上
slambook/ch7/pose_estimation_3d2d.cpp
小白：这里的setMeasurement函数里的p来自向量points_2d，也就是特征点的图像坐标(x,y)了吧！
师兄：对，这正好呼应我刚才说的。另外，你看setVertex 有两个，一个是 0 和 VertexSBAPointXYZ 类型的顶点，一个是1 和pose。你觉得这里的0和1是什么意思？能否互换呢？
小白：0，1应该是分别指代哪个顶点吧，直觉告诉我不能互换，可能得去查查顶点定义部分的代码
师兄：你的直觉没错！我帮你 查过啦，你看这个是setVertex在g2o官网的定义：
这段代码在
g2o/core/hyper_graph.h
里可以找到。你看 _vertices[i] 里的i就是我们这里的0和1，我们再去看看这里边的类型： g2o::EdgeProjectXYZ2UV
的定义，前面我们也放出来了，就这两句
你看 _ vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点，也就是三维点， _vertices[1] 对应的是VertexSE3Expmap 类型的顶点，也就是位姿pose。
因此前面 1 对应的就应该是 pose，0对应的 应该就是三维点。
6. 设置优化参数，开始执行优化
设置SparseOptimizer的初始化、迭代次数、保存结果等
初始化
设置迭代次数，开始执行图优化
- 练习题目：
题目：给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)，以及相机的内参矩阵。使用bundle adjustment 方法（g2o库实现）来估计相机的位姿T。初始位姿T为单位矩阵。