Estimating Motion Uncertainty with Bayesian ICP

Fahira Afzal Maken, Fabio Ramos, Lionel Ott

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.

Introduction

Iterative Closest Point (ICP) is the standard algorithm for aligning 3D point clouds in robotics and computer vision. However, standard ICP is deterministic, providing a single transformation estimate without quantifying uncertainty. In real-world scenarios, sensor noise and geometric ambiguities—such as symmetric objects or featureless corridors—make deterministic estimates unreliable. Existing uncertainty estimation methods are often too slow for real-time use or produce overconfident estimates by ignoring re-association errors. Overconfidence in robotics can lead to significant failures in navigation and mapping.

Bayesian ICP Methodology

The proposed Bayesian ICP leverages Stochastic Gradient Langevin Dynamics (SGLD) to transform the optimization problem into a sampling problem.

  1. Stochastic Foundation: It utilizes SGD-ICP, which employs mini-batches of points for gradient calculation, ensuring high computational efficiency.
  2. Langevin Dynamics: By injecting controlled Gaussian noise into gradient updates, the algorithm explores the parameter space to mimic the true posterior distribution rather than settling in a single local minimum.
  3. Preconditioning: A preconditioning matrix (similar to RMSProp) is used to handle varying scales between translation and rotation parameters.
  4. Priors: Gaussian distributions for translation and von Mises distributions for rotation are incorporated as prior knowledge.

The resulting collection of pose samples provides both an expected transformation (the mean) and a measure of uncertainty (the spread).

Experimental Evaluation

The method was evaluated using the RGB-D Scenes Dataset, focusing on both "under-constrained" objects with symmetries (soda cans, bowls) and "constrained" asymmetrical objects (mugs with handles). A "ground truth" distribution was established by running standard ICP 10,000 times with varying initializations to observe convergence patterns.

Results and Discussion

A primary advantage of Bayesian ICP is its handling of geometric symmetry. For symmetric objects like soda cans, the distribution for "yaw" (rotation around the vertical axis) is appropriately broad, reflecting the inherent ambiguity. Unlike previous methods that might pick a single angle with high certainty, Bayesian ICP captures the full range of possible alignments.

Posterior distributions for a bowl, soda can, and mug. The broader distributions for the bowl and can reflect the geometric symmetries captured by Bayesian ICP.

Key performance metrics include:

  • Speed: The algorithm generates 1,000 samples in 3 to 9 seconds on a standard CPU, reaching approximately 3 seconds with multi-threading (OpenMP).
  • Accuracy: Using Kullback-Leibler (KL) divergence, Bayesian ICP was shown to match the "true" pose distribution more accurately than common closed-form estimators like Online-ICP, particularly for symmetric objects.
  • Convergence: Near-optimal results are achieved with 2,000 to 3,000 samples, making it suitable for near-real-time applications.
Convergence of KL divergence relative to the number of samples for the bowl and can objects.

Conclusion

Bayesian ICP provides a robust framework for estimating pose uncertainty by capturing data association errors and geometric symmetries. While current limitations include the need for hyperparameter tuning, the method's efficiency opens doors for integration into 3D SLAM pipelines and robotic grasping tasks, where understanding uncertainty is vital for robust performance.

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},
}
Figure