📚 课程目录第4章使用g2o实现3D位姿图优化

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

任务目标

在3D SLAM中，位姿图优化用于消除里程计或回环检测累积的漂移。本案例使用g2o实现一个包含6个节点（SE3位姿）和5个里程计边+1个回环边的3D位姿图优化，旋转部分采用四元数表示，验证优化后回环误差被正确消除。

环境依赖

需要安装g2o库（推荐版本20201223或更新），并确保CMake能够找到它。核心依赖为g2o::VertexSE3、g2o::EdgeSE3以及内置的g2o::RobustKernelHuber。

关键完整代码

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/robust_kernel_impl.h>

using namespace g2o;
using namespace Eigen;

// 构造一个带噪声的3D位姿图并优化
int main() {
    // 1. 构建优化器
    typedef BlockSolver<BlockSolverTraits<6, 6>> BlockSolverType;
    typedef LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto* solver = new OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    // 2. 添加6个位姿顶点 (SE3, 四元数表示旋转)
    std::vector<VertexSE3*> vertices;
    for (int i = 0; i < 6; ++i) {
        auto* v = new VertexSE3();
        v->setId(i);
        // 初始估计：沿X轴前进，每步1.0m，带轻微扰动
        Isometry3d pose = Isometry3d::Identity();
        pose.translation() = Vector3d(i * 1.0 + 0.05 * i, 0.0, 0.0);
        if (i > 0) {
            // 给旋转加一点噪声（绕Z轴小角度）
            AngleAxisd rot(0.02 * i, Vector3d::UnitZ());
            pose.linear() = rot.toRotationMatrix();
        }
        v->setEstimate(pose);
        optimizer.addVertex(v);
        vertices.push_back(v);
    }

    // 3. 添加里程计边（相邻节点之间）
    for (int i = 0; i < 5; ++i) {
        auto* edge = new EdgeSE3();
        edge->setVertex(0, vertices[i]);
        edge->setVertex(1, vertices[i + 1]);
        // 真实运动：沿X轴平移1.0m，无旋转
        Isometry3d T = Isometry3d::Identity();
        T.translation() = Vector3d(1.0, 0.0, 0.0);
        edge->setMeasurement(T);
        // 信息矩阵：平移噪声小，旋转噪声稍大
        Matrix6d info = Matrix6d::Identity();
        info.block<3, 3>(0, 0) *= 100.0;   // 平移
        info.block<3, 3>(3, 3) *= 10.0;    // 旋转
        edge->setInformation(info);
        optimizer.addEdge(edge);
    }

    // 4. 添加回环边：节点5 回到 节点0
    auto* loop_edge = new EdgeSE3();
    loop_edge->setVertex(0, vertices[5]);
    loop_edge->setVertex(1, vertices[0]);
    // 真实回环：从节点5回到原点，即平移(-5, 0, 0)
    Isometry3d T_loop = Isometry3d::Identity();
    T_loop.translation() = Vector3d(-5.0, 0.0, 0.0);
    loop_edge->setMeasurement(T_loop);
    Matrix6d info_loop = Matrix6d::Identity();
    info_loop.block<3, 3>(0, 0) *= 50.0;
    info_loop.block<3, 3>(3, 3) *= 5.0;
    loop_edge->setInformation(info_loop);
    // 使用Huber核降低错误回环影响
    auto* kernel = new RobustKernelHuber();
    kernel->setDelta(1.0);
    loop_edge->setRobustKernel(kernel);
    optimizer.addEdge(loop_edge);

    // 5. 固定第一个顶点以消除自由度
    vertices[0]->setFixed(true);

    // 6. 执行优化
    optimizer.initializeOptimization();
    optimizer.optimize(50);

    // 7. 输出优化后的位姿
    std::cout << "优化后位姿：" << std::endl;
    for (size_t i = 0; i < vertices.size(); ++i) {
        Isometry3d pose = vertices[i]->estimate();
        Vector3d t = pose.translation();
        Quaterniond q(pose.linear());
        std::cout << "节点 " << i << ": t = (" << t.transpose() << "), q = ("
                  << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << ")" << std::endl;
    }

    // 8. 验证回环误差
    Isometry3d T_5_0 = vertices[0]->estimate().inverse() * vertices[5]->estimate();
    Vector3d residual_t = T_5_0.translation() - Vector3d(5.0, 0.0, 0.0);
    std::cout << "\n回环平移误差 (应为接近0): " << residual_t.norm() << std::endl;

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(pose_graph_3d)
set(CMAKE_CXX_STANDARD 14)
find_package(g2o REQUIRED)
find_package(Eigen3 REQUIRED)
add_executable(pose_graph_3d main.cpp)
target_link_libraries(pose_graph_3d g2o_core g2o_types_slam3d g2o_solver_eigen)
target_include_directories(pose_graph_3d PRIVATE ${EIGEN3_INCLUDE_DIR})

运行与验证

编译运行：

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

通过条件：

程序正常结束，无崩溃或内存错误；
优化后节点5的平移接近(5.0, 0, 0)，节点0保持固定在原点附近；
控制台输出的回环平移误差小于0.05，表明回环约束被正确满足；
所有四元数满足单位范数（输出中q.w²+q.x²+q.y²+q.z²≈1）。

关键说明

g2o的VertexSE3内部使用四元数+平移向量存储SE3位姿，无需手动转换；
信息矩阵的数值代表对该约束的信任程度，平移信息矩阵大于旋转表示更信任平移测量；
固定节点0是消除SE3流形上6个自由度（3平移+3旋转）的必要操作，否则优化问题秩亏；
回环边添加Huber核可防止错误回环对结果产生过大影响，本案例中回环正确，核函数影响很小。