X-ICP: Localizability-Aware LiDAR Registration for Robust Localization in Extreme Environments

TL;DR

This work proposes a robust fine-grained localizability detection module and a localizability-aware constrained ICP optimization module, which couples with the localizability detection module in a unified manner to overcome LiDAR-challenging environments.

Abstract

Modern robotic systems are required to operate in challenging environments, which demand reliable localization under challenging conditions. LiDAR-based localization methods, such as the Iterative Closest Point (ICP) algorithm, can suffer in geometrically uninformative environments that are known to deteriorate point cloud registration performance and push optimization toward divergence along weakly constrained directions. To overcome this issue, this work proposes i) a robust fine-grained localizability detection module, and ii) a localizability-aware constrained ICP optimization module, which couples with the localizability detection module in a unified manner. The proposed localizability detection is achieved by utilizing the correspondences between the scan and the map to analyze the alignment strength against the principal directions of the optimization as part of its fine-grained LiDAR localizability analysis. In the second part, this localizability analysis is then integrated into the scan-to-map point cloud registration to generate drift-free pose updates by enforcing controlled updates or leaving the degenerate directions of the optimization unchanged. The proposed method is thoroughly evaluated and compared to state-of-the-art methods in simulated and real-world experiments, demonstrating the performance and reliability improvement in LiDAR-challenging environments. In all experiments, the proposed framework demonstrates accurate and generalizable localizability detection and robust pose estimation without environment-specific parameter tuning.

Figures (20)

• Figure 1: Top Row: Ground truth map and path of the robot during the Seemühle experiment. Certain sections of the environment are illustrated through real images. Bottom Row: Point cloud maps created using the proposed approach and compared against two state-of-the-art methods. The color bar indicates the point-to-point distance error with respect to the ground truth map.
• Figure 2: Overview of the proposed localizability-aware point cloud registration framework. The pose prior is used to transform and undistort the input point cloud, which is, together with the existing point cloud map, fed to the iterative ICP optimization loop. The optimized drift-free pose within the ICP loop is calculated using the proposed localizability detection (Section \ref{['section:detection']}) and aware optimization (Section \ref{['section:aware_opt']}) modules.
• Figure 3: Overview of the localizability detection module. A - Information Analysis: An exemplary histogram shown for one eigenvector $\boldsymbol{v}_j$ direction. The histogram is color-coded to illustrate the strong- () and the weak-contribution () regions. The red () region contains the to-be-filtered information pairs. B - Filtering: This step measures the alignment of all vectors in the histogram to the eigenvector $\boldsymbol{v}_j$, with $(\boldsymbol{s}_1,\boldsymbol{w}_1)$ illustrating strong and weak alignment vectors, respectively. C - Categorization: Localizability categories are assigned to each optimization eigenvector based on a decision tree.
• Figure 4: A 2D-example illustrating the contribution to localizability. Points $\boldsymbol{p}$ (green arrows), surface normals $\boldsymbol{n}$ (blue arrows), and the LiDAR center (red dot ) are shown. The span of one of the eigenvectors is depicted in orange. Finally, the induced torques ($\boldsymbol{\tau}$) are shown for three point- and normal-pairs.
• Figure 5: Overview of the constrained optimization module. The Opt.-Module steps are: i) linear Constraint Calculation, which is in the form of a 3D plane, and ii)Constrained Optimization which employs these constraints.