On the Covariance of ICP-based Scan-matching Techniques

TL;DR

This paper analyzes the covariance estimation of ICP-based scan-matching, revealing limitations of existing formulas, especially for point-to-point ICP, and validating the approach for point-to-plane ICP through mathematical proof and experiments.

Contribution

It clarifies when covariance formulas for ICP are valid, providing a formal proof for point-to-plane ICP and highlighting issues with point-to-point ICP covariance estimation.

Findings

Existing covariance formulas are invalid for point-to-point ICP due to rematching issues.

The covariance estimation approach is mathematically validated for point-to-plane ICP.

Experimental results support the theoretical findings on ICP covariance accuracy.

Abstract

This paper considers the problem of estimating the covariance of roto-translations computed by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots and vehicles equipped with depth-sensing cameras (e.g., Kinect) or Lidar (e.g., Velodyne). The closed-form formulas for covariance proposed in previous literature generally build upon the fact that the solution to ICP is obtained by minimizing a linear least-squares problem. In this paper, we show this approach needs caution because the rematching step of the algorithm is not explicitly accounted for, and applying it to the point-to-point version of ICP leads to completely erroneous covariances. We then provide a formal mathematical proof why the approach is valid in the point-to-plane version of ICP, which validates the intuition and experimental results of practitioners.