Observability, Covariance and Uncertainty of ICP Scan Matching

In the first part, we study the observability and covariance of estimates of an ICP-based scan matching algorithm in both 2D and 3D cases. Using numerical examples and mathematical proofs we demonstrate that the Hessian of the point-to-plane ICP's linearized cost function correctly models the observability of the algorithm, whereas the point-to-point variant does not. In the second part, assuming noisy data, we obtain a closed-form expression for the covariance of the algorithm, which is equal to the (scaled) inverse of the Hessian for white noise, but which is completely different otherwise. This difference is very important, since between random noise and resolution errors in the scanned data, only the latter is shown to be relevant to the computation. While the analysis is based on mathematical proofs, the end goal of the paper is to provide the practitioner with simple to compute closed-form expressions.