g2o原生位姿图

（用g2o_viewer打开/slambook2/ch10/sphere.g2o）

仿真位姿图（sphere.g2o）

它的真实轨迹为一个球，由从下往上的多个层组成。
每层为一个正圆形，很多个大小不一的圆形层组成了一个完整的球体，共包含2500个位姿节点，可看成一个转圈上升的过程。
仿真程序生成了t-1到t时刻的边（里程计边）
层与层之间的边（回环）
在每条边上添加观测噪声，并根据里程计边的噪声，重新设置节点的初始值。这样就得到了带累积误差的位姿图数据。
它局部看起来像球体的一部分，但整体形状与球体相差甚远。

现在，我们从带噪声的边和节点初始值出发，尝试优化整个位姿图，得到近似真值的数据
仿真成球的好处是能够直观地看到优化结果是否正确（只要看它各个角度圆不圆就行了）

单击g2o_viewer中的optimize函数，看到每步的优化结果和收敛过程

sphere.g2o也是一个文本文件
(终端执行)

vim ~/Documents/slambook2/ch10/sphere.g2o

节点类型VERTEX_SE3，表达一个相机位姿
g2o默认使用四元数和平移向量表达位姿
各字段含义：ID、$t_x$、$t_y$、$t_z$、$q_x$、$q_y$、$q_z$、$q_w$
(前3个为平移向量元素,后4个表示旋转的单位四元数)

边类型EDGE_SE3:QUAT，表达两个位姿节点之间相对运动的估计
边的信息为两个节点的ID、$t_x$、$t_y$、$t_z$、$q_x$、$q_y$、$q_z$、$q_w$、以及信息矩阵的右上角（本例中信息矩阵大小为$6\times 6$且被设成了对角阵）

如何用Levenberg-Marquart方法对该位姿图进行优化，并将结果存储至result.g2o文件中

CMakeLists.txt

cmake_minimum_required(VERSION 3.13)
project(g2o_SE3)

set(CMAKE_CXX_STANDARD 14)

list(APPEND CMAKE_MODULE_PATH ${g2o_SE3_SOURCE_DIR}/cmake_modules)

find_package(G2O REQUIRED)

include_directories(${G2O_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")

add_executable(g2o_SE3 pose_graph_g2o_SE3.cpp)

target_link_libraries(g2o_SE3 ${CHOLMOD_LIBRARIES} g2o_core g2o_stuff g2o_types_slam3d)

pose_graph_g2o_SE3.cpp

#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

using namespace std;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
 * **********************************************/

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // 设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    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);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            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->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if (!fin.good()) break;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;
    optimizer.save("result.g2o");

    return 0;
}

在IDE终端中执行以下命令（工作路径为该项目的路径）

mkdir build
cd build
cmake ..
make
cd ..
./build/g2o_SE3 sphere.g2o
#用g2o_viewer打开程序生成的优化位姿文件result.g2o
/usr/local/bin/g2o_viewer result.g2o

Result

结果从一个不规则的形状优化成了一个看起来完整的球。这个过程实质上和我们单击g2o_viewer上的Optimize按钮没有区别