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

2026-06-29

AI Summary

This work addresses the susceptibility of pose graph optimization (PGO) to outliers under high rates of erroneous loop closures, which often leads to catastrophic trajectory estimation failure. To this end, the authors propose TACO, a two-stage online outlier rejection framework comprising a testing phase and a verification phase. In the testing phase, Incremental Probabilistic Consensus (IPC) evaluates the consistency of new loop closures in real time, while the verification phase periodically refines the consensus set using switchable constraints. Notably, TACO efficiently approximates the maximum consensus set without explicit inlier/outlier modeling, achieving strong robustness and real-time performance simultaneously. Experimental results demonstrate that on both 2D and 3D SLAM datasets with outlier rates as high as 50%, TACO achieves success rates exceeding 90% and 83%, respectively, with average convergence times of approximately 45 ms and 100 ms—matching the accuracy of state-of-the-art offline methods while remaining suitable for online deployment.

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.

Problem

• Pose Graph Optimization
• Outliers
• SLAM
• Perceptual Aliasing
• Robustness

Innovation

• Pose Graph Optimization
• Outlier Rejection
• Incremental Probabilistic Consensus
• Switchable Constraints
• Robust SLAM