PALoc: Robust Prior-assisted Trajectory Generation for Benchmarking

Xiangcheng Hu, Jin Wu, Jianhao Jiao, Ruoyu Geng, Ming LiuarXiv 2023 · DOI · arXiv

Introduction

Evaluating the performance of Simultaneous Localization and Mapping (SLAM) algorithms requires high-precision "ground truth" (GT) trajectories. Traditionally, researchers rely on Motion Capture (MOCAP) systems for indoor environments or Global Navigation Satellite Systems (GNSS) for outdoor settings. However, these solutions are often limited by spatial constraints or signal availability. PALoc addresses this gap by providing a robust, prior-assisted SLAM system capable of generating centimeter-level 6-DOF trajectories in challenging environments, such as long, featureless corridors, where traditional sensors and existing map-based localization methods often fail.

Methodology

PALoc utilizes a Factor Graph framework to integrate various sensor inputs into a globally consistent path. The system is designed to be "loosely coupled," allowing it to refine the outputs of existing high-end odometry systems like FAST-LIO2 or LIO-SAM. The core innovations of PALoc include:

  • Degeneracy-Aware Map Factor (DM): LiDAR sensors often struggle in "degenerate" environments like long hallways where geometry is repetitive. PALoc employs a two-stage detection method based on the eigenvalues of the Hessian matrix to identify these weak spatial directions. By doing so, the system prevents mathematical "slipping" and maintains accuracy along the length of the structure.
  • Norm-Constrained Gravity Factor (NG): To combat orientation drift, PALoc utilizes stationary periods (Zero Velocity Updates) to calibrate the robot's orientation relative to gravity using the IMU's accelerometer. This ensures the "down" direction remains accurate over long durations.
  • Point-to-Plane Optimization: The system aligns LiDAR scans to the surfaces of a pre-existing high-resolution point cloud map, providing a more robust reference than simple point-to-point matching.

Experimental Setup and Datasets

The system was evaluated using the FusionPortable dataset, which features data from an Ouster-128 LiDAR and a STIM300 IMU. The sensors were mounted on various platforms, including handheld devices and quadruped robots, to test performance across diverse campus environments such as gardens, canteens, and escalators.

The sensor suite used for data collection, featuring a LiDAR, IMU, and FLIR cameras.

Prior maps for these environments were generated using a professional-grade Leica BLK360 laser scanner, providing a high-fidelity reference for the PALoc optimization process.

Key Results

PALoc demonstrated superior performance compared to state-of-the-art (SOTA) methods like LIO-SAM and HDL-Localization in both accuracy and robustness:

  • Centimeter-Level Accuracy: In Motion Capture Room (MCR) tests, PALoc achieved an Absolute Trajectory Error (ATE) as low as 2.00 cm, effectively refining the raw odometry inputs.
  • Robustness in Degenerate Scenes: In narrow, repetitive corridor sequences, PALoc maintained a map accuracy of 3.99 cm. While many SOTA algorithms failed completely in these structureless environments, PALoc maintained high completeness scores, often exceeding 90%.
Visualization of the generated trajectory (red) within a high-resolution prior point cloud map.

Limitations and Future Work

While PALoc provides high-precision trajectories suitable for benchmarking, the Map Factor (DM) module introduces significant computational overhead, currently requiring approximately 141.8ms per frame. This makes the current implementation more suitable for offline processing rather than real-time navigation. Future research will focus on optimizing efficiency for larger-scale scenes and incorporating uncertainty analysis to provide users with confidence metrics for the generated trajectories.

Summary updated

Abstract

Evaluating simultaneous localization and mapping (SLAM) algorithms necessitates high-precision and dense ground truth (GT) trajectories. But obtaining desirable GT trajectories is sometimes challenging without GT tracking sensors. As an alternative, in this paper, we propose a novel prior-assisted SLAM system to generate a full six-degree-of-freedom ($6$-DOF) trajectory at around $10$Hz for benchmarking under the framework of the factor graph. Our degeneracy-aware map factor utilizes a prior point cloud map and LiDAR frame for point-to-plane optimization, simultaneously detecting degeneration cases to reduce drift and enhancing the consistency of pose estimation. Our system is seamlessly integrated with cutting-edge odometry via a loosely coupled scheme to generate high-rate and precise trajectories. Moreover, we propose a norm-constrained gravity factor for stationary cases, optimizing pose and gravity to boost performance. Extensive evaluations demonstrate our algorithm's superiority over existing SLAM or map-based methods in diverse scenarios in terms of precision, smoothness, and robustness. Our approach substantially advances reliable and accurate SLAM evaluation methods, fostering progress in robotics research.
BibTeX
@misc{hu2023paloc,
  author = {Xiangcheng Hu and Jin Wu and Jianhao Jiao and Ruoyu Geng and Ming Liu},
  title = {{PALoc: Robust Prior-assisted Trajectory Generation for Benchmarking}},
  year = {2023},
  eprint = {2305.13147},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2305.13147},
}
Figure