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

TL;DR

This work addresses the need for reliable ground-truth trajectories to evaluate SLAM, especially under degeneracy and dynamic conditions. It proposes PALoc, a prior-map–assisted factor-graph framework that generates dense $6$-DoF trajectories, leveraging degeneracy-aware map factors and ZUPT-based constraints, and providing per-pose uncertainty estimates. The method integrates LO, LC, NM, GF, and DM factors, with uncertainty propagation across the graph and an open-source map-evaluation toolbox. Experimental results on indoor and outdoor data show substantial improvements over ICP in both map accuracy and trajectory fidelity, and the approach demonstrates robustness in degenerate environments, making it a practical benchmark augmentation tool for SLAM research.

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.

Figures (8)

• Figure 1: (a) Sensor configuration with corresponding coordinate frames for the SLAM benchmarking. (b) Quadruped robot equipped with sensor suite in Motion Capture Room (MCR). (c) Prior RGB point cloud map with the estimated trajectory (red line) and map (blue point cloud) by PALoc on garden_day.
• Figure 2: System Pipeline Overview. This figure illustrates the architecture of our system, starting with initialization on a prior map and LiDAR odometry. Degeneracy analysis and point-to-plane registration are employed to create a degeneracy-aware map factor. The system also processes odometry and IMU data for stationary detection, forming no motion factors, and integrating gravity factors. Following the optimization of the factor graph on a frame-wise basis, loop closure detection is carried out, contributing to the Loop factor. This sequential pipeline results in the generation of estimated poses and maps, which assists in the indirect evaluation of trajectory accuracy.
• Figure 3: Factor Graph of Our Proposed System. Gray circles represent different system states at specific times, and colored rectangles symbolize various factors. The purple rectangle signifies the gravity factor, connected to states outlined in purple, illustrating gravity constraints during stationary periods. States with green outlines are indicative of LiDAR degenerate scenarios, and those with red outlines mark instances of successful loop closure detection. An in-depth discussion on uncertainty propagation is detailed in Section \ref{['sec:uncertainty_analysis']}.
• Figure 4: Error Map of Diverse Campus Scenes. The degree of color transition from blue to red indicates an increasing error in the mapped area. (a) garden_day (259.5), (b) canteen_day (253.1), (c) XZ-view of escalator_day (600.1), (d) XY-view of escalator_day (600.1) with ceiling removal, (e) corridor_day (656.4), (f) building_day (717.8).
• Figure 5: Translation Degeneracy Analysis in Corridor. (a) and (d) represents real-world corridor scenes. The black point cloud represents the prior map, and the red sphere with coordinate axes represents the relative constraint strength in the XYZ dimensions but is unrelated to the overall size of the ellipsoid. The flatter the ellipsoid, the more severe the degeneration in a specific dimension. The blue and light blue trajectories and the red points on the trajectories represent the FL2 odometry trajectory, our algorithm trajectory, and the pose with DM constraints. Our algorithm eliminates Z-axis drift error while ensuring robustness in a U-turn intersection (c). The point clouds of different colors in (f) indicate the corresponding number of constraints in XYZ dimensions (Section \ref{['sec:enhanced_degeneration_detection']}).