Stein ICP for Uncertainty Estimation in Point Cloud Matching

Fahira Afzal Maken, Fabio Ramos, Lionel Ott

Abstract: Quantification of uncertainty in point cloud matching is critical in many tasks such as pose estimation, sensor fusion, and grasping. Iterative closest point (ICP) is a commonly used pose estimation algorithm which provides a point estimate of the transformation between two point clouds. There are many sources of uncertainty in this process that may arise due to sensor noise, ambiguous environment, and occlusion. However, for safety critical problems such as autonomous driving, a point estimate of the pose transformation is not sufficient as it does not provide information about the multiple solutions. Current probabilistic ICP methods usually do not capture all sources of uncertainty and may provide unreliable transformation estimates which can have a detrimental effect in state estimation or decision making tasks that use this information. In this work we propose a new algorithm to align two point clouds that can precisely estimate the uncertainty of ICP's transformation parameters. We develop a Stein variational inference framework with gradient based optimization of ICP's cost function. The method provides a non-parametric estimate of the transformation, can model complex multi-modal distributions, and can be effectively parallelized on a GPU. Experiments using 3D kinect data as well as sparse indoor/outdoor LiDAR data show that our method is capable of efficiently producing accurate pose uncertainty estimates.

Introduction

Iterative Closest Point (ICP) is the gold-standard algorithm for aligning two 3D point clouds. However, standard ICP only provides a single "point estimate" of the transformation (translation and rotation). In real-world robotics—like autonomous driving or SLAM—this isn't enough. Sensor noise, partial overlaps, and ambiguous environments (e.g., a featureless corridor or a rotationally symmetric object like a bottle) create uncertainty.

Knowing this uncertainty, especially epistemic uncertainty intrinsic to the geometry of the object, is critical for safely fusing data from multiple sensors. Existing uncertainty-aware ICP methods fall into two flawed categories: closed-form solutions, which are fast but often overconfident due to poor data association modeling, and sampling-based methods like Bayesian ICP, which are accurate but computationally expensive and difficult to parallelize due to their sequential nature.

The Stein ICP Framework

The authors introduce Stein ICP, which bridges the gap between the speed of variational inference and the accuracy of MCMC sampling. Instead of finding a single pose, Stein ICP maintains a set of $K$ "particles," where each particle represents a candidate transformation. The method combines Stochastic Gradient Descent ICP (SGD-ICP) with Stein Variational Gradient Descent (SVGD) to update these particles iteratively.

Stein ICP estimates the posterior distribution of transformation parameters (x, y, z, roll, pitch, yaw) by updating a set of particles, capturing uncertainty in point cloud alignment.

The SVGD update rule consists of two parts:

  1. Gradient Descent: Particles are pulled toward the optimal alignment using the mini-batch gradients of the standard ICP cost function.
  2. A Repulsive Force: A kernel function acts as a repulsive force between the particles, preventing them from collapsing into a single local minimum.

To handle 3D rotations properly, the authors also introduced a modified Radial Basis Function (RBF) kernel specifically designed for angular distances. Because the particles do not depend on a sequential Markov chain, they can be updated independently, allowing the entire algorithm to be parallelized on a GPU.

Key Insights and Results

A primary advantage of Stein ICP is that the repulsive force allows the algorithm to capture multi-modality. For example, when aligning a rotationally symmetric object like a bowl, the particles spread out across the yaw axis. Conversely, an object with a unique feature, like a mug handle, constrains the geometry, causing the particles to peak at a single, certain pose.

Qualitative comparison of objects with varying geometric constraints: a rotationally symmetric bowl versus a mug with a handle.

Experimental evaluations on the RGB-D Object Dataset and the Newer College Dataset revealed several key performance metrics:

  • Speed: Stein ICP achieved a >5x speedup compared to Bayesian ICP. On an NVIDIA Titan-V GPU, it generated high-quality distributions in just a few seconds.
  • Distribution Quality: Stein ICP consistently outperformed state-of-the-art closed-form and covariance-based methods in Kullback-Leibler (KL) divergence and Overlapping Coefficient (OVL).
  • Efficiency: Just 100 particles were sufficient to achieve high-quality posterior distributions, whereas Bayesian ICP typically requires 1,000+ sequential samples.
  • Accuracy: Estimating the full distribution did not degrade mean accuracy; Stein ICP's mean pose estimates remained as accurate as standard point-estimate methods.

Conclusion

Stein ICP transforms probabilistic ICP from a sequential bottleneck into a highly parallelizable GPU task without sacrificing accuracy. While the method's speed is reliant on GPU availability, it provides a robust framework for real-time uncertainty estimation. Future work includes integrating these calibrated uncertainties into downstream tasks like SLAM or robotic grasping, where knowing the confidence of an object's pose can inform safer approach trajectories.

Summary updated

Abstract

Quantification of uncertainty in point cloud matching is critical in many tasks such as pose estimation, sensor fusion, and grasping. Iterative closest point (ICP) is a commonly used pose estimation algorithm which provides a point estimate of the transformation between two point clouds. There are many sources of uncertainty in this process that may arise due to sensor noise, ambiguous environment, and occlusion. However, for safety critical problems such as autonomous driving, a point estimate of the pose transformation is not sufficient as it does not provide information about the multiple solutions. Current probabilistic ICP methods usually do not capture all sources of uncertainty and may provide unreliable transformation estimates which can have a detrimental effect in state estimation or decision making tasks that use this information. In this work we propose a new algorithm to align two point clouds that can precisely estimate the uncertainty of ICP's transformation parameters. We develop a Stein variational inference framework with gradient based optimization of ICP's cost function. The method provides a non-parametric estimate of the transformation, can model complex multi-modal distributions, and can be effectively parallelized on a GPU. Experiments using 3D kinect data as well as sparse indoor/outdoor LiDAR data show that our method is capable of efficiently producing accurate pose uncertainty estimates.
BibTeX
@article{maken2022stein,
  author = {Fahira Afzal Maken and Fabio Ramos and Lionel Ott},
  title = {{Stein ICP for Uncertainty Estimation in Point Cloud Matching}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2022},
  url = {https://openreview.net/forum?id=UJIvTHLMe6M},
}
Figure