SLAM十四讲 ch10 pose_graph_g2o_SE3.cpp 代码详解

g2o原生位姿图优化

这个代码的位姿图优化使用的顶点和边是g2o默认的顶点和边（g2o默认使用四元素和平移向量表达位姿）。

sphere.g2o位姿图中的顶点和边的信息如下所示

该位姿图的真实轨迹为一个球，添加噪声后得到的带累积误差的位姿图如下所示，尝试优化它得到近似真实的数据。

#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;

int main(int argc, char **argv) 
{
    string filepath = "../sphere.g2o";
    ifstream fin(filepath); // 创建输入流对象fin
    if (!fin)  // 判断文件是否正确读取
    {
        cout << "file " << filepath << " does not exist." << endl;
        return 1;
    }

    // 设定g2o
    // 设置优化变量维度（6），误差值维度（6），求解器类型为线性。注意：信息矩阵=协方差矩阵的逆，其大小=(误差值维度,误差值维度)
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    
    // 设置梯度下降方法（这里使用L-M），make_unique是智能指针，有助于代码管理
    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);  // 设置顶点ID
            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");  // 保存优化结果（.g2o文件）

    return 0;
}

优化后的位姿如下图所示：