PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Turcan Tuna, Julian Nubert, Yoshua Nava, Shehryar Khattak, Marco Hutter

arXiv:2211.16335v4cs.RO

TL;DR

LiDAR ICP can become under-constrained in geometrically degenerate environments, compromising pose estimation along weakly observable directions. X-ICP detects localizability from scan-map correspondences and uses it to constrain ICP updates, achieving robust estimation and reliable detection across simulated and real-world challenging environments without environment-specific parameter tuning.

  • Problem

    LiDAR registration can become under-constrained in geometrically degenerate environments, where one or more transformation directions are nearly unobservable from point correspondences.

  • Method

    X-ICP combines fine-grained localizability detection from scan-map correspondences with constrained ICP optimization that controls or prevents updates along degenerate directions.

  • Results

    Across simulated environments and real-world experiments, X-ICP reliably detects degeneracy and improves robustness and accuracy in challenging environments without heuristic parameter tuning.

  • Takeaways & Limitations

    X-ICP provides robust pose estimation by using correspondence information and additional constraints to prevent drift along degenerate directions while exploiting partial localizability.

Abstract

from arXiv · show

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.

I. INTRODUCTION

X-ICP addresses LiDAR registration failures in geometrically degenerate environments with fine-grained localizability detection and constrained ICP optimization. Experiments indicate robust detection and improved pose estimation without environment-specific parameter tuning.

  • a) Point Cloud Registration: Scan-to-map registration reduces drift relative to scan-to-scan registration by estimating incoming point-cloud pose against an existing map.ICP is a widely used iterative error-minimization approach for this registration task.
  • b) Current Limitations: ICP performance is limited by local minima, sensor noise and bias, inaccurate transformation priors, and insufficient geometric constraints.The lack of environmental constraints is especially relevant in degenerate settings.
  • c) Proposed Approach: X-ICP combines a localizability detection module with a constrained ICP optimization module to address LiDAR degeneracy in featureless environments.The detection module analyzes point and surface-normal correspondences, while the optimization module uses the resulting information to constrain registration.
  • d) Contributions: Across simulated environments and real-world missions, X-ICP reliably detects localizability without heuristic parameter tuning and consistently outperforms state-of-the-art approaches in challenging scenarios.The experiments use varied environments and sensor setups, including the comparison illustrated in Fig. 1.
  • d) Contributions: The framework identifies principal optimization directions as localizable, partially localizable, or non-localizable using a heuristic-free detection algorithm.This categorization supports controlled use of available correspondence information in partially localizable directions.

B. Degeneracy Detection

The paper reviews degeneracy detection methods that quantify weakly constrained registration directions, then motivates X-ICP’s direct use of degeneracy information as optimization constraints. Existing approaches face thresholding, modeling, or scope limitations that X-ICP targets.

  • B. Degeneracy Detection: Prior degeneracy detectors use uncertainty models, observability measures, Hessian condition metrics, or minimum eigenvalues to characterize insufficient geometric constraints.These approaches range from covariance-based formulations to single metrics over all six pose degrees of freedom.
  • B. Degeneracy Detection: The state-of-the-art degeneracy-factor and solution-remapping approach requires environment-dependent threshold tuning and assumes well-conditioned directions remain unaffected by degeneracy.Its binary formulation also cannot represent translation and rotation scales with one singular-value threshold.
  • B. Degeneracy Detection: Learning-based methods estimate localizability from scan overlap or learned risk metrics, offering alternatives to geometric degeneracy measures.The supplied review identifies these as distinct from direct geometric analysis of registration structure.
  • C. Constrained Optimization in Point Cloud Mapping: Existing constrained-registration methods improve global robustness or accuracy but do not address using constraints to limit degeneracy effects in the optimization.A related factor-graph approach is tested only in 2D and does not introduce constraints directly into the registration optimization.
  • B. Degeneracy Detection: X-ICP is framed as a localizability-guided constrained registration method that detects and mitigates LiDAR degeneracy within scan-to-map ICP.The formulation is demonstrated by embedding its modules in an existing scan-to-map registration system.

A. Point Cloud Registration

The registration problem estimates a rigid SE(3) transformation aligning a LiDAR scan with a map through correspondences and point-to-plane ICP. Linearized optimization repeatedly solves for translation and rotation until convergence.

  • A. Point Cloud Registration: Point cloud registration estimates the rigid transformation T_ML ∈ SE(3) aligning a LiDAR reading cloud with a reference map cloud.The scan and map contain Np and Nq points, respectively, in their associated coordinate frames.
  • A. Point Cloud Registration: ICP forms correspondences by matching each transformed scan point to a nearby map point, with matched surface normals supporting the registration objective.The initial transformation improves correspondence search and optimization convergence.
  • A. Point Cloud Registration: The method uses a point-to-plane cost function and solves its linearized quadratic optimization with direct linear-algebra solvers such as SVD.The resulting six-dimensional system estimates the alignment translation and rotation.
  • A. Point Cloud Registration: The optimization variables comprise a rotation vector and translation vector, while the Hessian captures the local behavior and conditioning of the registration problem.The Hessian and constraint vector define the linearized optimization system.
  • A. Point Cloud Registration: Because ICP is nonlinear and iterative, correspondence, linearization, and optimization operations repeat until convergence.The framework overview places this loop after transforming and undistorting the input scan using a pose prior.

B. Operation in Degenerate Environments

X-ICP addresses under-constrained LiDAR registration by detecting localizability per optimization direction and constraining pose updates accordingly. Its modules preserve unchanged estimates in non-localizable directions, control partially localizable updates, and leave fully localizable directions unconstrained.

  • X-ICP detects LiDAR degeneracy and mitigates its optimization effects through coupled localizability-detection and constrained-optimization modules.The framework is embedded in scan-to-map ICP for reliable registration in featureless extreme environments.
  • The localizability vector approximates the ICP Hessian null space directly in eigenspace, so degenerate directions need not align with robot or map frames.The vector represents localizability across all six optimization degrees of freedom using translation and rotation eigenvectors.
  • The Opt.-Module leaves non-localizable directions unchanged, controls partially localizable updates, and applies no constraint to fully localizable directions.These outcomes use the categorical localizability state for each principal direction.
  • X-ICP provides reliable pose estimation in degenerate settings and reports accurate, generalizable detection and robust estimation across simulated and real-world experiments.The framework reportedly avoids environment-specific parameter tuning and consistently outperforms state-of-the-art robotic approaches in challenging experiments.
  • The Loc.-Module analyzes scan-map correspondences through information analysis, filtering, and fine-grained categorization for each optimization eigenvector.Correspondences are transformed to the LiDAR frame before localizability analysis to reduce map-scale effects on rotational directions.

A. Information Analysis

X-ICP estimates directional localizability from geometric information in point-normal correspondences rather than relying directly on Hessian eigenvalues. It projects force- and torque-related information into translation and rotation eigenspaces to quantify contributions along optimization directions.

  • X-ICP forms rotational and translational information from surface normals and point-normal torques, then projects these quantities into Hessian eigenspaces.The Jacobian contains n for translation and p × n for rotation, while moment normalization addresses point-distance scale effects.
  • The Loc.-Module separates translation and rotation Hessian blocks before eigen-analysis because their variables differ in scale and type.The resulting matrices V_t and V_r contain the corresponding eigenvectors, while Σ_t and Σ_r contain eigenvalues.
  • Eigenvalues measure information along paired eigenvectors but are excluded from localizability estimation because they vary inconsistently across environments and sensors.The method instead uses Jacobian-derived information contributions.
  • Each information pair contributes differently to different principal directions, allowing surface normals and induced torques to characterize translational and rotational localizability.The 2D example illustrates directional contributions from point-normal pairs and torque magnitudes.
  • Contribution values are projected per eigenvector, with 1.0 denoting maximum contribution and 0.0 denoting no contribution.A histogram can reveal mostly weak contributions alongside a small highly contributing structure that must later be consolidated.

B. Filtering

The filtering step removes redundant or weakly informative correspondence contributions before summarizing geometric information. It uses a user-defined alignment threshold to reject values likely dominated by noise and retain reliable contributions.

  • Filtering removes redundant localizability contributions so the remaining information becomes interpretable for categorization.Low contributions may be indistinguishable from measurement or feature-extraction noise.
  • The binary filtering operation retains contribution values meeting the filtering parameter κ_f and rejects the rest as outliers.κ_f captures sensor and feature-extraction noise characteristics.
  • The filtering threshold is sensor-dependent: Velodyne experiments use κ_f = cos(80°) ≈0.1736, while the Seemühle Ouster experiment uses cos(60°) = 0.5.The higher Ouster threshold produces more aggressive filtering because of higher point-variance noise.

2) Filtering High Contribution:

X-ICP separately aggregates reliable contributions and strong alignment contributions, then discretizes the resulting directional evidence into localizability categories. These categories determine whether optimization directions receive no, controlled, or unrestricted updates.

  • The combined localizability vector summarizes reliable geometric information across all six eigenvector directions after filtering.Higher numbers of filtered contribution values indicate more available contribution information.
  • Strong-contribution filtering keeps alignment values above cos(45°) ≈0.707, producing a strong vector that is less sensitive to sensor noise.The combined vector retains weak but reliable contributions needed for partial localizability, whereas the strong vector emphasizes the most aligned evidence.
  • Discrete categories enable hard constraints that avoid divergence along constrained directions during least-squares optimization.The constrained module represents ill-conditioned directions with linear constraints in 3D planes.
  • Three thresholds define the categories: κ1 bounds full localizability, κ2 bounds partial localizability, and κ3 marks minimum information for the partial-to-none transition.They satisfy κ1 ≥ κ2 > κ3 and are selected based on the employed ICP algorithm's convergence basin.
  • The decision tree classifies each eigenvector as full, partial, or none by comparing combined and strong contributions against κ1, κ2, and κ3.Full indicates a well-constrained direction; otherwise the comparisons distinguish partial from non-localizable directions.

VI. LOCALIZABILITY AWARE OPTIMIZATION MODULE

The Opt.-Module converts localizability categories into direction-specific constraints for reliable optimization under degeneracy. It uses correspondences to estimate partial constraints and applies them along ill-conditioned eigenvector directions.

  • VI. LOCALIZABILITY AWARE OPTIMIZATION MODULE: The Opt.-Module calculates optimization constraints from localizability categories and eigenvectors to solve ill-conditioned registration problems.The constraints are applied along degenerate directions while preserving the optimization structure.
  • VI. LOCALIZABILITY AWARE OPTIMIZATION MODULE: Full-localizability directions remain unconstrained, non-localizable directions receive zero-update constraints, and partial directions receive correspondence-derived limits.Partial constraints are obtained by re-sampling information pairs associated with the relevant direction.
  • VI. LOCALIZABILITY AWARE OPTIMIZATION MODULE: Constraint values are computed by selecting correspondence pairs according to localizability conditions and solving a simplified directional minimization problem.The resulting t0 or r0 values estimate motion along the degenerate eigenvector direction.
  • VI. LOCALIZABILITY AWARE OPTIMIZATION MODULE: The re-sampling procedure exploits sparse information along partially localizable directions while requiring conditioning safeguards for directionally selected Hessian matrices.LU decomposition with pivoting and RIF preconditioning reduce risks from ill-conditioning.

B. Constrained Optimization

The calculated directional constraints are assembled into a constrained least-squares ICP problem. Lagrangian multipliers and SVD then produce the optimal pose estimate for the current iteration.

  • B. Constrained Optimization: The directional constraints are extended to six dimensions and arranged as equality constraints in matrix form Cx = d.The total number of constraints is at most six, spanning translational and rotational components.
  • B. Constrained Optimization: The constrained registration problem is transformed into an augmented least-squares system using Lagrangian multipliers.This formulation preserves the equality constraints while integrating them into the ICP optimization.
  • B. Constrained Optimization: SVD solves the augmented system and provides the optimal pose estimate for the current ICP iteration.The resulting estimate is denoted x′∗.
  • B. Constrained Optimization: A truly bad initial guess can still make registration unreliable because correspondence errors and unchanged fully degenerate directions preserve errors from the initial pose.This limitation affects all compared methods because they use the same initial guess.

VII. RESULTS

The framework is evaluated in controlled simulations and real-world settings designed to expose translational, rotational, and broader LiDAR degeneracy. Results emphasize localizability detection and registration robustness under noisy or geometrically weak conditions.

  • VII. RESULTS: The real-world evaluation uses an ANYmal-C robot with a Velodyne VLP-16 LiDAR, IMU, and joint encoders, alongside reimplemented comparison methods in a common ICP pipeline.The experiments include diverse simulated and field environments with multiple sensor setups.
  • VII. RESULTS: Simulated environments provide controlled tests of localizability detection across planar, corridor-like, cylindrical, and open-area geometries known to induce ICP degeneracy.The experiments also vary the registration prior to assess detection under controlled conditions.
  • VII. RESULTS: X-ICP correctly identifies single-axis translational degeneracy under a noisy prior, while Zhang et al. detects three degenerate directions and produces greater tunnel-direction error and drift.The comparison uses the translational degeneracy dataset shown in Fig. 8.
  • VII. RESULTS: In the cylindrical rotational-degeneracy test, X-ICP detects degeneracy near the environment center, whereas Zhang et al. reports it near the circular wall.The differing predictions expose sensitivity to the observed number of points.

3) Combined Degeneracy:

The combined-degeneracy experiment tests whether X-ICP distinguishes corridor and open-space failure directions during a 246 m traversal. Real-world Seemühle results further show reliable tunnel registration and better local consistency than the compared methods.

  • 3) Combined Degeneracy:: X-ICP detects one non-localizable axis in corridor sections and three non-localizable axes only during the 200–300 s open-space interval.The 246 m trajectory therefore tests changing degeneracy structure within one mission.
  • 1) Velodyne VLP-16 LiDAR:: In Seemühle, Zhang et al. and Hinduja et al. suffer drift or larger registration error, while X-ICP maintains reliable tunnel registration with a low-error map.The compared methods use pessimistic or incorrect localizability estimates that over-rely on noisy odometry.
  • 1) Velodyne VLP-16 LiDAR:: X-ICP detects tunnel degeneracy in eigenspace even when the degenerate direction is not aligned with Cartesian axes.Its categorization also distinguishes local geometric differences at a tunnel bend.
  • 1) Velodyne VLP-16 LiDAR:: APE and end-translation results indicate drift for the compared methods, while RPE results show that X-ICP performs better in local consistency.The reported metrics are evaluated using the Seemühle VLP-16 experiment tables.

2) Ouster OS0-128:

With the denser Ouster OS0-128 sensor, all evaluated approaches improve, while X-ICP retains more accurate mapping and detects tunnel degeneracy without heuristic threshold tuning.

  • Ouster OS0-128: All three approaches perform better with the higher-density, larger-field-of-view Ouster OS0-128 sensor than with the VLP-16 LiDAR.The experiment reduces the filtering threshold κf to 60° to accommodate increased sensor noise.
  • Ouster OS0-128: X-ICP shows less map point-to-point error in the cave interior and correctly captures tunnel degeneracy with the Ouster OS0-128 LiDAR.The comparison reports that Zhang et al. completes the trajectory without substantial drift, whereas Hinduja et al. still produces visible drift.
  • Rümlang Construction Site: X-ICP detects the Rümlang site's three degenerate directions and the smooth transition between partial and non-localizability.The three degenerate directions have comparably low combined contribution values, while one direction is mostly partially localizable.
  • Rümlang Construction Site: Zhang et al.'s single Hessian-eigenvalue threshold performs poorly because rotation and translation eigenvalues occupy different scales.The experiment identifies single-threshold detection as difficult across rotational and translational subspaces.
  • Opfikon City Park: In the Opfikon park, X-ICP produces the most accurate and fine-detailed map despite noisy odometry and vegetation-related sensing challenges.The comparison reports visible degeneracy effects for Zhang et al. even after threshold tuning, while Hinduja et al. introduces significant drift.
  • Ablation: Xs-ICP removes partial localizability and uses binary detection, whereas X-ICP retains three-level categorization for finer analysis.The ablation defines Xs-ICP with κ1 ≥ κ2 = κ3, using κ1 = 250 and κ2 = 180.

1) Seemühle:

The Seemühle and Rümlang ablations show that partial localizability improves registration, while the proposed method adds computational overhead to analyze correspondences during ICP iterations.

  • Seemühle: X-ICP consistently improves translation estimation over Xs-ICP in Seemühle, while rotation estimation remains comparable between the variants.The APE and end-translation errors are consistently better for X-ICP, whereas RPE rotation performance is comparable.
  • Seemühle: Disabling partial localizability increases pose drift because Xs-ICP treats a partially informative tunnel direction as non-localizable.X-ICP uses sparse but valuable information in that direction, while Xs-ICP relies fully on the prior.
  • Rümlang: In Rümlang, Xs-ICP produces duplicated wall point clouds and slight rotational misalignment, whereas the partial category improves localizability estimation and mapping.X-ICP identifies two non-localizable translation directions and one partially localizable rotation direction from informative correspondence pairs.
  • Computational Analysis: X-ICP's localizability detection adds per-iteration computational overhead that scales with the number of scan-to-map correspondences.The scalability analysis considers one- and three-axis degeneracy cases for X-ICP and all degeneracy conditions for Xs-ICP.
  • Conclusion: The framework combines environmental degeneracy detection with constraints that prohibit pose updates along degenerate directions.The conclusion emphasizes partial localizability as a way to exploit sparse but valuable information in real-world scenarios.
  • Limitations: The method remains sensitive to the quality of the initial pose guess, and future work targets sensor-dependent threshold selection.The authors propose point-wise confidence weights to reduce noise effects in contribution calculation.
Loading 2211.16335v4…