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

2025-09-09

AI Summary

To address inaccurate uncertainty estimation and difficulty in adaptive noise parameter inference for LiDAR odometry in degenerate environments, this paper proposes a manifold-based Stein Variational Newton (SVN) ICP algorithm. The method is the first to integrate SVN into the ICP optimization framework on Lie group manifolds, enabling direct particle-based approximation of the posterior pose distribution and consistent uncertainty quantification—without requiring prior noise modeling or manual hyperparameter tuning. Furthermore, an error-state Kalman filter tightly coupled with IMU measurements is incorporated to construct a robust multi-sensor odometry system. Extensive experiments on diverse real-world platforms and challenging scenarios—including low-texture regions, dynamic obstacles, and motion blur—demonstrate that the proposed approach significantly outperforms state-of-the-art methods in both localization accuracy and uncertainty calibration fidelity.

Technology Category

• Intelligent Robots: State Estimation
• Machine Learning: Calibration & Uncertainty Quantification
• Search and Optimization: Sampling/Simulation-based Search

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Experiences and lessons learnt from Web-based algorithms and system deployments
• Security and Privacy: Large-scale security measurements

Abstract

This letter introduces SVN-ICP, a novel Iterative Closest Point (ICP) algorithm with uncertainty estimation that leverages Stein Variational Newton (SVN) on manifold. Designed specifically for fusing LiDAR odometry in multisensor systems, the proposed method ensures accurate pose estimation and consistent noise parameter inference, even in LiDAR-degraded environments. By approximating the posterior distribution using particles within the Stein Variational Inference framework, SVN-ICP eliminates the need for explicit noise modeling or manual parameter tuning. To evaluate its effectiveness, we integrate SVN-ICP into a simple error-state Kalman filter alongside an IMU and test it across multiple datasets spanning diverse environments and robot types. Extensive experimental results demonstrate that our approach outperforms best-in-class methods on challenging scenarios while providing reliable uncertainty estimates.

Problem

• Estimating ICP-based LiDAR odometry uncertainty in degraded environments
• Fusing LiDAR odometry without explicit noise modeling
• Providing accurate pose estimation and consistent noise inference

Innovation

• Stein Variational Newton for uncertainty estimation
• Manifold-based particle approximation without noise modeling
• Integrated Kalman filter with IMU fusion