FA：Formulas and Algorithm，FF：Fusion and Filtting

图优化工程实例

我们选用g2o做最经典的**2D 位姿图优化（Pose Graph）**案例：机器人在平面移动，顶点是 SE (2) 位姿((x,y,\theta))，边分为里程计二元边 + 回环二元边。

场景说明：小车沿矩形行走，里程计存在漂移，最后回到起点形成回环；用图优化修正整条轨迹漂移。
SE (2)：平面位姿，(x,y)平移，(\theta)航向角；适合室内机器人、园区小车 2D 定位。

前置说明：

  1. g2o 结构：Vertex顶点 + Edge边 + SparseOptimizer优化器
  2. 目标：(\min\sum e_{ij}^T\Omega_{ij}e_{ij})
  3. 残差：(e = \boldsymbol{z}{-1}\cdot(\boldsymbol{T}_i{-1}\boldsymbol{T}_j))，(\boldsymbol{z})是观测到的相对位姿

一、工程流程分步拆解

Step1：构建图优化求解器（SparseOptimizer）

  1. 设置求解器类型：LM（Levenberg-Marquardt），鲁棒性优于高斯牛顿，工程首选
  2. 设置线性求解器：BlockSolver + 线性分解（CSparse/Cholesky，利用稀疏性）
  3. 设置优化迭代次数，开启日志（调试用）

Step2：定义顶点（VertexSE2）

  • 待优化变量：每个时刻机器人全局位姿([x,y,\theta])
  • 编号 id 唯一；固定第一个顶点（id=0），固定原点消除自由度（缺少全局约束，优化有无穷多解）
  • 顶点存储初始值：里程计递推得到的初值（有漂移）

Step3：定义边（EdgeSE2）

每条二元边连接两个顶点(i,j)，存储相对位姿观测(z = [dx,dy,d\theta])（里程计 / ICP 匹配输出），同时配置信息矩阵(\Omega)（协方差逆）。

  • 里程计边：连续帧之间（i 和 i+1），局部运动观测
  • 回环边：检测回到历史位置，比如最后一帧和第 0 帧，用来消除漂移，是图优化核心价值
  • 信息矩阵：对角阵，代表 x/y/ 航向角的置信度，噪声越小值越大

Step4：添加鲁棒核（可选，工程必加）

Huber 核：小残差用二范数，大残差线性衰减，抑制外点（匹配错误、里程计跳变），防止一条坏观测把整个轨迹带崩。

Step5：执行优化

  1. 初始化优化器
  2. 迭代优化（比如 50 轮），迭代过程逐步降低总残差
  3. 迭代完成后读取所有顶点优化后的位姿，对比优化前后轨迹

Step6：结果评估

  • 优化前：里程计漂移，回环闭合误差很大
  • 优化后：整条轨迹被回环约束拉回，全局一致性更好

二、完整 C++ 代码（g2o 2D 位姿图）

依赖：g2o 核心库，g2o/types/slam2d，g2o/solvers/csparse
CMakeLists 一并附上

// pose_graph_2d.cpp
#include <iostream>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/csparse_wrapper.h>
#include <g2o/types/slam2d/vertex_se2.h>
#include <g2o/types/slam2d/edge_se2.h>
#include <g2o/core/robust_kernel_huber.h>

using namespace g2o;
using namespace std;

int main()
{
    // ========= Step1：配置稀疏优化器 + LM求解器 =========
    // 每个顶点维度3(x,y,theta)，误差维度3
    using BlockSolverType = BlockSolver<BlockSolverTraits<3,3>>;
    using LinearSolverType = LinearSolverCSparse<BlockSolverType::PoseMatrixType>;

    auto linearSolver = make_unique<LinearSolverType>();
    auto blockSolver = make_unique<BlockSolverType>(move(linearSolver));
    OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(move(blockSolver));

    SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);  // 打印迭代残差信息

    // ========= Step2：添加SE2顶点，里程计初值（矩形轨迹，带漂移） =========
    // 矩形：0→1→2→3→0，4个位姿节点
    vector<VertexSE2*> vertices(4);
    // 顶点0：原点，固定
    vertices[0] = new VertexSE2();
    vertices[0]->setId(0);
    vertices[0]->setEstimate(SE2(0, 0, 0));
    vertices[0]->setFixed(true);
    optimizer.addVertex(vertices[0]);

    // 顶点1：向右走 2m，初值
    vertices[1] = new VertexSE2();
    vertices[1]->setId(1);
    vertices[1]->setEstimate(SE2(2.1, 0.05, 0.02)); // 加一点漂移噪声
    optimizer.addVertex(vertices[1]);

    // 顶点2：向上走2m
    vertices[2] = new VertexSE2();
    vertices[2]->setId(2);
    vertices[2]->setEstimate(SE2(2.06, 2.08, 1.59));
    optimizer.addVertex(vertices[2]);

    // 顶点3：向左走2m
    vertices[3] = new VertexSE2();
    vertices[3]->setId(3);
    vertices[3]->setEstimate(SE2(0.07, 2.03, 3.16));
    optimizer.addVertex(vertices[3]);

    // ========= Step3：添加里程计二元边（连续约束） =========
    // 信息矩阵：x,y,theta的权重，对角矩阵
    Eigen::Matrix3d info;
    info.setIdentity();
    info(0,0) = 100;    // x方向置信度
    info(1,1) = 100;    // y方向置信度
    info(2,2) = 400;    // 角度置信度更高

    // 边0-1，观测相对位姿：dx=2, dy=0, dtheta=0
    EdgeSE2* e01 = new EdgeSE2();
    e01->setVertex(0, vertices[0]);
    e01->setVertex(1, vertices[1]);
    e01->setMeasurement(SE2(2, 0, 0));
    e01->setInformation(info);
    optimizer.addEdge(e01);

    // 边1-2：dx=0, dy=2, dtheta=pi/2
    EdgeSE2* e12 = new EdgeSE2();
    e12->setVertex(0, vertices[1]);
    e12->setVertex(1, vertices[2]);
    e12->setMeasurement(SE2(0, 2, M_PI/2));
    e12->setInformation(info);
    optimizer.addEdge(e12);

    // 边2-3：dx=-2, dy=0, dtheta=pi/2
    EdgeSE2* e23 = new EdgeSE2();
    e23->setVertex(0, vertices[2]);
    e23->setVertex(1, vertices[3]);
    e23->setMeasurement(SE2(-2, 0, M_PI/2));
    e23->setInformation(info);
    optimizer.addEdge(e23);

    // ========= Step4：添加【回环边】3→0！图优化核心 =========
    EdgeSE2* e_loop = new EdgeSE2();
    e_loop->setVertex(0, vertices[3]);
    e_loop->setVertex(1, vertices[0]);
    e_loop->setMeasurement(SE2(0, 0, 0)); // 观测：3号应该回到0号，相对位姿为0
    e_loop->setInformation(info);
    // 加Huber鲁棒核，抑制回环误匹配
    RobustKernelHuber* huber = new RobustKernelHuber;
    huber->setDelta(1.0);
    e_loop->setRobustKernel(huber);
    optimizer.addEdge(e_loop);

    // ========= Step5：执行优化 =========
    cout << "===== 优化前位姿 =====" << endl;
    for(auto v : vertices){
        cout << "id:" << v->id() << " x:" << v->estimate().translation()[0]
             << " y:" << v->estimate().translation()[1]
             << " theta:" << v->estimate().rotation().angle() << endl;
    }

    optimizer.initializeOptimization();
    optimizer.optimize(50); // 最多迭代50次

    cout << "\n===== 优化后位姿 =====" << endl;
    for(auto v : vertices){
        cout << "id:" << v->id() << " x:" << v->estimate().translation()[0]
             << " y:" << v->estimate().translation()[1]
             << " theta:" << v->estimate().rotation().angle() << endl;
    }

    // 释放内存略，工程用智能指针更好
    return 0;
}

### CMakeLists.txt
cmake_minimum_required(VERSION 3.10)
project(g2o_pose_graph)
set(CMAKE_CXX_STANDARD 14)

find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(pose_graph_2d pose_graph_2d.cpp)
target_link_libraries(pose_graph_2d
    ${G2O_LIBRARIES}
)

三、代码逐段重点解读

  1. 求解器选择OptimizationAlgorithmLevenberg：LM 算法，迭代中自动调节阻尼，初值不好时不容易发散，机器人定位工程默认。LinearSolverCSparse：稀疏 Cholesky 分解，只处理非零元素，大规模轨迹才有优势。
  2. VertexSE2setEstimate()：设置优化初值（来自里程计 / IMU 前端，必须给初值，非线性优化依赖初值）setFixed(true)：固定第一个顶点，固定全局坐标系原点，否则优化问题秩亏，有无穷多解。
  3. EdgeSE2setMeasurement(SE2(dx, dy, dtheta))：传感器观测得到两个顶点之间的相对变换，不是全局坐标！这点是新手最容易踩坑。setInformation()：信息矩阵(\Omega=\Sigma^{-1})，噪声越小，信息矩阵权重越大。
  4. 回环边 + 鲁棒核
    回环边是图优化区别于 EKF 的关键：EKF 很难修正很久之前的历史漂移；回环边直接约束相隔很久的两个位姿，全局修正整条轨迹。RobustKernelHuber：如果回环匹配错了、观测是外点，不会让残差爆炸污染全部轨迹，自动驾驶 / 激光 SLAM 几乎必开。
  5. 输出对比
  • 优化前：里程计累积漂移，顶点 3 离原点有明显偏移
  • 优化后：在里程计约束 + 回环约束联合最小残差下，顶点 3 被拉回靠近原点，整条轨迹一致性提升

四、拓展到自动驾驶 / 3D 场景（衔接前面 Waymo/GTSAM）

上面是2D 位姿图 (g2o)，而自动驾驶多源融合一般用GTSAM 因子图（iSAM2 增量优化），区别：

  1. g2o 位姿图：顶点只有位姿；适合离线建图、2D 激光 SLAM
  2. GTSAM 因子图：顶点可以是 IMU bias、速度、位姿；因子可以是 IMU 预积分因子、GPS 因子、激光地图匹配因子；支持增量优化 iSAM2，新增帧只更新受影响部分，适合车载实时定位。

GTSAM 的工程差异点（简单示例结构）

// GTSAM伪代码逻辑
NonlinearFactorGraph graph;
Values initial;

// 加先验因子（固定初始位姿）
graph.add(PriorFactor<Pose3>(0, Pose3(), noise));
// IMU预积分因子，连接pose i和i+1
graph.add(ImuFactor(...));
// GPS一元因子，约束当前pose
graph.add(GPSFactor(...));
// 回环因子
graph.add(BetweenFactor<Pose3>(...));

// iSAM2增量优化器，实时滑动窗口
ISAM2 isam;
isam.update(graph, initial);
Values result = isam.calculateEstimate();

五、工程常见问题

  1. 把全局坐标放进边的观测：边的 measurement 一定是相对位姿，不是全局 xy，会直接优化爆炸
  2. 信息矩阵随便填：噪声标定错误，多源融合权重失衡，GPS 和 IMU 互相打架
  3. 忘记固定顶点：无全局先验，优化解不唯一，结果随机漂移
  4. 不使用鲁棒核：单次错误匹配直接毁掉整条轨迹
  5. 初值太差：非线性优化收敛到局部最小值，回环修正失效
# 人工智能 # 自动驾驶 # 机器人
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

Agent 工程实战 01：从聊天机器人到 Agent，先搭建一个能运行的项目

avatar DAMO开发者矩阵
cover

WLA³：面向语义、动力学与运动学的世界潜动作建模

avatar DAMO开发者矩阵
cover

具身智能创新设计方案（48）：TVA-VLA基础架构的产业重塑与演进路径

avatar DAMO开发者矩阵