LOCUS 2.0: Robust and Computationally Efficient Lidar Odometry for Real-Time 3D Mapping

Andrzej Reinke, Matteo Palieri, Benjamin Morrell, Yun Chang, Kamak Ebadi, Luca Carlone, Ali-Akbar Agha-Mohammadi

TL;DR

This paper introduces LOCUS 2.0, a lidar odometry system designed for robust and efficient real-time 3D mapping in challenging, GNSS-denied environments. It tackles the critical issue of limited onboard computation and memory on robotic platforms by introducing novel GICP formulation, an adaptive voxel grid filter, and a sliding-window map approach.

What Problem Does This Paper Address?

Autonomous robots operating in environments like underground mines, caves, or disaster sites often lack reliable GPS signals, making localization a significant challenge. Lidar sensors are a promising solution, offering accurate 3D perception. However, existing lidar odometry systems can be computationally intensive and memory-hungry, hindering their deployment on resource-constrained robotic platforms, especially for large-scale mapping tasks. This paper aims to bridge this gap by developing a lidar odometry system that is both accurate and efficient enough to run in real-time on such platforms.

Methods

LOCUS 2.0 builds upon its predecessor, LOCUS 1.0, with several key improvements:

  • GICP from Normals: A novel formulation of Generalized Iterative Closest Point (GICP) that leverages pre-computed point cloud normals. Instead of recalculating point covariances (a computationally expensive step), it uses normals to approximate them. This significantly speeds up the scan-to-scan and scan-to-submap registration process.
  • Adaptive Voxel Grid Filter: This filter dynamically adjusts the voxel leaf size to maintain a fixed number of points for processing, regardless of the environment's geometry or the number of lidar sensors. This ensures a consistent and predictable computation load, crucial for real-time performance.
  • Sliding-Window Map: To address memory limitations in large-scale environments, LOCUS 2.0 employs a sliding-window approach for map management. This keeps only a local, robot-centered portion of the map in memory, preventing unbounded memory growth. Two implementations are explored: a multi-threaded octree and an incremental k-d tree (ikd-tree).
  • Health-Aware Sensor Integration: The system includes a health monitor that dynamically manages lidar inputs, ignoring delayed or failed sensors to maintain robustness.

Datasets

The authors introduce a new, challenging lidar odometry dataset collected from real-world underground environments (limestone mines, lava tubes, urban subway stations) during the DARPA Subterranean Challenge. This dataset features:

  • Heterogeneous Platforms: Data collected from both legged (Spot) and wheeled (Husky) robots.
  • Challenging Conditions: Includes environments with fog, dust, darkness, and geometrically degenerate features.
  • Scale: A total of 11 hours of operation and 16 km of distance traveled.
  • Ground Truth: Trajectories are derived from LOCUS 1.0 operating in a high-accuracy, scan-to-map mode against survey-grade maps.

This dataset is released as open-source to promote further research.

Key Insights and Ideas

  • Normals as a Shortcut: The insight that point cloud normals can effectively approximate covariances for GICP is a significant computational win, especially for scan-to-submap matching where recalculating covariances for a large map would be prohibitive.
  • Predictable Performance: The adaptive voxel grid filter is key to achieving consistent real-time performance. By decoupling computation from environmental complexity, it makes the system more reliable on resource-constrained robots.
  • Memory Management is Crucial: The sliding-window map approach is a practical necessity for large-scale exploration. The paper demonstrates that even with increased computational cost for map updates, the memory savings are substantial and essential.
  • Real-World Challenges: The new dataset highlights the importance of testing odometry systems in diverse and degraded conditions, which are common in real-world robotic applications.

Key Results

  • Computational Efficiency: GICP from normals reduces computational metrics (CPU usage, odometry delay, scan-to-scan/submap times) by an average of 18.57% compared to standard GICP.
  • Accuracy Trade-off: The GICP from normals approach shows a slight increase in mean and max Absolute Pose Error (APE) (around 10.82%), but this is considered a reasonable trade-off for the significant computational gains.
  • Memory Reduction: The sliding-window map approaches (ikd-tree and multi-threaded octree) significantly reduce memory usage compared to a full map storage. For example, ikd-tree shows a -68.09% memory change relative to the baseline LOCUS 1.0.
  • Robustness: LOCUS 2.0 demonstrates state-of-the-art performance across various environments, including challenging tunnel scenarios where other methods failed. It achieves the smallest APE errors in 5 out of 6 tested datasets when compared to FAST-LIO and LINS.

Limitations and Future Work

  • APE Increase: While computational gains are substantial, there's a slight increase in APE. Future work could explore methods to mitigate this while retaining efficiency.
  • Ikd-tree Performance: The ikd-tree data structure, while effective for memory management, incurs higher computation costs for search, deletion, and insertion compared to octree-based structures. Further optimization of ikd-tree operations could be beneficial.
  • Sensor Fusion: While LOCUS 2.0 is designed to be a lidar-centric system, its architecture allows for the integration of other sensors. Future work could explore more tightly-coupled fusion strategies for enhanced robustness and accuracy.
  • Dataset Expansion: The paper releases a valuable dataset, but continued collection and expansion of such challenging datasets will be crucial for advancing the field.

Summary updated

Abstract

Lidar odometry has attracted considerable attention as a robust localization method for autonomous robots operating in complex GNSS-denied environments. However, achieving reliable and efficient performance on heterogeneous platforms in large-scale environments remains an open challenge due to the limitations of onboard computation and memory resources needed for autonomous operation. In this work, we present LOCUS 2.0, a robust and computationally-efficient lidar odometry system for real-time underground 3D mapping. LOCUS 2.0 includes a novel normals-based Generalized Iterative Closest Point (GICP) formulation that reduces the computation time of point cloud alignment, an adaptive voxel grid filter that maintains the desired computation load regardless of the environment’s geometry, and a sliding-window map approach that bounds the memory consumption. The proposed approach is shown to be suitable to be deployed on heterogeneous robotic platforms involved in large-scale explorations under severe computation and memory constraints. We demonstrate LOCUS 2.0, a key element of the CoSTAR team’s entry in the DARPA Subterranean Challenge, across various underground scenarios. We release LOCUS 2.0 as an open-source library and also release a lidar-based odometry dataset in challenging and large-scale underground environments. The dataset features legged and wheeled platforms in multiple environments including fog, dust, darkness, and geometrically degenerate surroundings with a total of $\text{11}\;\text{h}$ of operations and $\text{16}\;\text{km}$ of distance traveled.
BibTeX
@article{reinke2022locus,
  author = {Andrzej Reinke and Matteo Palieri and Benjamin Morrell and Yun Chang and Kamak Ebadi and Luca Carlone and Ali-Akbar Agha-Mohammadi},
  title = {{LOCUS 2.0: Robust and Computationally Efficient Lidar Odometry for Real-Time 3D Mapping}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2022},
  url = {https://openreview.net/forum?id=kUx2gVzFnW},
}
Figure