Estimating Motion Uncertainty with Bayesian ICP

Fahira Afzal Maken, Fabio Ramos, Lionel Ott

TL;DR

This paper introduces Bayesian ICP, a novel approach to 3D point cloud registration that doesn't just give you a single "best guess" for a transformation, but a full probability distribution of possible poses. By combining Stochastic Gradient Descent (SGD) with Langevin dynamics, the authors created a method that is both computationally efficient and capable of capturing the true uncertainty caused by sensor noise and geometric symmetries.

What Problem Does This Paper Address?

Iterative Closest Point (ICP) is the "bread and butter" algorithm for robotics and computer vision when it comes to aligning two 3D point clouds. However, standard ICP is deterministic; it gives you one answer and no indication of how "sure" it is.

In the real world, sensors are noisy, and environments are often ambiguous. For example, if a robot is looking at a perfectly symmetrical soda can or a long, featureless corridor, there are many "correct" ways to align the data. Existing methods to estimate this uncertainty are either too slow for real-time use (like traditional Markov Chain Monte Carlo) or produce "overconfident" estimates that ignore the errors introduced when the algorithm re-associates points during the alignment process. Overconfidence in robotics can lead to catastrophic failures in navigation and mapping.

Methods

The authors propose Bayesian ICP, which leverages a technique called Stochastic Gradient Langevin Dynamics (SGLD).

  1. Stochastic Foundation: It builds on SGD-ICP, which uses small "mini-batches" of points rather than the entire cloud to calculate gradients, making it very fast.
  2. Injecting "Smart" Noise: SGLD turns an optimization problem into a sampling problem. By adding a specific, controlled amount of Gaussian noise to the gradient updates, the algorithm doesn't just settle into one local minimum; it "wanders" around the parameter space in a way that mimics the true posterior distribution.
  3. Preconditioning: To handle the fact that different parameters (like translation vs. rotation) change at different rates, they use a preconditioning matrix (similar to the RMSProp optimizer) to equalize the gradients.
  4. Priors: They incorporate prior knowledge using Gaussian distributions for translation and von Mises distributions for rotation.

The result is a collection of pose samples. The average of these samples gives the expected transformation, while the spread of the samples represents the uncertainty.

Datasets

The researchers evaluated their method using the RGB-D Scenes Dataset.

  • Objects: They tested on "under-constrained" objects (those with symmetries like soda cans, bowls, and table tops) and "constrained" objects (asymmetrical items like a coffee mug with a handle).
  • Ground Truth: They generated a baseline "ground truth" distribution by running standard ICP 10,000 times with different initializations to see where it converged.

Key Insights and Ideas

  • Symmetry is Uncertainty: The most striking insight is how the algorithm handles symmetry. For a soda can, the "yaw" (rotation around the vertical axis) distribution is very broad because the can looks the same from many angles. Bayesian ICP captures this "flat" uncertainty perfectly, whereas previous methods would often pick one angle and claim 100% certainty.
  • Efficiency via Mini-batches: By using mini-batches, the algorithm avoids the "big data" bottleneck of traditional MCMC. It can produce high-quality distributions in seconds rather than hours.
  • Avoiding Local Minima: Because the algorithm is essentially "shaking" the estimate with noise (Langevin dynamics), it is actually better at escaping poor local minima than standard SGD-based ICP, leading to slightly better overall accuracy.

Key Results

  • Speed: Bayesian ICP can generate 1,000 samples in about 3 to 9 seconds on a standard consumer CPU. With multi-threading (OpenMP), this drops to ~3 seconds.
  • Quality: Using the Kullback-Leibler (KL) divergence metric, the authors showed that Bayesian ICP matches the "true" distribution of poses much more accurately than "Online-ICP" (a common closed-form uncertainty estimator), especially for symmetric objects.
  • Sample Count: They found that 2,000 to 3,000 samples are sufficient to reach near-optimal results, making it practical for near-real-time robotics applications.

Limitations and Future Work

  • Hyperparameter Tuning: The method relies on several parameters (step size $\alpha$, dataset size $N$, and noise scaling) that currently require manual tuning or coarse picking.
  • Integration: While the paper proves the method works on individual objects, the next step is integrating it into full 3D SLAM (Simultaneous Localization and Mapping) pipelines.
  • Grasping: The authors suggest this could be revolutionary for robotic grasping—if a robot knows it is uncertain about an object's orientation, it can choose a more robust grasp or move to a different vantage point to reduce that uncertainty.

Summary updated

Abstract

Accurate uncertainty estimation associated with the pose transformation between two 3D point clouds is critical for autonomous navigation, grasping, and data fusion. Iterative closest point (ICP) is widely used to estimate the transformation between point cloud pairs by iteratively performing data association and motion estimation. Despite its success and popularity, ICP is effectively a deterministic algorithm, and attempts to reformulate it in a probabilistic manner generally do not capture all sources of uncertainty, such as data association errors and sensor noise. This leads to overconfident transformation estimates, potentially compromising the robustness of systems relying on them. In this paper we propose a novel method to estimate pose uncertainty in ICP with a Markov Chain Monte Carlo (MCMC) algorithm. Our method combines recent developments in optimization for scalable Bayesian sampling such as stochastic gradient Langevin dynamics (SGLD) to infer a full posterior distribution of the pose transformation between two point clouds. We evaluate our method, called Bayesian ICP, in experiments using 3D Kinect data demonstrating that our method is capable of both quickly and accuractely estimating pose uncertainty, taking into account data association uncertainty as reflected by the shape of the objects.
BibTeX
@inproceedings{maken2020estimating,
  author = {Fahira Afzal Maken and Fabio Ramos and Lionel Ott},
  title = {{Estimating Motion Uncertainty with Bayesian ICP}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2020},
  url = {https://openreview.net/forum?id=yIcIMjmSFxx},
}

Loading related content...

Figure