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 ICPs 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 Hess ian for white noise, but which is completely different otherwise. This difference is very important, since between random noi se 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-f orm expressions. I. I NTRODUCTION This paper considers the observability and covariance of estimates obtained by applying the Iterative Closest-Poin t (ICP) algorithm [1], [2] to pairs of successive point clouds captured by a scanning sensor (for instance a 2D laser or a Kinect camera) moving through a structured environment. This so-called scan matching [3] is used to estimate the pose of the robot carrying the scanner in either 2D (heading R ∈ SO(2) and in-plane positionp ∈ R) or 3D (attitude R ∈ SO(3) and positionp ∈ R). The observability and covariance of the scan matching estimates enables diagnosi ng their accuracy and fusing them with data from other sensors, using for instance an Extended Kalman Filter (EKF) e.g. [4], [5] or more recent filters such as the Unscented Kalman Filter (UKF) or the particle filter. Observability of point-to-plane ICP (aka ICP stability or sensitivity analysis) was previously studied in various wo rks, see e.g. [6], [7]. The first contribution of the present paper is to provide a formal proof that in the case of point-to-plane ICP — butnot point-to-point — the lack of observability in under-constrained environments (such as corridors) is cor rectly captured by the Hessian matrix of the linearized cost function. This deterministic portion is presented through out Section II with the proof given in Section II-E. Section III is concerned with the different (probabilistic ) problem of computing the covariance (i.e. variability) of the ICP output estimate over repeated experiments assuming noisy data. This has been studied in [8], [9] using repeated numerically costlyoffline computations, and in [10], [11] which deals with the slightly different problem of matching a scan with a known surface map, and focuses on the covariance projected on the observable directions. The secon d The authors are with MINES ParisTech, PSL Research University, Centre de robotique, 60 Bd St Michel 75006 Paris, Franc e {firstname.lastname}@mines-paristech.fr contribution of this paper is to derive a (easily computed) closed form for the covariance matrix, which is equal to the scaled inverse of the Hessian derived in Section II in the case of white noise, but which is completely different for other types of noise. We then demonstrate by numerical example that white noise actually has negligible impact on the covariance as compared to scanner resolution errors. II. ICP OBSERVABILITY AND STABILITY A. The ICP Algorithm The ICP algorithm [1], [2] is an iterative procedure for finding the optimum rigid-body transformation (R, T ) ∈ SO(3)× R between two sets of points in R (aka clouds) {ai} and{bi}, which do not necessarily have equal number of entries. The ICP algorithm consists of the following sequence of steps: 1) SelectN points from{ai} 2) Match these with points in{bi} 3) Minimize a distance cost function using (δR, δT ) 4) Transform{ai} and update(R, T ) by (δR, δT ) 5) If (R, T ) converged then quit, else goto 1. The details of steps 1 and 2 can vary quite a bit between implementations as seen in the excellent survey paper [12]. Step 3 uses one of two possible approaches: the point-topoint cost function [1],