LP-ICP: General Localizability-Aware Point Cloud Registration for Robust Localization in Extreme Unstructured Environments

2025-01-05

AI Summary

In unstructured environments, LiDAR SLAM suffers from poor localization robustness and low ICP registration accuracy due to degeneracy. To address this, we propose LP-ICP, a local observability-aware point cloud registration framework. Our method introduces three key innovations: (1) a novel local observability analysis module that identifies degenerate directions at fine granularity by leveraging geometric attributes of edge and planar points; (2) a weighted hybrid distance metric combining point-to-line and point-to-plane distances; and (3) a soft–hard constraint co-optimization mechanism that suppresses pose drift or incorporates prior knowledge along unobservable directions. Evaluated on both synthetic and real-world unstructured datasets, LP-ICP significantly reduces pose jitter and achieves state-of-the-art accuracy. The source code and benchmark datasets are publicly released.

Technology Category

• Intelligent Robots: Localization, Mapping, and Navigation
• Search and Optimization: Local Search
• Natural Language Processing: Safety and Robustness

Application Category

• Graph Algorithms and Modeling for the Web: Algorithms and analysis for incomplete, noisy, or partially observed Web-related graphs
• Security and Privacy: Large-scale security measurements
• Systems and Infrastructure for Web, Mobile and WoT: Location- and context-aware Web and WoT applications and services

Abstract

The Iterative Closest Point (ICP) algorithm is a crucial component of LiDAR-based SLAM algorithms. However, its performance can be negatively affected in unstructured environments that lack features and geometric structures, leading to low accuracy and poor robustness in localization and mapping. It is known that degeneracy caused by the lack of geometric constraints can lead to errors in 6-DOF pose estimation along ill-conditioned directions. Therefore, there is a need for a broader and more fine-grained degeneracy detection and handling method. This paper proposes a new point cloud registration framework, LP-ICP, that combines point-to-line and point-to-plane distance metrics in the ICP algorithm, with localizability detection and handling. LP-ICP consists of a localizability detection module and an optimization module. The localizability detection module performs localizability analysis by utilizing the correspondences between edge points (with low local smoothness) to lines and planar points (with high local smoothness) to planes between the scan and the map. The localizability contribution of individual correspondence constraints can be applied to a broader range. The optimization module adds additional soft and hard constraints to the optimization equations based on the localizability category. This allows the pose to be constrained along ill-conditioned directions, with updates either tending towards the constraint value or leaving the initial estimate unchanged. This improves accuracy and reduces fluctuations. The proposed method is extensively evaluated through experiments on both simulation and real-world datasets, demonstrating higher or comparable accuracy than the state-of-the-art methods. The dataset and code of this paper will also be open-sourced at https://github.com/xuqingyuan2000/LP-ICP.

Problem

• Lidar-based SLAM
• ICP Algorithm
• Accuracy in Featureless Environments

Innovation

• LP-ICP
• Point-to-Line and Point-to-Plane Measurement
• Localization and Mapping Accuracy