SVN-ICP: Uncertainty Estimation of ICP-based LiDAR Odometry using Stein Variational Newton

Basic Information

• Paper ID: 2509.08069
• Title: SVN-ICP: Uncertainty Estimation of ICP-based LiDAR Odometry using Stein Variational Newton
• Authors: Shiping Ma (TU Berlin), Haoming Zhang (TU Munich), Marc Toussaint (TU Berlin)
• Classification: cs.RO (Robotics)
• Publication Date/Venue: IEEE Robotics and Automation Letters, Accepted September 2025
• Paper Link: https://arxiv.org/abs/2509.08069

Abstract

This paper proposes SVN-ICP, a novel Iterative Closest Point (ICP) algorithm that performs uncertainty estimation on manifolds using Stein Variational Newton (SVN) methods. The approach is designed for LiDAR odometry fusion in multi-sensor systems, ensuring accurate pose estimation and consistent noise parameter inference even in LiDAR-degraded environments. By employing particle approximations of the posterior distribution within the Stein variational inference framework, SVN-ICP eliminates the need for explicit noise modeling or manual parameter tuning. The authors integrate SVN-ICP into a simple error-state Kalman filter combined with IMU measurements and test it on multiple datasets covering diverse environments and robot platforms. Experimental results demonstrate that the method outperforms state-of-the-art approaches in challenging scenarios while providing reliable uncertainty estimates.

Research Background and Motivation

Core Problem

Traditional ICP algorithms provide only point estimates, lacking the uncertainty characterization necessary for multi-sensor fusion. Existing fusion methods typically rely on fixed heuristic rules and manually designed noise models based on sensor noise, geometry, or scan convergence properties.

Problem Significance

In robot navigation, accurate uncertainty estimation is crucial for robust state estimation, particularly in LiDAR-degraded environments such as dust-filled caves and geometrically degenerate corridors. These environments may simultaneously exhibit multiple sources of uncertainty, making uncertainty modeling a challenging problem.

Limitations of Existing Methods

1. Stein ICP Limitations: As a first-order method, it performs poorly on ill-conditioned problems, exhibits slow convergence, and is sensitive to step sizes
2. Euclidean Representation Issues: Requires separate gradient and kernel design computations for translation and rotation, deviating from SE(3) manifold structure
3. Manual Noise Models: Difficult to adapt to different environments, often resulting in overconfident uncertainty estimates

Research Motivation

Develop a method capable of deriving direct uncertainty measures through non-parametric variational distributions without requiring explicit uncertainty modeling or detection techniques.

Core Contributions

1. Novel Algorithm: Proposes SVN-ICP, an ICP LiDAR odometry method based on Stein Variational Newton on manifolds with built-in uncertainty quantification capabilities
2. Comprehensive Evaluation: Evaluates estimated uncertainties by integrating SVN-ICP into a Kalman filter on two challenging datasets and benchmarks against state-of-the-art methods
3. Efficient Implementation: Develops an early stopping mechanism for SVN with GPU-accelerated C++ implementation

Methodology Details

Task Definition

Given source point cloud P = {pₙ} and target point cloud Q = {qₙ}, along with prior pose Ť and covariance Σ̌, output posterior pose T̂ and covariance Σ̂.

Core Technical Framework

1. ICP on Manifolds

Employs right-hand perturbation model for pose representation on SE(3) manifold:

L(ξ) = Σₙ ||(exp(ϑ)pₙ + p) - qₙ||²

where ξ = p ϑᵀ ∈ R³×so(3) represents the minimal pose increment between two point clouds.

2. Stein Variational Newton Method

Unlike the first-order gradient flow of SVGD, SVN incorporates second-order information for Newton-like updates:

SVGD Update:

ϕ*(ξₖ) = (1/K) Σₗ [k(ξₗ,ξₖ)b(ξₖ) + ∇ξₗk(ξₗ,ξₖ)]

SVN Update:

ξₖ ← ξₖ + H̃⁻¹ϕ*(ξₖ)

where the preconditioned Hessian matrix is:

H̃(ξₖ) = (1/K) Σₗ [Hₖ(ξ)k(ξₗ,ξₖ)² + (∇ξₖk(ξₗ,ξₖ))⊗²]

3. Algorithm Pipeline

1. Initialize K perturbed particles
2. Find corresponding target point cloud subsets via KNN
3. Iteratively execute SVN updates until convergence or early stopping condition is met
4. Compute mean perturbation state and covariance among particles
5. Propagate covariance to global coordinate system using adjoint transformation

Technical Innovations

1. Manifold Representation: Employs right-hand perturbation model for better decoupling of translation and rotation, improving numerical stability
2. Second-Order Optimization: SVN incorporates curvature information for more stable and rapid convergence under ill-conditioned problems
3. Voxel Sampling: Replaces mini-batch sampling to ensure higher variational inference quality while significantly reducing computational cost
4. Early Stopping Mechanism: Monitors average norm of particle state updates to improve computational efficiency

Experimental Setup

Datasets

• SubT-MRS Dataset: Primarily contains mixed indoor environments covering UGV, aerial, handheld, and legged robot platforms
• GEODE Dataset: Includes challenging outdoor scenarios such as off-road trails and waterways

Evaluation Metrics

• General Error Metrics: Absolute Pose Error (APE), Relative Pose Error (RPE)
• Uncertainty Quality Metrics: Kullback-Leibler divergence, Normalized Norm Error (NNE)

Comparison Methods

• System-Level Methods: Liu et al., Weitong et al., Kim et al., Zhong et al.
• Odometry Methods: LIO-EKF, KISS-ICP, GenZ-ICP, Stein ICP
• Paper Variants: SVGD-ICP, SVN-ICP, SVN-ICP+KF

Implementation Details

• 30 particles, maximum 100 iterations
• RBF kernel: k(ξₗ,ξₖ) = exp(1/h||ξₗ-ξₖ||₂²)
• GPU-accelerated C++ implementation with ROS2 interface

Experimental Results

Main Results

SubT-MRS Dataset Performance

Among 11 test sequences, SVN-ICP+KF demonstrates superior performance in multiple scenarios:

• Urban Sequence: APE 0.478m (vs. baseline methods 0.307-13.16m)
• Tunnel Sequence: APE 0.085m, comparable to best methods
• Cave Sequence: APE 0.532m, outperforming most baseline methods

GEODE Dataset Performance

• Waterway Long Sequence: APE 15.58m, significant improvement compared to 36.80m with fixed noise parameters
• Bridge1 Sequence: Maintains stability in extremely challenging featureless repetitive structures (APE 44.27m)

Uncertainty Estimation Quality

Comparison with 1000 Monte Carlo samples on Long Corridor sequence:

Ablation Studies

Convergence Analysis

• SVN-ICP converges within 30-75 iterations
• SVGD-ICP exhibits slow sawtooth convergence, often requiring full 100 iterations

Particle Count Impact

• 30 particles provide good balance between accuracy and efficiency
• 5-10 particles suffice for robust sensor fusion
• Runtime: SVN-ICP (0.483s) vs SVGD-ICP (0.669s)

Key Findings

1. Adaptive Uncertainty: Dynamic ICP noise parameter updates significantly improve Kalman filter robustness
2. Geometric Degeneracy Handling: In structured degenerate environments like corridors, uncertainty estimates align with environmental structure and robot motion
3. Multimodal Distributions: Estimated uncertainties exhibit non-Gaussian characteristics with multimodal and heavy-tailed features

Related Work

LiDAR Odometry

• Feature-Based Methods: Match sparse geometric features (edges, planes)
• Direct Methods: Raw point cloud matching based on ICP or NDT
• Fusion Methods: Kalman filter schemes with tightly-coupled IMU integration

Uncertainty-Aware Methods

• Degeneracy Detection: Analyze normal vectors, scan matching residuals, or Hessian matrices
• Uncertainty Estimation: Closed-form covariance estimation, learning-based methods, Bayesian ICP

Stein Variational Inference

• SVGD: Particle variational inference using kernelized Stein discrepancy
• SVN: Newton-style updates incorporating second-order information

Conclusions and Discussion

Main Conclusions

1. SVN-ICP successfully combines accurate pose estimation with consistent uncertainty quantification
2. In LiDAR-degraded environments, adaptive uncertainty estimation significantly improves sensor fusion performance
3. Second-order optimization methods exhibit better convergence and stability under ill-conditioned problems

Limitations

1. Motion Unobservability: Still limited during aggressive motion in unstructured environments
2. Dynamic Objects: Scenes containing moving objects may cause severe map corruption
3. Simple Framework: Current implementation employs basic odometry design and simple Kalman filtering

Future Directions

1. Efficient uncertainty-aware LiDAR odometry for active SLAM
2. Research on kernel parameterization
3. Extension to other range sensors (e.g., radar)

In-Depth Evaluation

Strengths

1. Theoretical Innovation: Introduces SVN to ICP problems, providing principled uncertainty estimation methods
2. Strong Practicality: Requires no manual tuning with strong adaptability
3. Comprehensive Experiments: Thorough evaluation on multiple challenging datasets
4. Open-Source Contribution: Provides efficient C++ implementation with GPU support

Weaknesses

1. Computational Complexity: Still exhibits higher computational overhead compared to traditional ICP methods
2. Parameter Sensitivity: Selection of parameters such as kernel bandwidth may affect performance
3. Insufficient Theoretical Analysis: Lacks theoretical guarantees for convergence and uncertainty quality

Impact

1. Academic Value: Provides novel perspectives for uncertainty estimation in SLAM
2. Practical Value: Directly applicable to multi-sensor fusion systems
3. Reproducibility: Provides detailed implementation and open-source code

Applicable Scenarios

• Multi-sensor fusion systems requiring uncertainty estimation
• Robust localization in LiDAR-degraded environments
• Applications with strict pose estimation quality requirements

References

The paper cites 68 relevant references covering important works in SLAM, probabilistic inference, sensor fusion, and other domains, providing a solid theoretical foundation for this research.

Overall Assessment: This is a high-quality paper with significant contributions to robotics, successfully applying advanced variational inference techniques to LiDAR odometry problems while achieving a good balance between theoretical innovation and practical utility.