TACO: A Test and Check Framework for Robust Pose Graph Optimization
2026-06-29 • Robotics
Robotics
AI summaryⓘ
The authors introduce TACO, a new method to improve Pose Graph Optimization (PGO), which helps robots figure out where they are while mapping. PGO often struggles when some data, called outliers, is wrong due to confusing similar places. TACO works by testing and checking measurements step-by-step to remove these errors without needing to label data as good or bad from the start. They tested TACO on different mapping tasks and found it works well, running quickly enough to be used in real-time systems. They also made their code available for others to use.
Pose Graph OptimizationSimultaneous Localization and MappingOutliersLoop ClosurePerceptual AliasingIncremental Probabilistic ConsensusSwitchable ConstraintsRobust Optimization2D SLAM3D Visual SLAM
Authors
Emilio Olivastri, Alberto Pretto, Tobias Fischer
Abstract
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.