1. 基于g2o的位姿图优化入门

任务目标

构建一个包含5个节点和4条边的二维位姿图，使用g2o库进行图优化，使机器人轨迹在闭环约束下趋于一致。通过本案例掌握g2o的核心流程：定义顶点与边、配置优化器、执行优化、读取结果。

环境依赖

• g2o（需编译安装，推荐版本：g2o 1.0.0 或最新master分支）
• Eigen3（g2o的依赖）
• CMake ≥ 3.10，C++11及以上编译器

关键完整代码

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam2d/vertex_se2.h>
#include <g2o/types/slam2d/edge_se2.h>

using namespace g2o;

int main() {
    // 1. 构建优化器
    typedef BlockSolver< BlockSolverTraits<-1, -1> > BlockSolverType;
    typedef LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    // 2. 添加顶点（5个位姿节点，ID 0~4）
    std::vector<VertexSE2*> vertices;
    for (int i = 0; i < 5; ++i) {
        auto* v = new VertexSE2();
        v->setId(i);
        if (i == 0) {
            v->setFixed(true);  // 固定第一个节点，消除自由度
            v->setEstimate(SE2(0, 0, 0));
        } else {
            // 初始估计：沿x轴前进1米（含噪声）
            double noise = (i % 2 == 0) ? 0.1 : -0.1;
            v->setEstimate(SE2(i * 1.0 + noise, 0, 0));
        }
        optimizer.addVertex(v);
        vertices.push_back(v);
    }

    // 3. 添加边（里程计约束 + 闭环约束）
    // 里程计边：相邻节点间相对位姿 (dx=1, dy=0, dtheta=0)
    for (int i = 0; i < 4; ++i) {
        auto* edge = new EdgeSE2();
        edge->setVertex(0, vertices[i]);
        edge->setVertex(1, vertices[i + 1]);
        edge->setMeasurement(SE2(1.0, 0, 0));  // 理想里程计
        edge->setInformation(Matrix3d::Identity());  // 单位信息矩阵
        optimizer.addEdge(edge);
    }
    // 闭环边：节点4应回到节点0附近（形成环路）
    auto* loop_edge = new EdgeSE2();
    loop_edge->setVertex(0, vertices[4]);
    loop_edge->setVertex(1, vertices[0]);
    loop_edge->setMeasurement(SE2(0.2, 0.1, 0.05));  // 检测到回环
    loop_edge->setInformation(Matrix3d::Identity());
    optimizer.addEdge(loop_edge);

    // 4. 执行优化
    optimizer.initializeOptimization();
    optimizer.optimize(50);  // 最多迭代50次

    // 5. 输出优化结果
    std::cout << "Optimization done. Final poses:\n";
    for (int i = 0; i < 5; ++i) {
        SE2 pose = vertices[i]->estimate();
        std::cout << "Vertex " << i << ": x=" << pose.translation().x()
                  << ", y=" << pose.translation().y()
                  << ", theta=" << pose.rotation().angle() << "\n";
    }
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(pose_graph_demo)
set(CMAKE_CXX_STANDARD 11)

find_package(g2o REQUIRED)
find_package(Eigen3 REQUIRED)

add_executable(pose_graph_demo main.cpp)
target_link_libraries(pose_graph_demo g2o_core g2o_types_slam2d g2o_solver_dense)

运行与验证

1. 编译：在源码目录执行 mkdir build && cd build && cmake .. && make
2. 运行：./pose_graph_demo
3. 通过条件：程序正常退出，输出5个顶点的优化后位姿。观察结果应满足：
   • 顶点0保持 (0,0,0) 不变（固定节点）
   • 顶点4与顶点0的位姿差接近闭环约束 (0.2, 0.1, 0.05)，即优化后顶点4的坐标应接近 (0.2, 0.1) 附近
   • 相邻顶点间的相对位姿接近里程计约束 (1,0,0)，轨迹整体平滑

核心流程说明

本案例演示了g2o位姿图优化的最小闭环：定义顶点（SE2位姿）→ 添加约束边（里程计+闭环）→ 配置优化器（LM算法）→ 执行优化 → 读取估计值。关键点在于：第一个顶点必须固定以消除 gauge freedom；边的信息矩阵表示约束的可信度，单位矩阵表示各方向等权重。闭环边是优化的核心驱动力——它迫使轨迹末端与起点对齐，从而将误差传播到整个图，调整所有中间节点的位姿。

若输出中顶点4的坐标明显偏离 (0.2, 0.1)，请检查闭环边的测量值是否正确设置，或增加迭代次数（如 optimizer.optimize(100)）。