CELLO-3D: Estimating the Covariance of ICP in the Real World

The fusion of Iterative Closest Point (ICP) registrations in existing state estimation frameworks relies on an accurate estimation of their uncertainty. In this paper, we study the estimation of this uncertainty in the form of a covariance. First, we scrutinize the limitations of existing closed-form covariance estimation algorithms over 3D datasets. Then, we set out to estimate the covariance of ICP registrations through a data-driven approach, with over 5100000 registrations on 1020 pairs from real 3D point clouds. We assess our solution upon a wide spectrum of environments, ranging from structured to unstructured and indoor to outdoor. The capacity of our algorithm to predict covariances is accurately assessed, as well as the usefulness of these estimations for uncertainty estimation over trajectories. The proposed method estimates covariances better than existing closed-form solutions, and makes predictions that are consistent with observed trajectories.

I. Introduction

The ICP algorithm [1], [2] is ubiquitous in mobile robotics for the tasks of localization and mapping. It estimates the rigid transformation between the reference frames of two point clouds, by iteratively pairing closest points in both point clouds and minimizing a distance between those pairs. This is equivalent to optimizing an objective function that maps rigid transformations to a scalar optimization score for a pair of point clouds. There is an abundance of ICP variants [3], each of which yields slightly different trans- formations due to their different objective functions. One notable variation is the choice of error metric between each pair of points, where common choices of metric are point-to- point [1] and point-to-plane [2]. The registration process is subject to a number of sources of uncertainty and error, be- cause of a bad adequation between the objective function and the desired result. Chief among them is the presence of local minima in the objective function. Other causes of uncertainty comprise noise from the range sensor, and underconstrained environments such as featureless hallways [4].