← Back ICRA 2026

SVN-ICP: Uncertainty Estimation of ICP-Based LiDAR Odometry Using Stein Variational Newton

Shiping Ma, Haoming Zhang, Marc Toussaint

PDF

AI summary

Key figure (auto-extracted from paper)

SVN-ICP automatically quantifies LiDAR odometry uncertainty in degraded environments without manual tuning, enabling robust sensor fusion.

LiDAR Odometry Uncertainty Estimation Stein Variational Newton ICP Sensor Fusion Probabilistic Inference

Problem

Standard ICP-based LiDAR odometry lacks reliable uncertainty quantification, forcing multisensor fusion systems to rely on fixed heuristics or hand-crafted noise models that degrade in complex or degenerative environments.

Approach

SVN-ICP approximates the pose posterior using particles via Stein Variational Newton on the SE(3) manifold, automatically deriving consistent uncertainty estimates without explicit noise modeling.

Key results

Eliminates manual noise tuning while maintaining accurate pose estimation
Provides reliable, adaptive uncertainty estimates that reflect LiDAR degradation
Outperforms best-in-class baselines on SubT-MRS and GEODE datasets
Achieves efficient computation via GPU acceleration and early-stopping

Why it matters

Enables robust, uncertainty-aware sensor fusion for autonomous navigation in real-world, LiDAR-degraded environments where traditional methods fail.

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. We re- lease our code at https://github.com/LIS-TU-Berlin/SVN-ICP.git. A high-resolution video demonstration is available at https: //youtu.be/c4QsMd1weik.

Index terms

SLAM Probabilistic Inference Sensor Fusion