Pose Graph Optimization

• 又称: PGO, pose-graph SLAM, pose graph optimisation
• 类别: State estimation & navigation › Localisation
• 标签: slam, state-estimation, optimization, localization, mapping, factor-graphs

一种 SLAM 后端技术，通过调整由位姿构成的图，使它们之间的里程计约束和回环约束尽可能一致，从而精化机器人的轨迹。

定义

位姿图优化（Pose graph optimization）是通过在一张图上求解最小二乘问题来估计机器人轨迹的过程。图中的每个节点是一个机器人位姿（某一时刻的位置和朝向），每条边是两个位姿之间测得的相对变换及其不确定性，后者通常用信息矩阵表示。边来自相邻位姿之间的里程计，以及回环，即机器人识别出自己曾经到过的地方。

由于每个测量都有噪声，各条边永远不会完全一致：沿着一个环把里程计边串起来，并不能让机器人恰好回到回环边所指出的位置。位姿图优化找出使所有边上的加权平方误差总和最小的位姿集合，按照每个测量的确定程度，把修正量按比例分摊到整条轨迹上。

由于旋转不能以加法方式合成，这个问题是非线性的，因此要用 Gauss-Newton 或 Levenberg-Marquardt 等方法迭代求解，每一步都在当前估计附近进行线性化。由此得到的线性方程组是稀疏的，因为每个位姿只与少数几个其他位姿相连，而稀疏求解器使包含成千上万个位姿的图也能处理。g2o、GTSAM 和 Ceres Solver 等开源库常用于这一步。

位姿图优化是大多数基于图的 SLAM 系统的后端。它与光束法平差的不同之处在于不把路标作为变量保留：只优化位姿，这样更快，但会丢弃部分信息。它已知的弱点是对错误回环敏感，这就是为什么鲁棒代价函数和离群值剔除是标准的附加手段。

为何重要

里程计和视觉里程计会随时间无限制地漂移，而位姿图优化是在找到回环后消除这种累积漂移的标准方法。它把一组局部准确但全局不一致的测量，转变为在整栋建筑或整个场地范围内都一致的轨迹和地图。

主要应用

• 基于激光雷达和相机建图的基于图的 SLAM 后端
• 消除轮式、视觉和视觉惯性里程计的累积漂移
• 多会话建图，合并在不同时间记录的地图
• 多机器人建图，把多台机器人的轨迹合并到同一坐标系中

实际应用

在典型的激光雷达或视觉 SLAM 栈中，前端在机器人每移动一段设定的距离或角度时添加一个新节点，并用一条里程计边把它与上一个节点相连。位置识别模块提出候选回环，这些回环在作为边加入之前会先经过几何验证（例如用 ICP 或特征匹配）。随后后端运行 g2o 或 GTSAM 等优化器，可以在每次回环后批量运行，也可以增量运行（如 iSAM2），修正后的位姿被用来重建地图。大量调参工作花在为每条边分配的协方差，以及 Huber 或 Cauchy 等鲁棒核函数上，因为一个置信度很高的错误回环就可能让整张地图弯曲变形。

常见误解

• 位姿图优化负责构建地图 — 它只修正位姿；地图是事后把传感器数据放到修正后的位姿上重新构建的
• 回环越多越好 — 一个错误的回环就可能扭曲整条轨迹，因此回环必须经过验证，并用鲁棒代价降低其权重
• 它与光束法平差相同 — 光束法平差还会优化路标位置，而位姿图只保留位姿和相对约束

谁需要了解

• SLAM 和定位工程师
• 移动机器人和自动驾驶车辆开发者
• 从事视觉里程计和建图的计算机视觉工程师
• 学习概率机器人学和状态估计的学生

相关技术

• g2o
• GTSAM
• Ceres Solver
• 因子图（Factor Graphs）
• 光束法平差（Bundle Adjustment）
• Levenberg-Marquardt 算法（Levenberg-Marquardt Algorithm）

相关术语

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

相关项目

• LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
• oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
• Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO

视频

• Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB