TACO: A Test and Check Framework for Robust Pose Graph Optimization

📄 arXiv: 2606.29851v1 📥 PDF

作者: Emilio Olivastri, Alberto Pretto, Tobias Fischer

分类: cs.RO

发布日期: 2026-06-29

💡 一句话要点

提出TACO框架以增强姿态图优化的鲁棒性

🎯 匹配领域: 支柱三：空间感知与语义 (Perception & Semantics)

关键词: 姿态图优化 异常值处理 增量算法 SLAM 机器人导航 计算机视觉 鲁棒性

📋 核心要点

  1. 现有的姿态图优化方法对异常值非常敏感，这会导致轨迹估计质量下降，影响SLAM系统的可靠性。
  2. TACO框架通过增量概率共识算法和可切换异常值清理机制，动态评估和过滤不一致的测量，提升了PGO的鲁棒性。
  3. 在2D和3D SLAM数据集上的实验结果显示，TACO在高达50%的异常值率下仍能保持较高的成功率和快速的收敛时间。

📝 摘要（中文）

姿态图优化（PGO）是解决同时定位与地图构建（SLAM）问题的重要方法，但其对异常值的敏感性会显著降低轨迹估计的质量。本文提出了TACO（测试与检查优化），一个旨在过滤PGO系统中异常值的鲁棒优化框架。TACO通过两个互补组件实现：测试组件采用增量概率共识（IPC）算法在线评估每个闭环的测量一致性；检查组件则利用可切换约束定期清理IPC可能错误包含的不一致测量。实验结果表明，TACO在2D和3D SLAM数据集上表现出与最先进的离线方法相当的鲁棒性，同时保持在线部署所需的计算效率，2D成功率超过90%，3D成功率为83%。

🔬 方法详解

问题定义：本文解决的是姿态图优化中异常值对轨迹估计质量的影响，现有方法在处理环境中的感知混淆时容易引入错误的测量关联，导致结果不可靠。

核心思路：TACO框架的核心思路是通过增量评估和定期清理不一致测量，逐步找到最大一致测量集，从而提高PGO的鲁棒性。这样的设计使得系统能够在动态环境中实时适应和优化。

技术框架：TACO的整体架构包括两个主要组件：测试组件（IPC算法）用于在线评估闭环一致性，检查组件（可切换异常值清理）用于定期清理不一致测量。两者相辅相成，确保了测量集的准确性。

关键创新：TACO的关键创新在于其增量处理机制和可切换约束的结合，这与传统方法直接将测量标记为内点或外点的方式有本质区别，提供了更灵活的异常值处理策略。

关键设计：在设计中，IPC算法通过概率一致性评估来判断测量的有效性，而可切换约束则允许系统根据当前状态动态调整清理策略，确保测量集的持续优化。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

在2D SLAM数据集上，TACO的成功率超过90%，在3D SLAM数据集上成功率达到83%。在异常值率高达50%的情况下，TACO仍能保持约45毫秒和100毫秒的平均收敛时间，展现出优异的性能和效率。

🎯 应用场景

TACO框架在机器人导航、自动驾驶和增强现实等领域具有广泛的应用潜力。通过提高SLAM系统在复杂环境中的鲁棒性，TACO能够显著提升这些技术的可靠性和实用性，推动智能系统的进一步发展。

📄 摘要（原文）

Pose Graph Optimization (PGO) is one of the most widely adopted approaches for solving Simultaneous Localization and Mapping (SLAM) problems. However, PGO approaches are particularly sensitive to outliers, which can substantially degrade the quality of the estimated trajectories. These outliers arise from incorrect place recognition associations caused by perceptual aliasing in the environment. In this paper, we present TACO (short for Test And Check Optimization), a robust optimization framework designed to filter out outliers from PGO systems. Rather than explicitly modeling measurements as inliers or outliers, TACO finds an approximation to the maximally consistent set of measurements incrementally through two complementary components: (i) The test component, namely the Incremental Probabilistic Consensus (IPC) algorithm, evaluates the consistency of each incoming loop closure online. (ii) The check component dubbed Switchable Outlier Sanitization leverages the existing Switchable Constraints to periodically sanitize any inconsistent measurements from the consistent set that IPC may have mistakenly included. We evaluate TACO on 2D SLAM and 3D Visual SLAM datasets against several state-of-the-art methods. The results show robustness comparable to state-of-the-art offline methods while preserving the computational efficiency required for online deployment, achieving a success rate above 90% in 2D and 83% in 3D across outlier rates up to 50%, with mean convergence times of approximately 45 ms and 100 ms, respectively. We release an open-source implementation of our method with this paper.