Martin Brossard
email: martin.brossard@mines-paristech.fr

Silvere Bonnabel
email: silvere.bonnabel@mines-paristech.fr

Axel Barrau

Sven Behnke

A New Approach to 3D ICP Covariance Estimation

des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

A New Approach to 3D ICP Covariance Estimation Martin Brossard 1 , Silvère Bonnabel 1 , and Axel Barrau 2 Abstract-In mobile robotics, scan matching of point clouds using Iterative Closest Point (ICP) allows estimating sensor displacements. It may prove important to assess the associated uncertainty about the obtained rigid transformation, especially for sensor fusion purposes. In this paper we propose a novel approach to 3D uncertainty of ICP that accounts for all the sources of error as listed in Censi's pioneering work [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], namely wrong convergence, underconstrained situations, and sensor noise. Our approach builds on two facts. First, the uncertainty about the ICP's output fully depends on the initialization accuracy. Thus speaking of the covariance of ICP makes sense only in relation to the initialization uncertainty, which generally stems from odometry errors. We capture this using the unscented transform, which also reflects correlations between initial and final uncertainties. Then, assuming white sensor noise leads to overoptimism as ICP is biased owing to e.g. calibration biases, which we account for. Our solution is tested on publicly available real data ranging from structured to unstructured environments, where our algorithm predicts consistent results with actual uncertainty, and compares favorably to previous methods.

Index Terms-probability and statistical methods, localization

I. INTRODUCTION

P OINT clouds and the Iterative Closest Point (ICP) algo- rithm play a crucial role for localization and mapping in modern mobile robotics [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF][START_REF] Holz | Registration with the Point Cloud Library[END_REF]. ICP computes an estimate of the 3D rigid transformation that aligns a reading point cloud to a reference point cloud (or more generally a model or a surface). The algorithm starts with a first transformation estimate, and repeats -until convergence -point association and least-square minimization, where initialization is naturally provided in mobile robotics by odometry [START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF][START_REF] Geneva | LIPS: LiDAR-Inertial 3D Plane SLAM[END_REF] based on wheel speeds, inertial sensors, or vision. The point association matches points between the two clouds by generally associating each point of the second cloud to its closest point in the first one. Then, the algorithm minimizes a userchosen metric between the matched points that provides an update of the current estimate. In spite of robust filtering that are broadly used during the alignment of point clouds, a.k.a. registration, ICP is subject to errors stemming from axel.barrau@safrangroup.com Digital Object Identifier (DOI): see top of this page. Fig. 1. Horizontal translation estimates according to ICP ( Ticp , red dots) for various initial estimates (T ini , black dots) and ground-truth (Ttrue, square) for registering two scans of the sequence Stairs of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where we sample 1000 initial estimates from two distributions reflecting accurate (a) and dispersed (b) ICP initialization and that respectively correspond to the easy and medium scenarios of [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF]. We see the uncertainty on the ICP estimate, that is, dispersion of red points, wholly depends on the accuracy of initialization.

There is no "uncertainty of ICP" per se.

sensor noises, underconstrained environments that result in unobservable directions, and local minima [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF][START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF].

A. Sources of ICP Uncertainty

The pioneering work of Censi [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] identifies the following sources of error for ICP registration: wrong convergence (not handled by Censi's formula), underconstrained situations, and sensor noise. As indicated by preliminary remarks in [START_REF] Pfister | Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation[END_REF][START_REF] Barczyk | Towards Realistic Covariance Estimation of ICP-based Kinect V1 Scan Matching: The 1D Case[END_REF] we believe a fourth important source is missing: the one that stems from sensor biases. In the present paper we consider indeed the following sources of error:

1) Initial Transformation: ICP is subject to error due to wrong initialization that makes the algorithm converge to a local minimum out of the attraction basin of the true solution, as largely observed in practice, see e.g. [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF][START_REF] Iversen | Prediction of ICP Pose Uncertainties Using Monte Carlo Simulation with Synthetic Depth Images[END_REF] and Figure 1. In practice it often proves to be the dominant error.

2) Sensor White Noise: each point measured in a point cloud is affected by an independent random sensor noise of centimetric magnitude which is a function of point depth and beam angle [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF][START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF].

3) Sensor Bias Noise: the observed points share common errors that stem from: temperature drift effect, i.e. stability of the laser [START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF]; observed material [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF]; incidence and beam angles resulting in large bias [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF]; or wrong calibration, e.g. [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF] found a distortion of 0.22 deg of the scan point clouds due to intrinsic calibration process. This correlated noise, a.k.a. bias, strictly limits the confidence we may have in the ICP estimate. To our best knowledge this is often omitted with a few exceptions: e.g., [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF] removes bias on point measurements due to sensor beam angle, and preliminary ideas may be found in [START_REF] Pfister | Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation[END_REF][START_REF] Barczyk | Towards Realistic Covariance Estimation of ICP-based Kinect V1 Scan Matching: The 1D Case[END_REF].

4) Randomness Inherent to the ICP Algorithm: ICP is generally configured with random filtering processes [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF], e.g. sub-sampling, such that two solutions with exactly the same inputs would differ.

In the following we address uncertainty coming from 1), 2) and 3) and do not consider 4), which should be marginal.

B. Brief Literature Review

Various approaches exist for estimating the covariance of the ICP algorithm, each of which being a trade-off between accuracy and execution time. Monte-Carlo algorithms, e.g. [START_REF] Iversen | Prediction of ICP Pose Uncertainties Using Monte Carlo Simulation with Synthetic Depth Images[END_REF][START_REF] Bengtsson | Robot Localization Based on Scanmatching[END_REF], sample noisy scans (from a reference scan) and ICP initializations to compute a large number of ICP registration results, define the covariance of the sampled results as the covariance estimation, and use the estimated covariance for all future registration with the reference scan, thus getting a covariance function of the reference scan only. Another category of covariance estimation methods relies on closedform expressions [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Biber | The Normal Distributions Transform: a New Approach to Laser Scan Matching[END_REF]- [START_REF] Prakhya | A Closed-form Estimate of 3D ICP Covariance[END_REF], whose underlying assumption consists in linearizing the objective function used in ICP around the convergence point, ruling out the possibility for wrong convergence and the uncertainty that stems from it. Albeit still used in practice, Censi's pioneering formula [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] is widely considered as overoptimistic, see e.g. [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF]. Recently, [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF] leveraged learning based approaches to estimate ICP uncertainty stemming from inaccurate ICP initialization.

C. Contributions and Paper's Organization

Our approach introduced in Section II extends existing works in three ways: 1) we consider ICP uncertainty coming both from sensor errors and ICP initialization. 2) we raise an important point which is that ICP uncertainty in itself is meaningless as it is inherently related to uncertainty in the initialization pose (unless there is a global minimum). This is supported by experiments displayed in Figure 1. We address this problem by outputting a covariance matrix of larger dimension that also reflects the correlation between ICP final and initial estimates. And 3) we estimate in Section III the ICP uncertainty combining a closed-form expression using [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bonnabel | On the Covariance of ICP-based Scan-matching Techniques[END_REF] accounting for sensor biases, and derivative-free methods using the unscented transform of [START_REF] Julier | A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[END_REF][START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF], which comes at a lower computational cost than Monte-Carlo runs.

Besides, we evaluate, compare and discuss our approach on the dataset of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF] in Sections IV and V, where our approach obtains consistent estimates and achieves better results than existing methods. The code to reproduce the results of the paper is made publicly available at: https://github. com/CAOR-MINES-ParisTech/3d-icp-cov.

Throughout the article, we configurate the ICP as suggested in [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] with a point-to-plane error metric.

II. PROPOSED APPROACH A. Pose Representation and Pose Uncertainty Representation

The true transformation between two point clouds and its ICP-based estimate both live in the set of 3D rigid transformations

SE(3) := T = R t 0 1 ∈ R 4×4 |R ∈ SO(3), t ∈ R 3 ,
and are thus represented by a matrix T (a.k.a. homogeneous coordinates), where R denotes a rotation matrix and t a translation. Note that, it is consistent with matrix multiplication: if T 1 transforms a first point cloud into a second one, and then T 2 transforms the latter into a third cloud, then the matrix T 2 T 1 ∈ SE(3) encodes the transformation between the first and the third clouds. It is possible to linearize poses through the approximations cos(α)

1 and sin(α) α for small α. For example, we have for a small rotation around the x axis of angle α

R x,α =   1 0 0 0 cos(α) -sin(α) 0 sin(α) cos(α)     1 0 0 0 1 -α 0 α 1   I 3 +   0 0 0 0 0 -α 0 α 0   = I 3 + α(   1 0 0   ) × , (1)
where (b) × ∈ R 3×3 denotes the skew symmetric matrix associated with cross product with b ∈ R 3 . Along those lines, a full rotation R may be approximated as

R   1 -γ β γ 1 -α -β α 1   = I 3 + (   α β γ   ) × (2)
for small rotations around the x, y and z axes. The identity pose writes Id = I 4 and a transformation being close to identity may thus be linearized as T I 4 + ξ ∧ with

ξ ∧ := (φ) × ρ 0 0 ∈ R 4×4 , ξ = φ ρ , φ ∈ R 3 , ρ ∈ R 3 . (3)
This may serve as an uncertainty representation for poses as follows. If ξ is taken random, typically we take a Gaussian ξ ∼ N (0, Q), where Q ∈ R 6×6 is the covariance matrix, then I 4 + ξ ∧ defines a small random pose close to identity, i.e., a small transformation. In turn, for a given pose T, the transformation T = T (I 4 + ξ ∧ ) = T + Tξ ∧ denotes a random transformation being close to T. T may be viewed as the noise free mean of the random pose T, and Q encodes the dispersion around the mean value T.

A further theoretical step in this direction consists in using the notion of concentrated Gaussian distribution as advocated in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF], see also [START_REF] Barrau | Invariant Kalman Filtering[END_REF]- [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF],

T = T exp(ξ), where ξ ∼ N (0, Q) , ξ ∈ R 6 , (4)
with ξ a zero-mean Gaussian variable of covariance Q and where exp(•) denotes the exponential map of SE(3). The latter maps elements ξ to poses. Albeit sounder from a mathematical standpoint, this is very close to what we have just presented since the exp(•) map has the property that exp(ξ) I 4 + ξ ∧ up to first order term in ξ. For uncertainty representation (4), we adopt the notation T ∼ N L (T, Q). Note that, in (4), the vector ξ ∈ R 6 may be viewed as the error between T and T. Indeed the relative transformation between poses T and T is encoded in ξ as T -1 T = exp(ξ).

B. The Role of ICP Initialization

The ICP procedure seeks to estimate the transformation T true ∈ SE(3) that maps a first cloud of points P to a second cloud (or a model) Q as follows [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF][START_REF] Holz | Registration with the Point Cloud Library[END_REF]: i) we have a first "guess" for the transformation we call T ini , a.k.a. initial or coarse alignment [START_REF] Holz | Registration with the Point Cloud Library[END_REF]; ii) then we initialize the ICP algorithm by applying a transformation T -1 ini to the cloud Q. This way the transformation the ICP seeks to estimate become the relative pose T rel := T -1 ini T true . We thus get an estimate

Trel = icp P, T -1 ini Q for T rel ; iii) finally the estimate of T true that the algorithm outputs is Ticp := T ini Trel = T ini icp P, T -1 ini Q . Note that if T rel is perfectly estimated we recover T true as then Ticp = T ini T rel = T ini T -1
ini T true = T true no matter how far the initial guess T ini is from T true .

Let us introduce the various errors at play in ICP. In robotics, the initial guess in i) is typically provided through inertial sensors or wheeled odometry [START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF][START_REF] Geneva | LIPS: LiDAR-Inertial 3D Plane SLAM[END_REF]. We have thus an initialization error that stems from sensor imperfections, encoded by a vector ξ ini , and one may write

T ini = T true exp(ξ ini ), ξ ini ∼ N (0, Q ini ), (5)
which is advocated in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Long | The banana distribution is gaussian: A localization study with exponential coordinates[END_REF] to suit particularly well represent odometry errors in terms of pose. Then, ICP estimates the relative transformation between T true and T ini , that is, outputs an estimate Trel of the actual initial error T rel which writes

T rel = T -1 ini T true = exp(-ξ ini )T -1 true T true = exp(-ξ ini ) I 4 -ξ ∧ ini . (6)

C. ICP Estimate Trel of Relative Pose T rel

At step ii) above, that is, once initialization is done, see [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF], ICP provides an estimate Trel of the relative transformation T rel of (6) as a function

Trel := icp P, T -1 ini Q (7)
of the point clouds P and Q. Thus Trel appears as a function f (T rel ) ∈ SE(3) of the true relative transformation T rel , typically affected by the phenomena of wrong convergence. Moreover, sensor (scanner) noise induces small fluctuations in the point clouds that affects this estimation. This yields:

Trel = f (T rel ) exp(Gw) = f exp(-ξ ini ) exp(Gw), (8)
where w ∈ R 6K encodes errors due to sensor noise on each of the K pairs of points in the clouds, and Gw ∈ R 6 the resulting 6 degrees of freedom error made on T rel . Sensor noise stems from unknown parameters that depend upon the i) ICP initialization:

t ini = t true + ξ ini [see (5)]
ii) ICP relative pose estimate: trel = f (t truet ini , w)

iii) ICP final pose estimate: ticp = t ini + trel [see [START_REF] Iversen | Prediction of ICP Pose Uncertainties Using Monte Carlo Simulation with Synthetic Depth Images[END_REF]] calibration process and drift with temperature [START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF]. If the ICP is initialized on the true pose T true then there is no wrong convergence and the only error stems from noise, i.e., f (exp(0

Id Id Id t ini t ini t true t true t true ticp ticp ξ ini t rel = -ξ ini trel Id + J(t true -t ini ) + Gw Id -Jξ ini + Gw [see (9)] ξ icp = ticp -t true (I -J)ξ ini + Gw
) = f (I 4 ) = I 4 . Thus f (•) ∈ SE(3) is close to I 4 ,
and the model may be linearized around ξ ini = 0, w = 0 as

f exp(-ξ ini ) exp(Gw) f (I 4 -ξ ∧ ini ) exp(Gw) I 4 + (-Jξ ini + Gw) ∧ , (9)
where matrix J encodes the linear approximation of f (•).

D. ICP Final Pose Error

Let us now consider step iii) of the ICP algorithm, i.e., the final estimate

Ticp = T ini Trel (10) = T true exp(ξ ini )f exp(-ξ ini ) exp(Gw). (11)
(11) was obtained substituting ( 5) and ( 8) in [START_REF] Pfister | Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation[END_REF]. Linearizing (11) by recalling [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] and keeping only the first order in the small errors ξ ini and w yields

Ticp T true [I 4 + (ξ ini ) ∧ ] I 4 + (-Jξ ini + Gw)) ∧ T true I 4 + (ξ ini -Jξ ini + Gw)
∧ and thus in terms of uncertainty representation (4) we approximately find:

Ticp T true exp ((I 6 -J)ξ ini + Gw) . ( 12
)
Figure 2 recaps the computations for 1D translations. There are a couple of situations of interest. Let us momentarily assume sensor noise to be turned off, w = 0, for simplicity.

• If there is one global minimum, then the ICP systematically recovers the relative transformation (6) at step ii) of the algorithm, i.e. Trel = T rel and thus f (T rel ) = T rel . So f (exp(-ξ ini )) I 4ξ ∧ ini and we identify J = I 6 in this case. As a result the final estimate ( 12) is T true exp(0) = T true indeed.

• On the other hand, in the directions where we have no information, e.g. along hallways or in underconstrained environment [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], the relative transformation will not be affected in the corresponding directions meaning that (along those directions) J = 0 and the final error then has the form T -1 true Ticp = T -1 true T true exp(ξ ini ) = exp(ξ ini ), that is, the initialization error fully remains. In intermediate cases (when there are local minima) the remaining error is a fraction Jξ ini of the initialization error.

E. Corresponding ICP Error Covariance

If we represent ICP uncertainties resorting to concentrated Gaussian (4) as Ticp ∼ N L (T true , Q icp ), i.e., we posit T -1 true Ticp = exp(ξ icp ), then the covariance matrix Q icp of ξ icp describes dispersion (hence uncertainty) of the ICP error. Plugging the latter representation into (12), we have ξ icp = (I 6 -J)ξ ini + Gw. As the initialization error is assumed to have covariance matrix Q ini typically inferred though an odometry error model [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Long | The banana distribution is gaussian: A localization study with exponential coordinates[END_REF] and by denoting Q sensor the covariance of scan sensor noise w, the covariances add up owing to independence of sensor noises, and by squaring ξ icp = (I 6 -J)ξ ini + Gw we find

Q icp = (I 6 -J)Q ini (I 6 -J) T + GQ sensor G T . ( 13
)
This is our first result about ICP covariance. The first term related to the initialization uncertainty and that accounts for wrong convergence, lack of constraints in the clouds and unobservable directions, and the second one related to scan noise and that may be computed through "Censi-like" [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] formulas as we will show in section III.

F. Discussion

Albeit not obvious, J actually heavily depends on Q ini . This is an insight of the present paper: uncertainty of ICP does not exist in itself. Assume indeed there are various local minima. If Q ini is very small, then all initializations T ini fall within the attraction basin of T true and thus f (T rel ) = T rel and we identify J = I 6 . But if Q ini is large enough only a fraction of initializations T ini lead to f (T rel ) = T rel , the ones that get trapped in other local minimas do not lead to correct estimate of T rel and J = I 6 . Thus J is not the analytical Jacobian of function f (•), and may be viewed as its "statistical linearization" [START_REF] Gustafsson | Some Relations Between Extended and Unscented Kalman Filters[END_REF]. This prompts the use of an unscented transform [START_REF] Julier | A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[END_REF] to compute it, see Section III-B.

G. Maximum Likelihood Fusion of Initial and ICP Estimates

T ini and Ticp may be viewed as two estimates of T true associated with uncertainty respectively Q ini = cov(ξ ini ) and Q icp = cov(ξ icp ) where ξ icp = (I -J)ξ ini + Gw. The corresponding pose fusion problem of finding the Maximum Likelihood (ML) of a pose T true given two uncertain pose estimates was considered in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF], with the important difference that herein ξ ini and ξ icp are not independent, they are correlated, with joint matrix of initialization and ICP errors

Q := cov ξ ini ξ icp = Q ini Q ini (I -J) T (I -J)Q ini Q icp . (14)
Using linearization as previously and following first-order computations in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF], the maximum likelihood estimate of T true may be approximated as TML = T true exp(ξ ML ) with cov(ξ ML ) = Q ML , with Q ML defined through its inverse:

Q -1 ML = I I T Q ini Q ini (I -J) T (I -J)Q ini Q icp -1 I I . (15)
The latter stems from classical linear estimation theory and may be proved using the Kalman information filter: to the first order Tini and Ticp are considered as two noisy measurements of T true with joint covariance [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF], the measurement matrix is thus H := I I T and as T true is initially totally unknown the prior covariance satisfies P -1 = 0. The covariance of the Kalman estimate in the light of measurements is thus updated in information form as

P -1 ← 0 + H T Q -1 H.

III. PRACTICAL COVARIANCE COMPUTATION

This section describes our algorithm for estimating the 3D ICP uncertainty covariance (13) leveraging findings of Section II. We propose to first compute the rightmost term of ( 13) which is due to sensor noise.

A. Computation of Dispersion Owing to Sensor Noise

We now focus on the computation of GQ sensor G T . The cost function of point-to-plane ICP after initialization writes

J Trel P, T -1 ini Q = K k=1 Trel p k -qk • n k 2 ,
J Trel exp(ξ) P, T -1 ini Q K k=1 B k ξ -d k 2 , (16)
with d k a scalar being function of differences between pairs of points and point normals. Least squares formulas yield an optimal value ξ * = A -1 K k=1 B T k d k , where we let A = K k=1 B T k B k . Each d k is affected by k-th component w i of previously introduced sensor noise w, and this induces fluctuations in ξ * over various experiments. Let's postulate w i = b + ν i with ν i a white noise of variance σ 2 , and b and unknown calibration bias that is identical for all points but varies from one experiment to the next. Following least squares covariance, see [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bonnabel | On the Covariance of ICP-based Scan-matching Techniques[END_REF], we end up with:

GQ sensor G T = σ 2 A -1 + A -1 B cov (b) B T A -1 , (17) where A = K k=1 B T k B k , and B = K k=1 B T k .
We recover the covariance σ 2 A -1 of [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bonnabel | On the Covariance of ICP-based Scan-matching Techniques[END_REF] w.r.t. sensor white noise, and a new term, A -1 B cov (b) B T A -1 , that represents the covariance w.r.t. the unknown bias b, that is, correlated noise. This new additional term is paramount as A has magnitude proportional to K, hence A -1 is very small, explaining that Censi's formula (based on A -1 only) seems overoptimistic [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF]. For example A -1 has trace 0.2 cm for the registration displayed in Figure 1 whereas the covariance Algorithm 1: Computation of matrix J in ( 13)

Input: P, Q, T ini , Q ini , Ticp = T ini icp P, T -1 ini Q ; // set sigma points 1 ξ j ini = col √ 6Q ini j , j = 1, . . . , 6, ξ j ini = -col √ 6Q ini j-6 , j = 7, . . . , 12;
// propagate sigma points through (7) 2 T j ini = T ini exp ξ j ini , j = 1, . . . , 12;

Tj icp = T j ini icp P, (T j ini ) -1 Q , j =
ξj icp -ξicp ξ jT ini Q -1 ini + I 6 ; Output: J, (I 6 -J)Q ini (I 6 -J) T ;
w.r.t. the unknown bias has trace 2.6 cm. In practice b arises from sensor calibration, laser stability [START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF], observed material [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF], and incidence of beams [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF]. In the remainder we assume bias standard deviation to be approximately 5 cm as in [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF].

Note that (17) captures the effect of underconstrained situations like hallways. Indeed in unobservable directions the cost J Trel (•) is constant, yielding small eigenvalues for A and hence large covariance [START_REF] Biber | The Normal Distributions Transform: a New Approach to Laser Scan Matching[END_REF]. Derivation of B k and extraction of G in ( 17) are available with paper code.

B. Computation of Dispersion owing to ICP Initialization

Computation of (I 6 -J)Q ini (I 6 -J) T in ( 13) is of greater importance as in practice it largely dominates GQ sensor G T . We propose to compute it in a deterministic derivative-free method, in which we adapt the unscented transform [START_REF] Julier | A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[END_REF] for pose by following [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF][START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF]. The advantages of using our unscented based method rather than Monte-Carlo sampling are fourfold: 1) it is deterministic; 2) it remains computationally reasonable by adding only 12 ICP registrations; 3) it explicitly computes the cross-covariance matrix between Ticp and T ini as a by-product without extra computational operations; and 4) it scales with Q ini , i.e. our approach naturally selfadapts to the confidence we have in initialization without extra parameter tuning.

We compute the covariance as follows, see Algorithm 1:

• we consider the prior distribution T prior ∼ N L (T ini , Q ini ), which is approximated by a set of so-called sigma-points ξ j ini , see step 1); • we approximate the propagated distribution T prop = T prior icp(P, T -1 prior Q) as

T prop = N L (T ini , Q ini ) icp P, N L (T ini , Q ini ) -1 Q ∼ N L Ticp , (I 6 -J)Q ini (I 6 -J) T , (18)
after propagating each sigma-point in steps 2) and 3), where Ticp is the given ICP pose estimate. We compute the covariance and infer the matrix J as a by-product in respectively steps 4) and 6). We derive the algorithm by following [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] for pose measurement, zero-mean prior distribution, and where we set α = 1.

IV. EXPERIMENTAL RESULTS

A. Dataset Description & ICP Algorithm Setting

This section evaluates the ability of the approach to estimate ICP uncertainty on the Challenging data sets for point cloud registration algorithms [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF]. It comprises eight sequences where point clouds are taken in environments ranging from structured to unstructured, and indoor to outdoor. Each sequence contains between 31 and 45 point cloud scans along with ground-truth pose for each scan, that provides a total of 268 scans and 1020 different registrations as we align each scan with the three scans the following.

We configure the ICP as in [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] with 95% random subsampling, kd-tree for data association, and point-to-plane error metric where we keep the 70% closest point associations for rejecting outliers.

B. Compared Methods and Evaluation Metrics

This section evaluates the following methods: Qcensi : the close-form method of [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] adapted for the ICP setting defined above; Qmonte carlo : the covariance computed after sampling of 65 Monte-Carlo ICP estimates; Qicp : our proposed approach detailed in Section III. Each method assumes depth sensor white noise and bias with 5 cm standard deviation, which is the mean value found in [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF] for the Hokuyo sensor used for these experiments, and all methods know the initial uncertainty Q ini , whose magnitude 0.1 m and 10 deg corresponds to the easy scenario of [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF].

We compare the above methods using two metrics: 1) Normalized Norm Error (NNE): that evaluates the historically challenging [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bengtsson | Robot Localization Based on Scanmatching[END_REF] prediction of the covariance scale, and is computed as

NNE = 1 N N n=1 ξ n 2 2 / trace( Qn ) 1/2 , ( 19
)
where ξ n = exp -1 (T -1 true Tn ) with is the transformation error and Qn the estimated uncertainty covariance matrix, and averaged over N samples. This metric characterizes the x (m) (d) Mountain

Fig. 3. Results on data of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF] projected onto the ground plane for visualization. Ellipses represent the 95% (3σ) confidence sets for each uncertainty estimation method. (a): "true convergence situation", the errors are mainly caused by sensor noises and Censi's formula should apply. (b): however we see the Censi ellipse seems optimistic as ground truth is almost outside it whereas it falls well within our ellipse (green). (c, d): "wrong convergence", the large errors are due ICP initialization. Only our approach is consistent with ICP uncertainty in each environment and does not suffer from overoptimism.

uncertainty as only the true registration is known (the exact distribution of the point cloud is unknown). The target value is one, below one the estimation is pessimistic, whereas a value over one indicates an overoptimistic estimation.

2) Kullback-Leibler Divergence (KL div.): which is computed between a pseudo-true distribution and the estimated distribution. The pseudo-true distribution is computed after sampling 1000 ICP estimates of the evaluated registration over the initial position. As sensor noise is fixed in the point clouds, this distribution represents the uncertainty stemming from initialization errors.

C. Results

Results are averaged over 1000 initializations for each of the 1020 considered pairs of point clouds, representing a total of more than one million registrations, where the ICP is initialized with a different estimate T ini sampled from N L (T true , Q ini ). Table 1 provides average results over the eight sequences, and Figure 3 illustrates typical registrations from structured to unstructured environments. We observe:

• Qcensi is far too optimistic and unreliable for sensorfusion, as noted in [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF]. Its centimetric confidence interval makes sense only when ICP is very accurate;

• Qmonte carlo is overoptimistic when the discrepancy arising from ICP initialization remains negligible, see Figure 3 (b), for which the method predicts a confidence interval with millimetric size. This is naturally explained as the method assumes no error caused by sensor noises;

• the proposed method obtains the best results for both metrics as displayed in Table 1. It notably outperforms Qmonte carlo while deterministic hence more reliable, and computationally much cheaper. The dominant term is generally due to initial uncertainty. However in "global minimum" cases the sensor bias used for computing Qicp slightly inflates the covariance of [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], and more closely captures actual uncertainty, see Figure 3 (a,b). Besides outperforming the other methods, our method provides simple parameter tuning: we set the bias noise standard deviation as having same magnitude as sensor white noise, and the error stemming from ICP initialization does not need to be tuned when Q ini is an output of inertial, visual, or wheeled odometry system [START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF][START_REF] Geneva | LIPS: LiDAR-Inertial 3D Plane SLAM[END_REF].

Regarding computational complexity and execution time, step 2) of the algorithm requires 12 registrations which take 6 s when registrations are computed parallely, whereas the remaining part of the algorithm takes less than 0.1 s. The 65 Monte-Carlo runs are more than five times more demanding than the the proposed method.

V. COMPLEMENTARY EXPERIMENTAL RESULTS

This section provides an in-depth analysis of the method in term of trajectory consistency, robustness to high and misknown initial uncertainty, and discusses the advantages and the validity of the approach.

A. Application to Trajectory Consistency

We asses the quality of the covariance estimation in Section III over trajectories as follows. For each sequence of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], we compute the global pose estimate at scan l by compounding transformations such that Tl = T0,1 . . . Tl-1,l , whose covariance is computed with the closed-form expressions of [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF] which are valid up to 4-th order approximation. We compare three methods defined as: CELLO-3D : reproduced results of [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF], that proposes a learning based method for estimating the ICP covariance, which is trained on environments similar to the tested sequence. The results are indicative as the ICP setting of [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF] slightly differs from the setting of [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] we use; ini.+ICP : combines initialization and ICP measurements with the covariance estimate (15) without considering cross-covariance terms, i.e., applying formulas of [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF] ; proposed : based on the full proposed covariance of the maximum-likelihood estimate [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF]. We set initial errors as in Section IV and evaluate the above methods using the Mahalanobis Distance proposed in [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF] between final trajectory estimates and ground truth

Mah. dist. = N n=0 ξ T n Q-1 n ξ n dim(ξ n )N 1/2 , ( 20
)
where [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where Mountain is not considered in [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF]. Our method obtains on average the best uncertainty assessment, albeit slightly optimistic. [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where the "ellipses" (lines) represent the 95% (3σ) final confidence sets.

ξ n = exp -1 (T -
samples. The target value is one, below one the estimation is pessimistic, and above one the estimates are optimistic.

We average results over 40 different initial trajectories for each sequence, which are numerically displayed in Table 2 and illustrated in Figure 4. We observe:

• CELLO-3D is the only pessimistic method, which estimates uncertainty ranging from 3 to 10 times higher than actual uncertainty. It evidences how difficult it is to asses ICP uncertainty in practice;

• the proposed approach obtains on average the best results. It obtains similar estimates than ini.+ICP when the ICP algorithm is accurate (Gazebo and Wood). In more difficult environments, e.g. Stairs, it better incorporates initialization than ini.+ICP thanks to it accounting for measurement correlation encoded in [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF], see Figure 4. These results confirm the ability of the method to compute covariance estimates over trajectories also and the relevance of correlation terms between ICP and initial estimates.

B. Role of Initial Uncertainties in Covariance Estimation

We evaluate the influence of Q ini on the covariance estimation in challenging situations where Q ini is high, inaccurately known (the estimation of Q ini is in itself challenging), and sensor noise is inflated. For each environment of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], we evaluate the method in 9 situations where initial uncertainty is easy, medium and difficult with respectively 0.1, 0.5, 1 m and 10, 20, 50 deg standard deviation, see [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF]. In each situation we evaluate the algorithm with different magnitudes for Q ini , hence assessing its robustness to pessimistic and optimisitic parametrization. We finally add white and depth bias noises on already noisy point clouds with 5 cm standard deviation. Results are given in Table 3, and illustrated in Figure 5.

• The ICP algorithm obtains unreliable results for large initial uncertainty, see Figure 5, whereas it obtains centimetric errors for low levels of initial uncertainty.

As anticipated in Section II-B, significant ICP errors are caused by inaccurate initialization; Fig. 5. ICP results (dots) and 95% (3σ) confidence sets (lines) following our method for three of initial uncertainty. We see the latter highly influences the ICP registrations and ICP covariance estimations accordingly.

• ICP final outputs are agnostic to initialization when a global minimum exists, see e.g. the Gazebo and Wood environments in Table 3 for levels of Q ini corresponding to easy and medium scenario. The method obtains correct estimates where the sensor noise terms numerically dominate the estimated covariances, and J ≈ I, see Section II-D;

• another environments contain local minima, e.g. Hauptgebaude. Then the algorithm outputs reflect the pessimistic or optimistic belief about the initial uncertainty. We recommend in these situations to set Q ini sufficiently high to favour conservatism;

• our method is able to detect inaccurate ICP registrations by providing very high covariance estimates, although it cannot accurately describe non Gaussian distributions, see Figure 5 (a,b).

C. Discussion about the Proposed Approach

We finally examine the pros, cons, and fundamental assumptions of the method. The main advantages are: it being anchored in a mathematical theory, its efficiency to assess uncertainty for acceptable levels of initial uncertainties, its simplicity, while being computationally reasonable, see Algorithm 1. The cross-covariance term in (15) may be fruitful for increasing robustness of back-end systems, e.g. pose-graph [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF], as it correlates two previously supposed independent measurements. Comparisons between diagonal terms in ( 15) finally provides a way for trading-off between initial odometry guesses and ICP estimates, see [START_REF] Ovchinnikov | Windowed Multiscan Optimization Using Weighted Least Squares for Improving Localization Accuracy of Mobile Robots[END_REF].

The Gaussian error assumption of the ICP estimates is the core hypothesis of the method. We required this assumption to obtain a tractable method able to provide a covariance for a state estimator, e.g. a Kalman filter. However, if one pursues a more accurate estimation of the ICP distribution, we suggest massive sampling methods as an expansive alternative, although our method largely proves sufficient to detect problematic situations.

The method finally requires the covariance of the initial uncertainty as an input. If the provided initialization confidence is inexact, the method outputs may reflect the initial optimism or pessimism in general situations. Nonetheless an insight of the present paper, see Section II and e.g. Figure 5, is that ICP errors intrinsically depend on the initial accuracy so that a coarse idea of initial uncertainty is essential, whatever the method one desires to use.

Finally, [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF][START_REF] Long | The banana distribution is gaussian: A localization study with exponential coordinates[END_REF] show that concentrated Gaussian distribution (4) faithfully describe robot odometry models, such that methods like the Kalman filter are able to provide an accurate (concentrated) Gaussian approximation to the true initial uncertainty for which our approach has been designed.

VI. CONCLUSION

This paper presents a novel method for real time estimation of 3D uncertainty covariance matrix of the ICP algorithm. The method relies on a careful study of the influence of both sensor noises and algorithm initialization on the ICP estimates, that we leverage in a deterministic scheme which remains very simple in terms of parameter tuning. The core of our approach is versatile as one can apply it to various choices of error metrics. However with point-to-point ICP the closed form part of the covariance is not valid, see [START_REF] Bonnabel | On the Covariance of ICP-based Scan-matching Techniques[END_REF]. The approach is successfully validated on individual pairs of point clouds and over trajectories on challenging real datasets, where it obtains consistent results. Future work will address the benefit of the method for preventing ICP failures, particularly its coupling with learning-based methods, and for fusing odometry, ICP and GNSS in Kalman filtering and optimization-based schemes.

Fig. 2 .

 2 Fig. 2. Schematic illustration of the procedure, error definitions and linearizations in the case of 1D translation t ∈ R (Id = 0).

  where the qk 's denote the points of T -1 ini Q and K is the number of pairs of matched points. Linearizing on SE(3), we may linearize the cost J Trel exp(ξ) P, T -1 ini Q = K k=1 Trel exp (ξ) p kqk • n k 2 w.r.t. estimate Trel at Trel = T rel as

Fig. 4 .

 4 Fig.[START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF]. Results projected onto the ground plane for visualization in the Stairs sequence of[START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where the "ellipses" (lines) represent the 95% (3σ) final confidence sets.

  same as (b)

Table 1 .

 1 Results of ICP uncertainty estimation in term of Normalized Norm Error (NNE) and Kullbach-Leibler divergence (KL div.) divided into translation and rotation parts. As the ICP error distributions are not Gaussian[START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF], we provide robust statistics (starred, *) by removing both the more and less accurate quantiles of each registration. The proposed method outperforms the two others.

	metric	NNE trans.	rot.	KL div. trans. rot.	NNE* trans.	rot.	KL div.* trans. rot.
	Qcensi	10 3	10 3	10 4	10 5	38	10 2	10 3	10 5
	Qmonte carlo proposed	10 3 4.2	10 2 34	10 4 10 2	10 4 10 2	22 0.8	20 3.8	10 3 31	10 3 98

Table 2 .

 2 1 true Tn ) is the transformation error and Qn the estimated covariance matrix, averaged over N Trajectory consistency results in term of Mahalanobis distance (bold indicates best performance) split into translation and rotation parts for the sequences of

	sequence	Apartment	Hauptgebaude	Stairs	Mountain	Gazebo summer	Gazebo winter	Wood summer	Wood winter
	Mah. dist.	trans.	rot.	trans.	rot.	trans.	rot.	trans.	rot.	trans.	rot.	trans.	rot.	trans.	rot.	trans.	rot.
	CELLO-3D [7] ini.+ICP proposed	0.2 3.5 2.3	0.1 15 9.8	0.3 1.9 1.8	0.2 3.2 2.9	0.1 1.1 1.1	0.2 4.2 4.2	-1.5 1.2	-1.2 1.2	0.2 1.1 1.0	0.2 2.1 2.3	0.1 1.9 1.8	0.2 3.7 3.7	0.1 1.5 1.5	0.3 4.6 4.7	0.1 1.2 1.2	0.3 4.8 4.2

Table 3 .

 3 NNE, see[START_REF] Prakhya | A Closed-form Estimate of 3D ICP Covariance[END_REF], for different levels of true and supposed initial uncertainty. Difficult initial uncertainty leads to highly erroneous ICP outputs that the proposed method detects if correctly parametrized.

	Q ini (true)		easy			medium			difficult
	Qini (algo)	easy	med.	diff.	easy	med.	diff.	easy	med.	diff.
	Appart. Haupt. Stairs Montain Gazebo Wood	1.6 1.8 0.7 2.1 1.1 2.1	1.5 0.3 0.2 1.1 1.0 2.1	10 -6 10 -3 10 -3 10 -3 10 -5 10 -3	1.8 3.6 1.9 3.3 1.3 2.2	1.7 0.7 0.8 1.8 1.2 2.2	10 -6 10 -3 10 -3 10 -3 10 -5 10 -3	10 5 10 3 10 3 10 3 10 4 10 3	10 5 10 3 10 3 10 3 10 4 10 3	0.6 0.3 0.7 4.0 0.8 0.3

Axel Barrau is with Safran Tech, Groupe Safran, Rue des Jeunes Bois-Châteaufort, 78772, Magny Les Hameaux Cedex, France