On the covariance of scan-matching techniques for localization

This paper considers the problem of estimating the covariance of the relative displacement measurement output by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots (using a sensor of the Kinect type) or for autonomous vehicles (using a sensor of the Velodyne type) where the relative displacement measurement must be fused with other sensors' measurements, such as wheel encoders or inertial sensors, in a Kalman or a particle filter. The closed-form formulas proposed in previous literature generally build upon the fact that the solution to ICP is obtained by minimizing a function of the data. In this paper, we prove this approach is on shaky ground because the rematching step of the ICP is not explicitly accounted for, and a blind application to point-to-point ICP leads to completely erroneous covariances. Yet we justify that the method is applicable to point-to-plane ICP as already known to practitioners, by proposing a geometric mathematical proof bounding the errors made.