Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

Shiping Ma, Haoming Zhang, Marc Toussaint

TL;DR

SVN-ICP tackles the lack of principled uncertainty in ICP-based LiDAR odometry by casting pose estimation as a probabilistic inference problem on SE(3) and solving it with Stein Variational Newton on manifolds. By using a Newton-like, second-order variational update with kernel interactions, it delivers faster, more stable convergence than gradient-only methods and yields coherent uncertainty estimates that improve multisensor fusion through a Kalman filter. The approach is validated on SubT-MRS and GEODE datasets, showing strong robustness in degraded environments and offering explicit uncertainty that enhances state estimation in LiDAR–Inertial fusion. This work advances practical, uncertainty-aware LiDAR odometry suitable for real-time multisensor navigation and as a step toward active SLAM with principled uncertainty handling.

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.

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

TL;DR

SVN-ICP tackles the lack of principled uncertainty in ICP-based LiDAR odometry by casting pose estimation as a probabilistic inference problem on SE(3) and solving it with Stein Variational Newton on manifolds. By using a Newton-like, second-order variational update with kernel interactions, it delivers faster, more stable convergence than gradient-only methods and yields coherent uncertainty estimates that improve multisensor fusion through a Kalman filter. The approach is validated on SubT-MRS and GEODE datasets, showing strong robustness in degraded environments and offering explicit uncertainty that enhances state estimation in LiDAR–Inertial fusion. This work advances practical, uncertainty-aware LiDAR odometry suitable for real-time multisensor navigation and as a step toward active SLAM with principled uncertainty handling.

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.

Paper Structure

This paper contains 21 sections, 20 equations, 3 figures, 4 tables, 1 algorithm.

Table of Contents

  1. Introduction
  2. Related Work
  3. LiDAR-based Odometry
  4. Uncertainty-Aware LiDAR Odometry
  5. Preliminaries
  6. Stein Variational Gradient Descent
  7. Stein ICP
  8. SVN-ICP
  9. ICP on Manifold
  10. Solving ICP with Stein Variational Newton
  11. The SVN-ICP Algorithm
  12. Implementation
  13. Experiments and Results
  14. General Performance Metrics
  15. Results on SubT-MRS Dataset
  16. ...and 6 more sections

Figures (3)

  • Figure 1: Uncertainty-aware LiDAR-Inertial Odometry using SVN-ICP (ours) evaluated on the Long Corridor sequence from the SubT-MRS dataset Data_subtmrs. (a) Generated LiDAR map with timestamp annotations. (b-c) Estimated $6$-D uncertainties (variance) visualized as $3\hat{{\sigma}}_{}^{}$ bounds along with error samples from the test run. (d) Smoothed Kalman gain computed given the estimated ICP variance.
  • Figure 2: $1\sigma$ estimates in the $x$ direction with varying particles $K$.
  • Figure 3: Convergence analysis of SVGD-ICP and SVN-ICP on the Long Corridor sequence: (a-d) particle distributions of frame 1136, (e) norms of state updates over successive iterations. While SVN-ICP converges around the 75th iteration, the particles of SVGD-ICP continue to exhibit noticeable movement at 100th iteration.