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

Shiping Ma, Haoming Zhang, Marc Toussaint

Introduction

LiDAR Odometry (LO) is a foundational component of robotic navigation, providing high-precision localization by matching sequential point clouds. However, traditional Iterative Closest Point (ICP) methods often struggle with geometric degeneracy—scenarios where the environment lacks sufficient features to uniquely determine a pose, such as long, featureless corridors or tunnels. While standard algorithms provide a point estimate of the robot's position, they frequently fail to quantify the uncertainty of that estimate accurately. SVN-ICP addresses this gap by leveraging Stein Variational Newton (SVN) to provide robust pose estimation alongside reliable, real-time uncertainty quantification.

Problem Statement: The Challenge of Geometric Degeneracy

The primary limitation of conventional LO is its susceptibility to environments where every "slice" of the scene looks identical. In these cases, standard ICP provides a "best guess" that may be significantly drifted without any indication of low confidence. Previous attempts to solve this using probabilistic frameworks, such as Stein ICP, relied on first-order gradient descent. These methods are often computationally slow and prone to oscillation in ill-conditioned optimization spaces, making them difficult to deploy in complex, real-world robotic applications.

Methodology

SVN-ICP introduces a second-order variational inference framework designed to approximate the posterior distribution of a robot's pose using a set of particles. The technical core of the approach consists of three innovations:

  1. Stein Variational Newton (SVN): Unlike first-order methods, SVN incorporates second-order information via the Hessian. This allows the algorithm to account for the "curvature" of the optimization space. By understanding which directions are poorly constrained (e.g., the longitudinal axis of a tunnel), the algorithm achieves faster and more stable convergence.
  2. Manifold-based Formulation: To maintain mathematical rigor and handle 3D rotations effectively, the problem is formulated directly on the $SE(3)$ manifold rather than in Euclidean space.
  3. Right-hand Perturbation Model: The authors utilize a specific perturbation model to better decouple translation and rotation, improving numerical stability when the robot is distant from previous map points.

By maintaining a set of particles that represent the probability distribution, SVN-ICP provides a non-parametric estimate of uncertainty. These particles naturally "repel" one another to cover the posterior distribution, capturing non-Gaussian characteristics like multi-modality or "long tails" that traditional covariance matrices often miss.

Experimental Evaluation

The performance of SVN-ICP was validated using two challenging public benchmarks:

  • SubT-MRS: Collected during the DARPA SubT Challenge, featuring extreme geometric degradation in tunnels and caves.
  • GEODE: A dataset comprising off-road trails, uneven tunnels, and waterways.

The method was compared against industry-standard baselines, including LIO-SAM, FAST-LIO2, and the original Stein ICP.

Key Insights and Results

Robustness and Accuracy

In the "Long Corridor" sequences of the SubT-MRS dataset, SVN-ICP demonstrated superior robustness. While several best-in-class baselines diverged or suffered from significant drift due to the lack of longitudinal features, SVN-ICP maintained stable odometry. The second-order updates allowed the system to "sense" unobservable directions of motion, effectively growing the uncertainty estimate in those specific axes.

Truthful Uncertainty Quantification

Using metrics such as Kullback–Leibler (KL) divergence and Normalized Norm Error (NNE), the authors demonstrated that SVN-ICP produces uncertainty estimates that are significantly more consistent with actual errors than classical closed-form methods. This reliability allows for more effective sensor fusion; for instance, a Kalman Filter can dynamically lower the weight of LiDAR data when SVN-ICP signals high uncertainty, relying instead on Inertial Measurement Units (IMU).

Computational Efficiency

Despite the complexity of particle-based inference, the inclusion of the Hessian ensures that SVN-ICP converges in fewer iterations than first-order variational methods. This efficiency makes the algorithm suitable for real-time onboard processing.

Conclusion and Future Work

SVN-ICP represents a significant advancement in probabilistic LiDAR odometry, offering a bridge between high-performance localization and reliable uncertainty estimation. While the method cannot create data where none exists—truly featureless environments remain a challenge for all LiDAR-only systems—it provides the necessary metadata for a robot to navigate safely by knowing when to distrust its sensors. Future research will focus on more sophisticated map management and extending this variational framework to other sensors, such as Radar, to improve navigation in all-weather conditions.

Summary updated

BibTeX
@article{ma2025svn,
  author = {Shiping Ma and Haoming Zhang and Marc Toussaint},
  title = {{SVN-ICP: Uncertainty Estimation of ICP-based LiDAR Odometry using Stein Variational Newton}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2025},
  doi = {10.1109/lra.2025.3619836},
}
Figure