3. 使用g2o实现2D位姿图优化

任务目标

在2D SLAM中，前端里程计会持续累积漂移误差。本案例使用g2o构建位姿图（Pose Graph），将机器人一系列2D位姿（x, y, theta）作为节点，相邻位姿间的相对运动作为里程计边，回环检测得到的相对位姿作为回环边，通过图优化同时调整所有节点位姿，以最小化所有边的误差，从而消除累积漂移。

环境依赖

g2o（需编译安装，核心头文件位于g2o/core与g2o/types/slam2d）
Eigen3（g2o的依赖）
C++11及以上编译器，CMake

实现思路

构建一个包含5个节点、4条里程计边和1条回环边的极小位姿图。节点0固定在原点。里程计边连接相邻节点（0-1, 1-2, 2-3, 3-4），回环边连接节点4与节点1（模拟检测到机器人回到曾经到过的位置）。使用g2o的OptimizationAlgorithmLevenberg进行求解，迭代10次后输出优化前后的节点位姿对比。

关键完整代码

#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/core/sparse_optimizer.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;

// 便捷类型定义
typedef BlockSolver< BlockSolverTraits<3, 3> > SlamBlockSolver;
typedef LinearSolverDense<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

// 构造一个带噪声的2D位姿（用于模拟前端里程计累积误差）
Eigen::Vector3d addNoise(const Eigen::Vector3d& pose, double noise_xy, double noise_theta) {
    Eigen::Vector3d noisy = pose;
    noisy[0] += (std::rand() % 1000) / 1000.0 * noise_xy - noise_xy / 2.0;
    noisy[1] += (std::rand() % 1000) / 1000.0 * noise_xy - noise_xy / 2.0;
    noisy[2] += (std::rand() % 1000) / 1000.0 * noise_theta - noise_theta / 2.0;
    return noisy;
}

int main() {
    std::srand(42);  // 固定随机种子保证可复现

    // 1. 构建优化器
    auto* linearSolver = new SlamLinearSolver();
    linearSolver->setBlockOrdering(false);
    auto* blockSolver = new SlamBlockSolver(linearSolver);
    auto* algorithm = new OptimizationAlgorithmLevenberg(blockSolver);

    SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(false);

    // 2. 真实轨迹（用于生成带噪声的观测）
    std::vector<Eigen::Vector3d> true_poses = {
        Eigen::Vector3d(0, 0, 0),
        Eigen::Vector3d(1, 0, 0),
        Eigen::Vector3d(2, 0, 0),
        Eigen::Vector3d(3, 0, 0),
        Eigen::Vector3d(4, 0, 0)
    };

    // 3. 添加位姿节点（带累积噪声，模拟前端里程计输出）
    std::vector<VertexSE2*> vertices;
    Eigen::Vector3d current_pose(0, 0, 0);
    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 {
            // 模拟里程计累积漂移：每一步叠加小噪声
            Eigen::Vector3d step = true_poses[i] - true_poses[i-1];
            Eigen::Vector3d noisy_step = addNoise(step, 0.05, 0.02);
            current_pose += noisy_step;
            v->setEstimate(SE2(current_pose[0], current_pose[1], current_pose[2]));
        }
        optimizer.addVertex(v);
        vertices.push_back(v);
    }

    // 4. 添加里程计边（相邻节点间的相对变换，信息矩阵取较大值表示高置信）
    for (int i = 0; i < 4; ++i) {
        auto* edge = new EdgeSE2();
        edge->setVertex(0, vertices[i]);
        edge->setVertex(1, vertices[i+1]);
        // 真实相对变换（无噪声，因为噪声已注入节点估计中）
        Eigen::Vector3d rel = true_poses[i+1] - true_poses[i];
        edge->setMeasurement(SE2(rel[0], rel[1], rel[2]));
        edge->setInformation(Eigen::Matrix3d::Identity() * 100.0);
        optimizer.addEdge(edge);
    }

    // 5. 添加回环边：节点4 与 节点1 之间的约束（模拟回环检测）
    auto* loop_edge = new EdgeSE2();
    loop_edge->setVertex(0, vertices[4]);
    loop_edge->setVertex(1, vertices[1]);
    // 真实相对位姿：节点1在(1,0)，节点4在(4,0)，因此从4到1的变换为(-3, 0, 0)
    loop_edge->setMeasurement(SE2(-3.0, 0.0, 0.0));
    // 回环约束信息矩阵（比里程计略低，模拟检测不确定性）
    Eigen::Matrix3d loop_info = Eigen::Matrix3d::Identity() * 50.0;
    loop_edge->setInformation(loop_info);
    optimizer.addEdge(loop_edge);

    // 6. 输出优化前的位姿估计
    std::cout << "=== 优化前位姿（含累积漂移） ===" << std::endl;
    for (int i = 0; i < 5; ++i) {
        SE2 est = vertices[i]->estimate();
        std::cout << "节点 " << i << ": x=" << est.translation().x()
                  << ", y=" << est.translation().y()
                  << ", theta=" << est.rotation().angle() << std::endl;
    }

    // 7. 执行优化
    optimizer.initializeOptimization();
    optimizer.optimize(10);

    // 8. 输出优化后的位姿
    std::cout << "\n=== 优化后位姿（回环约束已传播） ===" << std::endl;
    for (int i = 0; i < 5; ++i) {
        SE2 est = vertices[i]->estimate();
        std::cout << "节点 " << i << ": x=" << est.translation().x()
                  << ", y=" << est.translation().y()
                  << ", theta=" << est.rotation().angle() << std::endl;
    }

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(pose_graph_2d)

set(CMAKE_CXX_STANDARD 11)

find_package(g2o REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(${EIGEN3_INCLUDE_DIR})

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

运行与验证

mkdir build && cd build
cmake ..
make -j4

运行：

./pose_graph_2d

通过条件：观察终端输出的优化前后位姿对比。由于固定随机种子为42，每次运行结果一致。优化前节点4的x坐标因累积噪声会明显偏离真实值4.0（通常在3.8~4.2之间）；优化后，由于回环边将节点4拉回与节点1形成一致约束，节点4的x坐标应显著接近4.0，且所有节点的y坐标和theta应接近0。若优化后节点4的x坐标比优化前更接近4.0，且整体轨迹趋于平滑直线，则说明位姿图优化正确消除了累积漂移。

关键说明

节点0被固定（setFixed(true)），否则整个图存在无约束的平移自由度，优化无法收敛到唯一解。
里程计边的测量值使用真实相对变换（而非带噪声的），因为噪声已通过addNoise注入到节点初始估计中，这模拟了前端里程计输出的位姿已含漂移、而相邻帧间约束相对准确的实际场景。
回环边连接节点4与节点1，测量值为(-3, 0, 0)，表示从节点4坐标系观察节点1的相对位置。该约束将修正节点4处累积的漂移。
信息矩阵数值表示对该约束的置信程度：里程计边取100，回环边取50，表示里程计短时更可靠、回环检测存在一定不确定性。

📚 课程目录