PALoc: Advancing SLAM Benchmarking with Prior-Assisted 6-DoF Trajectory Generation and Uncertainty Estimation

Xiangcheng Hu, Linwei Zheng, Jin Wu, Ruoyu Geng, Yang Yu, Hexiang Wei, Xiaoyu Tang, Lujia Wang, Jianhao Jiao, Ming LiuarXiv 2024 · DOI · arXiv

Abstract

Generating high-fidelity ground truth (GT) trajectories is a critical bottleneck in SLAM benchmarking. PALoc addresses this by introducing a prior-map-assisted factor graph framework that integrates degeneracy-aware constraints and rigorous uncertainty estimation. By leveraging survey-grade prior maps and specialized factors, PALoc improves trajectory accuracy by up to 20-30% compared to standard ICP-based methods, maintaining robustness in geometrically degenerate environments like long corridors.

Introduction

The evaluation of Simultaneous Localization and Mapping (SLAM) algorithms relies heavily on accurate "Ground Truth" (GT) trajectories. However, existing methods for GT generation face significant limitations. Motion Capture (MOCAP) systems are restricted to small indoor spaces, while GNSS/INS solutions fail in signal-denied environments like urban canyons or indoors. Traditional scanning-based methods using Iterative Closest Point (ICP) often struggle with rapid motion or "degenerate" environments—such as featureless hallways—where geometric ambiguity leads to significant drift. Furthermore, most systems provide a trajectory without a corresponding measure of confidence. PALoc bridges these gaps by providing a robust, uncertainty-aware framework for GT generation.

Methodology: Factor Graph Optimization

The core of PALoc is a factor graph optimization framework that processes the entire history of robot motion to determine the most probable path. This approach is enhanced by several key innovations:

Degeneracy-Aware Map Factor (DM)

A primary contribution of PALoc is the Degeneracy-Aware Map Factor. During point-cloud matching against a prior map, the system performs Singular Value Decomposition (SVD) on the Hessian matrix. By analyzing the condition number, PALoc identifies "weak" geometric directions (e.g., the longitudinal axis of a corridor) and adjusts the optimization constraints to prevent drift along those axes.

PALoc generates high-fidelity trajectories by aligning sensor data with a high-precision prior map.

Zero Velocity Update (ZUPT) and Uncertainty

To eliminate drift during stationary periods, PALoc incorporates ZUPT factors (No Motion and Gravity). Crucially, the framework derives mathematical covariance for every factor, allowing for global uncertainty propagation. This enables the visualization of confidence as 3D ellipsoids, providing researchers with a theoretical basis for the reliability of the generated GT.

Experimental Evaluation

The authors validated PALoc using the FusionPortable dataset and custom "stress test" sequences designed to induce geometric and rotational degeneracy. The hardware setup included high-density LiDARs (Ouster-128, Pandar XT32) and tactical-grade IMUs.

Key Results

  • Accuracy: PALoc demonstrated a 30% improvement in map accuracy and a 20% reduction in Absolute Trajectory Error (ATE) over standard ICP baselines.
  • Robustness: In challenging corridor environments where standard LiDAR-Inertial Odometry (LIO) typically suffers from massive Z-axis drift, PALoc maintained a stable trajectory by leveraging the prior map and degeneracy analysis.
  • Efficiency: The DM module is approximately 30% faster than standard Point-to-Plane ICP implementations, facilitating faster processing of large-scale datasets.

Conclusion

PALoc establishes a new standard for SLAM ground truth generation by combining the global consistency of prior maps with the robustness of factor graph optimization. While future work will address computational scaling for extremely long trajectories and further quantitative validation of the uncertainty models, PALoc currently provides a powerful tool for the SLAM research community to benchmark algorithms with unprecedented precision.

Summary updated

Abstract

Accurately generating ground truth (GT) trajectories is essential for Simultaneous Localization and Mapping (SLAM) evaluation, particularly under varying environmental conditions. This study introduces a systematic approach employing a prior map-assisted framework for generating dense six-degree-of-freedom (6-DoF) GT poses for the first time, enhancing the fidelity of both indoor and outdoor SLAM datasets. Our method excels in handling degenerate and stationary conditions frequently encountered in SLAM datasets, thereby increasing robustness and precision. A significant aspect of our approach is the detailed derivation of covariances within the factor graph, enabling an in-depth analysis of pose uncertainty propagation. This analysis crucially contributes to demonstrating specific pose uncertainties and enhancing trajectory reliability from both theoretical and empirical perspectives. Additionally, we provide an open-source toolbox (https://github.com/JokerJohn/Cloud_Map_Evaluation) for map evaluation criteria, facilitating the indirect assessment of overall trajectory precision. Experimental results show at least a 30\% improvement in map accuracy and a 20\% increase in direct trajectory accuracy compared to the Iterative Closest Point (ICP) \cite{sharp2002icp} algorithm across diverse campus environments, with substantially enhanced robustness. Our open-source solution (https://github.com/JokerJohn/PALoc), extensively applied in the FusionPortable\cite{Jiao2022Mar} dataset, is geared towards SLAM benchmark dataset augmentation and represents a significant advancement in SLAM evaluations.
BibTeX
@misc{hu2024paloc,
  author = {Xiangcheng Hu and Linwei Zheng and Jin Wu and Ruoyu Geng and Yang Yu and Hexiang Wei and Xiaoyu Tang and Lujia Wang and Jianhao Jiao and Ming Liu},
  title = {{PALoc: Advancing SLAM Benchmarking with Prior-Assisted 6-DoF Trajectory Generation and Uncertainty Estimation}},
  year = {2024},
  eprint = {2401.17826},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2401.17826},
}
Figure