ORB-SLAM3g2o之开篇

1 什么是图优化？

首先，认识一下啥叫图。图是一种数据结构。
图的定义就是由顶点的有穷非空集合和顶点之间边的集合组成，像下面这样：

当然，我们不在这里谈论算法和数据结构的内容，我们只需要感性的理解到图是由若干顶点和连接这些顶点的边组成的就好了。
举个小例子来理解图优化要干个什么事。

我们去“蜀留香“吃火锅时（非广告哈）会看到有一个送餐机器人在火锅店里移动，它在某个时刻 $t$ 的位姿（$pose$）就是一个顶点，这个就是待优化的变量。 而位姿之间的关系就构成了一个边，比如时刻 $t$ 和时刻 $t+1$ 之间的相对位姿变换矩阵就是边，这个边通常就表示误差项。

我们知道，在SLAM优化问题中，我们经常求解一个代价函数：

从举得例子来看，我们可以这样理解，图优化就是这个代价函数的求解问题，待优化的位姿和地图点就是图的顶点，而误差项是图的边。
这样我们可以这样概括图优化要干的事：

1、构建图。机器人位姿作为顶点，位姿间关系作为边。
2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

2 图优化的优势在哪里？

在SLAM后端算法领域，一般分为两种处理方法，一种是基于滤波的方法，特别是以扩展卡尔曼滤波（EKF）为代表；一种是基于非线性优化的方法，以图优化为代表。
不过，现在研究的主流是图优化的方法，因为图优化可以把SLAM系统的历史状态量和路标点位置考虑进去一起优化，这大大提高了轨迹和地图的精度。
在以图优化框架的视觉 $SLAM$ 算法里，$BA$ 起到了核心作用。基于视觉的SLAM方案，路标点（特征点）数据很大，意味着$BA$求解的计算量也很大，没法做到实时。But~，近十年来，研究者们发现，虽然视觉 SLAM 问题中 包含大量特征点和相机位姿，但是$BA$ 有着稀疏的特性，这使得它能够在实时的场景中使用。图优化成为主流方法的原因就是人们注意到了$BA$的稀疏特性！

3 了解g2o框架

在SLAM领域，基于图优化的一个用的非常广泛的库就是g2o，是一个用来优化非线性误差函数的c++框架。

其实g2o帮助我们实现了很多内部的算法，只是在进行构造的时候，需要遵循一些规则，毕竟一个程序不可能满足所有的要求，因此在以后g2o的使用中还是应该多看多记，这样才能更好的使用这个库。

首先，来了解一下g2o的基本框架图。

还记得我们说过SLAM优化问题就是要求解一个代价函数吧，还有图优化的两步走–构建图和优化图。
我们在图的组成部分可以看到，图是由许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）构成的。这些顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边）。
在稀疏求解器部分又可以看到：整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个。而求解算法呢？我们可以看到OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod。

4 g2o编程步骤

捋顺了上面的基本框架，我们再来理解构建图优化的编程步骤，同样照着图来理解图优化是怎么编程实现的。

我们以高翔《视觉SLAM十四讲》中的pose_graph_g2o_SE3.cpp程序说明，代码地址在这儿
程序中读取的是sphere.g2o文件。他的格式如下（只截取了两行代表性数据）
第一行类型数据表示顶点，表达一个相机位姿。g2o 默认使用四元数和平移向量表达位姿，所以后面的字段意义为：ID，tx, ty, tz, qx, qy, qz, qw。第二行类型数据表示边。边的信息为：两个节点的 ID，tx, ty, tz, qx, qy, qz, qw，及其信息矩阵的右上角元素（信息矩阵是对称矩阵）。

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;// 每个误差项优化变量维度为6，误差值维度为6。
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    //这条语句包含了第一、二、三步，从内到外分别是：
    //1、创建线性求解器；2、创建由线性求解器初始化的块求解器；3、创建一个由块求解器初始化的总求解器solver
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    //第四步：创建终极大boss 稀疏优化器（SparseOptimizer）
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出
	
	// 第五步：定义图的顶点和边。并添加到SparseOptimizer中
    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // SE3 顶点
            g2o::VertexSE3 *v = new g2o::VertexSE3();
            int index = 0;
            fin >> index;
            v->setId(index);//设置顶点id
            v->read(fin);//读入顶点数据
            optimizer.addVertex(v);//将顶点加入优化器中
            if (index == 0)
                v->setFixed(true);//固定第一个顶点不优化它
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            g2o::EdgeSE3 *e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin >> idx1 >> idx2;
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);//读入边数据
            optimizer.addEdge(e);//添加边
        }
        if (!fin.good()) break;
    }
	// 第六步：设置优化参数，开始迭代优化
    optimizer.initializeOptimization();//初始化优化器
    optimizer.optimize(30);//设置迭代次数,并优化

5 非常感谢您的阅读！