CELLO-3D: Estimating the Covariance of ICP in the Real World

David Landry Francois Pomerleau Philippe Giguere

Abstract— The fusion of Iterative Closest Point (ICP) reg-istrations in existing state estimation frameworks relies on anaccurate estimation of their uncertainty. In this paper, we studythe estimation of this uncertainty in the form of a covariance.First, we scrutinize the limitations of existing closed-formcovariance estimation algorithms over 3D datasets. Then, weset out to estimate the covariance of ICP registrations througha data-driven approach, with over 5 100 000 registrations on1020 pairs from real 3D point clouds. We assess our solutionupon a wide spectrum of environments, ranging from structuredto unstructured and indoor to outdoor. The capacity of ouralgorithm to predict covariances is accurately assessed, aswell as the usefulness of these estimations for uncertaintyestimation over trajectories. The proposed method estimatescovariances better than existing closed-form solutions, andmakes predictions that are consistent with observed trajectories.

I. INTRODUCTION

The ICP algorithm [1], [2] is ubiquitous in mobile roboticsfor the tasks of localization and mapping. It estimates therigid transformation between the reference frames of twopoint clouds, by iteratively pairing closest points in bothpoint clouds and minimizing a distance between those pairs.This is equivalent to optimizing an objective function thatmaps rigid transformations to a scalar optimization scorefor a pair of point clouds. There is an abundance of ICPvariants [3], each of which yields slightly different trans-formations due to their different objective functions. Onenotable variation is the choice of error metric between eachpair of points, where common choices of metric are point-to-point [1] and point-to-plane [2]. The registration process issubject to a number of sources of uncertainty and error, be-cause of a bad adequation between the objective function andthe desired result. Chief among them is the presence of localminima in the objective function. Other causes of uncertaintycomprise noise from the range sensor, and underconstrainedenvironments such as featureless hallways [4].

The fusion of ICP measurements in existing state esti-mation frameworks (e.g. SLAM) relies on an appropriateestimation of the uncertainty of ICP, expressed as a co-variance [5]. In this context, ICP is modeled as a functionof input point clouds and an initial estimate which yieldsa registration transformation that is normally distributed.Optimistic covariance estimates can lead to inconsistency andnavigation failures, whereas pessimistic ones inhibit efficientstate estimation. Figure 1 illustrates the process of estimatingthe covariance of a registration. The registration process

The authors are with the Department of Computer Scienceand Software Engineering, Universite Laval, Quebec City,Qc, G1V 0A6, Canada (emails davidlandry93@gmail.com,{philippe.giguere,francois.pomerleau}@ift.ulaval.ca).

Sampled uncertainty

Covariance

Fig. 1: A reading point cloud (red) is being registered against a referencepoint cloud (blue). This paper studies the estimation of covariances like thegreen ellipsoid to allow integration of ICP in state estimation toolchains.The result of ICP was sampled, and the black balls indicate the density ofregistration transformations at a particular location. There are three largerclusters which correspond to local minimas in the objective function causedby the regularly spaced pillars. We project the translation part of a covariancein R3 for illustration purposes, but in general the covariances of a 6 degreesof freedom phenomenon is studied here.

shown takes place in a hallway, and is consequently looselyconstrained in one axis. In this figure, optimistic covarianceestimation frameworks might miss this underconstrainednessand encompass only the central samples.

This paper focuses on the problem of estimating thecovariance of ICP in such real 3D environments. To thiseffect, we first provide an experimental explanation as towhy current covariance estimation algorithms may performpoorly in that context. Furthermore, we present CELLO-3D,a data-driven approach to estimating the uncertainty of 3DICP that works with any error metric.

II. RELATED WORKS

There are many approaches to estimating the covarianceof the ICP algorithm, each of which must balance quality ofprediction and computation time. On one end of the spec-trum, Monte-Carlo (also called brute force) algorithms suchas [6], [7] provide an accurate estimate ICP’s covariance.They consist in sampling a large number of ICP registrationtranssforms, and using the covariance of the sampled resultsas the covariance estimation. If a model of the environmentis available, brute-force algorithms can take sensor noise intoaccount by simulating many scans of the environment givensome noise model. However, Monte-Carlo algorithms cannotbe used online due to their high computational cost. Thislimits their practicality in mobile robotics applications.

Another important category of covariance estimation al-gorithms rely on the objective function’s Hessian [4], [6],[8]–[11]. These closed-form methods are motivated by theneed for a covariance estimation that can be used online.

arX

iv:1

810.

0147

0v1

[cs

.RO

] 2

Oct

201

8

−50 0 50x (mm)

−50

0

50y

(mm

)−2 0 2

x (mm)

−2

0

2

−0.250.00 0.25x (mm)

−0.25

0.00

0.25

−0.005 0.000 0.005x (mm)

−0.005

0.000

0.005

179.62 445.08 178.45 179.99 178.44 179.08 178.90 178.98

Fig. 2: The objective function of point-to-plane ICP around the ground truth when registering a simple cube. The black dots are sampled registrationtransformations, with their associated translation projected on the xy plane. The green circle is a 3σ covariance ellipse of the distribution of registrationtransformations. The white dashed circle is a similar representation, but of Censi’s covariance estimate. Rightmost frame: the transitions from one plateauto another are explained by point reassociations. At a scale smaller than the scale of the covariance of the registration transformations, the error landscapeis dominated by point reassociations.

Their underlying assumption is that the objective functionJpT q used in ICP can be linearized around the point ofconvergence. This allows the use of linear regression theoryto derive a covariance from the Hessian of J . If J isanalytically differentiable, then the Hessian can be computeddirectly [6], [10]. Otherwise, it can also be approximatednumerically by sampling [8], [9]. This approach accounts forerrors that are due to the environment structure, but not forerrors due to sensor noise. However, modelling the effect ofsensor noise on the objective function J proves to be crucialfor covariance estimation algorithm. Censi [4] addresses thisby using the implicit function theorem. His model of thecovariance contains the Hessian of J , but also the effect ofthe sensor noise on it. It is successfully evaluated on 2Ddatasets. The equations for the 3D case are derived in [11],but Censi’s algorithm is considered to be largely optimisticin that situation [12]. More elaborate noise models alleviatethis difficulty, but only for specific sensors [13].

Closed-form approaches have the shortcoming of nottaking point reassociation into account. Therefore, they mustassume that 1) ICP converged to a loosely defined region ofattraction of the “true” solution [4], and 2) the reassociationof points that occur in that region have a negligible influenceon the objective function. Bonnabel et al. [14] show that forpoint-to-point ICP variants, the second assumption is broken.However, they present a proof that Censi’s method is accurateusing the point-to-plane ICP variant in a noiseless context.They do so by demonstrating that changes in J due to pointreassociations are small enough for the covariance estimationmethods to remain valid under certain conditions. In spite ofthis, Mendes et al. [12] indicates that this covariance is stilloptimistic in a noisy experimental context.

A data-driven alternative to covariance estimation emergedfrom the Covariance Estimation and Learning through Likeli-hood Optimization (CELLO) framework [15]. It is a generalcovariance estimation strategy that projects point cloudsin a descriptor space, then estimate the covariance withinthis space. It uses a machine learning algorithm that firstestimates a distance metric between the predictors, andthen uses this metric to weigh the learning examples dur-ing online inference. This procedure can be done withground-truth data [15], but also without it [16] exploiting

expectation-maximization. In the presence of ground-truthdata, expectation-maximization should be avoided to sim-plify the machine learning process. The general strategyof CELLO was successfully applied to the estimation ofthe reliability of visual features for visual-inertial naviga-tion [17], [18]. Peretroukhin et al. [18] used the predictionspace to generate a noise model for visual landmarks, whichin turn was used to predict the ego-motion of the sensor.For an application to 3D ICP, these data-driven approachesare challenging in that extracting relevant features from 3Dpoint clouds is still an open problem. Hand-crafted featuredesigners must tread carefully between the expressivenessand the generality of the descriptor for this approach to beviable. Consequently, this method was never assessed in a3D ICP context to the best of our knowledge.

III. SHORTCOMINGS OF CLOSED-FORM COVARIANCEESTIMATION ALGORITHM

As discussed earlier, Bonnabel et al. [14] point out thatclosed-form covariance estimation methods are potentiallyill-founded if point reassociations occur at a scale that issmaller than that of the covariance to be estimated. Our ownanalysis on 3D simulated data shows that it is likely that pointreassociations happen at a scale this small. For example,Figure 2 shows the objective function JpT q observed whenregistering a pair of 1ˆ 1ˆ 1 m cube shaped point clouds.The points lie on the surface of the cube and have a σ “ 0.01m noise applied on them on every axis. At a larger scale, thisobjective function JpT q corresponds to our intuition, with aseemingly-smooth slope towards large global minimum. Ata smaller scale, however, it is composed of a large numberof “plateaus”, each of them corresponding to one fixedassociation of points between the reading and the reference.This litters the objective function with local minima to whichICP is sensitive. In turn, a larger covariance of ICP resultsis observed.

There is a mathematical explanation for the optimismof Censi’s algorithm for the point-to-point variant of ICPin Bonnabel et al. [14]. We do not know of such a proof thepoint-to-plane case. Furthermore, the results in Bonnabel etal. [14] were encouraging about the validity of Censi’s algo-rithm in the point-to-plane case. They show that the changein the objective function provoked by point reassociations

0.0 0.5 1.0 1.5 2.0Sensor noise standard deviation (cm)

0

1

2Tr

ace

ofco

vari

ance

mat

rix ×10−5

Sampled CovarianceCensi Cov. Estimate

Fig. 3: Trace of covariance computations of ICP against sensor noise.Censi’s covariance estimate increases slowly as the sensor noise modelgrows. The sampled covariance of ICP increases dramatically with sensornoise, even on simulated datasets. This is attributed to the point reassociationprovoked by sensor noise.

is bounded under certain conditions, proving the correctnessof Censi’s estimate. On the contrary, our own experimentsshow that caution is required even in the point-to-planecase. Indeed, Figure 3 shows that sensor noise significantlyimpacts the empirically sampled covariance of ICP as itgrows. On the other hand, Censi’s covariance estimate growsslowly as the estimate of the sensor noise grows. To useCensi’s estimate in that experimental context, we would needto inflate our estimation of the sensor noise to values that arebeyond a meaningful range.

Consequently, we argue that a viable covariance estimationalgorithm for ICP should take into account the effect of noise.More precisely, it should model both the direct effect of thesensor noise on the objective function of ICP, and also thepoint reassociations that it provokes around ground truth.Monte-Carlo based approaches circumvent those difficultiesby incorporating the effect of noise directly. The complexitywe observe in the 3D registration process motivates ourshift from analytical to data-driven solutions. Our generalapproach is to implement the CELLO framework for 3D ICPand work towards learning covariance models from trainingdata generated through a sampling process. We aim at gettingthe best of both world: the accuracy of brute-force methodsand the rapid inference of machine learning approaches.

IV. 3D COVARIANCE ESTIMATION OF ICP

Casting CELLO onto a 3D registration problem requiresa primer on notations. A rigid transformation a

bT P SEp3qallows to express a point cloud bP P R3ˆn in the coordinatesystem b in a second coordinate system a. Using the Liealgebra, we can express the matrix T as a vector ξ P sep3qusing logpT q and reverse the process using exppξq. Thevector ξ is split into a translation u P R3 and an angle-axisrotation ω P R3. This allows us us to express the uncertaintyon a rigid transformation as a covariance matrix Y P R6ˆ6,such that

ξ “

„

uω

and Y “

„

Yuu Yuω

Yωu Yωω

. (1)

Generally speaking, we need to rely on prior information,for which we use the notation |p¨q, to produce an estimatexp¨q of a true quantity p¨q. For example, having access to areference point cloud Q P R3ˆm, it is possible to produce atransformation estimate pT that reduces the alignment error

Translations

Rotations

\widecheck{\mathcal{\bm{T}}}_\omega

<latexit sha1_base64="m6O5z0zUZjaDecHA9a2cYP6vhTU=">AAANqHicnVfdbts2FFb323nN1m6XuyEaGOgCz5Cc2kkwZGiQNt0wtE2btulgeQFF0TIXitRIqkkm6A12sdvtzfY2O6TkxnKsFJgAW/TRd75zeP5ERxln2vj+vzc++PCjjz/59OZnnc9vrX3x5e07X73WMleEviKSS/UmwppyJugrwwynbzJFcRpxehyd7tvnx2+p0kyKl+Yio5MUJ4JNGcEGRK/DKC2elye31/3+aGs0HPrI7/vusoud0daOj4Jasu7V1+HJnVt/hrEkeUqFIRxrPQ78zEwKrAwjnJadMNc0w+QUJ3QMS4FTqieFc7dEXZDEaCoVfIRBTrqoUQhsIhaVnQZNppgwUvALq90Tcga7UbDtUyDGRElxkYJGt6ESTw0976FOF1Y4NzOpdosDhUVIClLOmEaHMgUSivMesnICIogZ1j30Am4iRkeMJmewrbJmcSHeLQ4lOIP2ucxj9IImkCflAor2eCIVM7NUIyaAJJIQEV32Ol23TedvD0XgoKLT3Qy87CErdE93sYKtIcIMXfydK375s9OdFG7roF92O40YFRjM2pjqclmcYjO7ItQXadQUWhiOziFHSFAa12kKz1hMyYyS0wY4Si0ukjyuEmmhlgDlmokEQW014DqfTtl5056hQtuSgLwpKugZkWkKYQ9FTjjYPADaIrSczNjkpjk3TMkzVOtXJq3wOys1GMpeNzdkHyJ42LQbsQRBznIDO8iUjHNCNYqoMVQhDSDrP1YyhwqwndXkjKQ8BVNLQQZJzrG6KK/ah+ytkF5xSkkDRSSSppRLkbiNXTXXlEB1ck1wRkt03bVUMUph67KLPc9TYYdE8aQcB5PihyIk0OAUGi8JHdDWrYb2mJVpsR6UzQ5d8L/TRZBqpKG7EIQXnE+R7SbMOaQphKY0Oo80USwzDQrQ4uZ8QJeYMU8yTXMYOTKmtujwvM0YWQZWcucDkWIKibZFMm+ZeeV0wphOQzLDGewPRoOEZ3ZKFUeU2EYuK4Cufl0DgG28D/POqUXI3qWn3epCj54+RId7+z/vPX505MK1/+zpwU+P+6gGQLd3Gxf06BmqW0aj7vJlQ+DeALrOrwMW4QW1aXAphgxbwFgl0aTw+9tBAG+Bnt8fDob3d3xY+NujwdawdMlukEQ8pwitIvE3dwbByOpu3R8GlmS0cz8ItleQwDxDq0mGOyN/a9uSDCyLXQQgqklgXCzSMJKVY39ShJgUjNAyPLc9TJeM8SwtC1d7xhScRZmd4TBbYLCpFhVMzp1zlhgMvwN1lreBY1v379jngtWsUAIUph3IFxTmogUbXfTUdZQUSwnMTLVdmmazwm2jxVIGb4IlpG6D8kUkvPvawphxvYxs4/w9x7HCC+hKAAPxWgW9SmPBRqd7kAtSx+UhhQOIok/gHfEMWhxDh21AvlSSMlHCeEvCnl2tBNq8Sg0o4t6YqxHa8djvTgskxgZPGYdBUhYL6zbfXMmVRXVDbZxwpMCuROJWjMwNZ9bofNGCg8EH79iiul2HcSGDL5ayP2gbEk46AmLmbjBZU1xPunawOzlYPxu/WjRsL7smhjTDSRbmJ7x/3TEELGl0zx4Gvm1WTZpQUbWpfUgwb/RqEzpVMHwvwVHUhg2ZcNjxYGK71B5TNsa/XmoNynJidYsF7WOY50fulLOCaGOJCRiuECxoTZVMX8rxJuh8j1Y5sFmWJ//HnYp44yozMJ68x6sifEvJy5miEEKrz+nUjOGgBydgUR8l4GC9HqAwROsD972JQgqK1UMUKpbMzAStJjd5xuvkgFe5onZ3lRUUwqBJOEWW3JHArZKU9v/L/E8Kal+8HvQDvx8899cf3K3/ydz0vvHueve8wNvyHng/eofeK494v3l/eX97/6xtrB2uHa/9UkE/uFHrfO01rrXoP0UZ850=</latexit>

<latexit sha1_base64="BldMbCYB/r1kbVdyYqCp63W1BVc=">AAANqHicnVfdbts2FFa7v85rtna73A3RwEAXeIHkxI6DIUODtOmGoa3XtEkHywsoipa5UKRGUk0yQW+wi91ub7a32SFlN5ZjpcAE2KKPvvOdw/MnOso408b3/711+4MPP/r4kzuftj67u/b5F/fuf3msZa4IfU0kl+pNhDXlTNDXhhlO32SK4jTi9CQ6O7DPT95SpZkUr8xlRscpTgSbMIINiI7DKC2G5em9dX8z6G71ewPkb/r9YDvwYdEfbA+CXRRs+u5a92bX8PT+3T/DWJI8pcIQjrUeBX5mxgVWhhFOy1aYa5phcoYTOoKlwCnV48K5W6I2SGI0kQo+wiAnXdQoBDYRi8pWjSZTTBgp+KXV7gg5hd0o2PYZEGOipLhMQaNdU4knhl50UKsNK5ybqVR7xaHCIiQFKadMo6FMgYTivIOsnIAIYoZ1B72Em4jREaPJOWyrnLG4EO8VQwnOoAMu8xi9pAnkSbmAon2eSMXMNNWICSCJJEREl51W223T+dtBETio6GQvAy87yArd0z2sYGuIMEMXf+eKX/1stceF2zrol+1WLUYFBrM2prpcFqfYTK8J9WUa1YUWhqMLyBESlMazNIXnLKZkSslZDRylFhdJHleJtFBLgHLNRIKgtmpwnU8m7KJuz1ChbUlA3hQV9JzINIWwhyInHGweAm0RWk5mbHLTnBum5Dma6VcmrfBbKzUYyl7XN2QfInhYtxuxBEHOcgM7yJSMc0I1iqgxVCENIOs/VjKHCrCdVeeMpDwDU0tBBknOsbosr9uH7K2QXnNKSQNFJJK6lEuRuI1dN1eXQHVyTXBGS3TTtVQxSmHrsos9z1Nhh0TxrBwF4+L7IiTQ4BQaLwkd0NathvaYlmmxHpT1Dl3wv9VGkGqkobsQhBecT5HtJsw5pCmEpjQ6jzRRLDM1CtDi5qJLl5gxTzJNcxg5Mqa26PC8zRhZBlZy5wORYgKJtkUyb5l55bTCmE5CMsUZ7A9Gg4RndkoVR5TYRi4rgK5+3QCAbbwP886pRcj+laft6kJPnj9Gw/2Dn/afPjly4Tp48fzwx6ebaAaAbm/XLujRczRrGY3ay5cNgXsD6Fl+HbAIL6lNg0sxZNgCRiqJxoW/OQiCnX6v42/2ur3tXR8W/qDf3emVLtk1kojnFKFVJP7WbjfoW92d7V5gSfq720EwWEEC8wytJunt9v2dgSXpWha7CEA0I4FxsUjDSFaO/HERYlIwQsvwwvYwXTLGs7QsXO0ZU3AWZXaGw2yBwaYaVDC5cM5ZYjD8DtRa3gaObd2/Y58LVrNCCVCYdiBfUJiLFmy00XPXUVIsJTAz1XZpmk0Lt40GSxm8CZaQugnKF5Hw7msKY8b1MrKJ8/ccxwovoCsBDMQbFfQqjQUbrfZhLsgsLo8pHEAUfQbviBfQ4hg6bAPypZKUiRLGWxJ27Gol0OZVakAR98ZcjdCOx363GiAxNnjCOAySslhYN/nmSq4sqhtq4oQjBXYlEjdiZG44s0bniwYcDD54xxbV7SaMCxl8sZT9QZuQcNIREDN3g8ma4tmkawa7k4P1s/arQcP2smtiSDOcZGF+wvvXHUPAkkYP7WHgm3rVpAkVVZvahwTzWq/WoRMFw/cKHEVN2JAJhx11x7ZL7TFlY/TrlVa3LMdWt1jQPoF5fuROOSuINpaYgOEawYLWRMn0lRxtgc53aJUDW2V5+n/cqYg3rjMD4+l7vCrCt5S8mioKIbT6nE7MCA56cAIWs6MEHKzXAxSGaL3rvrdQSEGxeohCxZKpGaPV5CbP+Cw54FWuqN1dZQWFMGgSTpEldyRwqySl/f8y/5OCmhfH3c0A/ub87K8/ejD7J3PH+9p74D30Am/He+T94A291x7xfvP+8v72/lnbWBuunaz9UkFv35rpfOXVrrXoP+K186k=</latexit>

<latexit sha1_base64="sa9JDmUlE0XuilTf19SUAmlpolU=">AAANuHicnVfrbts2FFa7W+c1W7vt3/4QDQx0gRdITnPbkKFB2nTD0DZrkraA5WUURctEKFIjqSYpoTfYQ+zv9kZ7mx1SSmP5kgITYIs++s53Ds9NdFJwpk0Y/nvj5gcffvTxJ7c+7Xx2e+nzL+7c/fKllqUi9JhILtXrBGvKmaDHhhlOXxeK4jzh9FVyuueev3pDlWZSHJmLgg5znAk2YgQbEJ3c+To+YyklY0pOrY2T3B5VVXVyZzlcDf2FZhdRs1gOmuvg5O7tP+NUkjKnwhCOtR5EYWGGFivDCKdVJy41LTA5xRkdwFLgnOqh9e5XqAuSFI2kgo8wyEsnNazAJmFJ1WnRFIoJIwW/cNo9IcewOwVhOAViTJQUFzlodFsq6cjQ8x7qdGGFSzOWasfuKyxiYkk1ZhodyBxIKC57yMkJiCCGWPfQC7iJFB0ymp3BtqqGxYd8xx5IcAbtcVmm6AXNIG/KBxjt8kwqZsa5RkwASSIhIrrqdbp+m97fHkrAQUVHOwV42UNO6J/uYAVbQ4QZOvm7VPzqZ6c7tH7roF91O60YWQxmXUx1NS3OsRnPCPVFnrSFDoaTc8gREpSmTZquaqYFTnKHSyRP60Q6qCNApWYiQ1BdLbguRyN23rZnqNCuJCBvigp6RmSeQ9hjURIONveB1saOkxmX3Lzkhil5hhr92qQTfuekBkMb6PaG3EMED9t2E5YhyFlpYAeFkmlJqEYJNYYqpAHk/MdKllABrtPanImUp2BqKsggKTlWF9WsfcjeHOmMU0oaKCKRtaVcisxvbNZcWwLVyTXBBa3QdddUxSiFncs+9rzMhRsa9mk1iIb2RxsTaHAKjZfFHujqVkN7jKvcLkdVu0Mn/O90EaQaaeguBOEF53PkuglzDmmKoSmNLhNNFCtMiwK0uDnv0ylmzLNC0xJGjkypKzp82WaMTANrufeBSDGCRLsiuWyZy8rpxCkdxWSMC9gfjAYJz9yUsoeUuEauaoCuf10DgG28D/POqUnI7pWn3fpCj589Qge7e7/sPnl86MO19/zZ/s9PVlEDgG7vti7o0TPUtIxG3enLhcC/EXSTXw+08QV1afAphgw7wEBlydCGq1tRtLmx3gtX1/vrD7ZDWIRbG/3N9conu0WS8JIiNI8kXNvuRxtOd/PBeuRINrYfRNHWHBKYZ2g+yfr2Rri55Uj6jsUtIhA1JDAuJmkYKapBOLQxJpYRWsXnrofplDFe5JX1tWeM5Swp3AyH2QKDTS1QweTcO+eIwfA7UGd6Gzh1df+O/VIwnxVKgMK0A/mEwqVowkYXPfMdJcVUAgtTb5fmxdj6bSywVMCbYAqpF0H5JBLefYvCWHA9jVzE+UeJU4Un0LUABuK1CnqexoSNTne/FKSJyyMKBxBFn8I74jm0OIYOW4F8qSxnooLxlsU9t5oLdHmVGlDEvzHnI7Tncd+dBZAUGzxiHAZJZSfWi3zzJVfZ+oYWccKRAvsSSRdiZGk4c0YvFwtwMPjgHWvr23UYHzL4Yjl7Sxch4aQjIGb+BpM1x82kWwz2JwfnZ+vXAg3Xy76JIc1wsoX5Ce9ffwwBSxrdd4eBb9tVk2dU1G3qHhLMW73aho4UDN8rcJIswsZMeOygP3Rd6o4pK4PfrrT6VTV0unZC+xXM80N/yplDtDLFBAwzBBNaIyXzIzlYA50f0DwH1uDc/n/cqYlXZpmB8eQ9Xtn4DSVHY0UhhE6f05EZwEEPTsCiOUrAwXo5QnGMlvv+ew3FFBTrhyhWLBubIZpPbsqCN8kBr0pF3e5qKyiGQZNxihy5J4FbLfH/X6Lpfyuzi5f91ShcjX4Nlx/ea/7J3Aq+Ce4F94Mo2AweBj8FB8FxQIK3wV/B38E/S98v/b6ULbEaevNGo/NV0LqW1H/+XvoU</latexit>

\mathrm{icp}(\bm{P}, \bm{Q}, \widecheck{\bm{T}})

<latexit sha1_base64="7g0fAT0w3cgVTw99G5UNkSz6pOU=">AAAN1XicnVdbb9s2FFa7W+c1W7o97oWoYaANvMBymhuGDA3SphuGtmmTNgUsL6AoWiZCkRpJNckEvg17GrDH/Zq9br9h/2aHlNJYvqTABMSiDr/z8fDcyMQ5Z9r0ev/euPnBhx99/MmtT1uf3V76/IvlO1++1rJQhL4ikkv1JsaaciboK8MMp29yRXEWc3ocn+65+eO3VGkmxZG5yOkww6lgI0awAdHJchhl2IxVVjKS23tRnJUHtovc+4V7n7GEkjElp6UTHVl7H50st3urPf+g2UFYD9pB/Ryc3Ln9e5RIUmRUGMKx1oOwl5thiZVhhFPbigpNc0xOcUoHMBQ4o3pY+r1Z1AFJgkZSwZ8wyEsnNUqBTcxi22rQ5IoJIwW/cNpdIcewdQU+OgViTJQUFxlodBoqycjQ8y5qdWCECzOWaqfcV1hEpCR2zDQ6kBmQUFx0kZMTEIGDse6il/ASCTpkND2DbdmaxcdjpzyQYAza47JI0EuaQlCV9z7a5alUzIwzjZgAkliCR7Tttjp+m97eLorBQEVHOzlY2UVO6Gd3sIKtIcIMnfwuFL/6bHWGpd866NtOq+GjEsOyzqfaTotdSswI9UUWN4UOhuNziBESlCZ1mK5ypgGOM4eLJU+qQDqoI0CFZiJ1GdeA62I0YufN9QwV2qUExE1RQc+IzDJweyQKwmHNfaAtfToz44KbFdwwJc9QrV8t6YTfOKnBUCO6uSE3iWCyuW7MUgQxKwzsIFcyKQjVKKbGUIU0gJz9WMkCMsCVYZMzlvIUlppyMkgKjtWFnV0fojdHOmOUkgaSSKRNKZci9RubXa4pgezkmuCcWnTdM5UxSmFnsvc9LzLhOkr51A7CYfldGREocAqFl0Ye6PJWQ3mMbVa2Q9us0An7Wx0EoUYaqguBe8H4DLlqwpxDmCIoSqOLWBPFctOgAC1uzvt0ihnzNNe0gJYjE+qSDl+WGSPTwErubSBSjCDQLkkuS+Yyc1pRQkcRGeMc9getQcKc61LlISWukG0F0NXXNQDYxvsw74yahOxeWdqpHvT42SN0sLv34+6Tx4feXXvPn+3/8GQV1QCo9k7jgRo9Q3XJaNSZfpwL/HGh6/h6YBldUBcGH2KIsAMMVBoPy97qVhhubqx3e6vr/fUH2z0Y9LY2+pvr1ge7QRLzgiI0j6S3tt0PN5zu5oP10JFsbD8Iw605JNDP0HyS9e2N3uaWI+k7FjcIQVSTQLuYpHEH3aA3LCNM4NCjNjp3NUynFuN5Zkufe8aUnMW56+HQW6CxqQUqmJx74xwxLPwO1JreBk5c3r9jvxTMZ4UUoNDtQD6hcCmaWKODnvmKkmIqgLmptkuzfFz6bSxYKYeTYAqpF0H5JBLOvkVuzLmeRi7i/LnAicIT6EoADfFaBT1PY2KNVme/EKT2yyMKFxBFn8IZ8RxKHEOFrUC8VJoxYaG9pVHXjeYCXVylBhTxJ+Z8hPY87re1AJJgg0eMQyOx5cR4kW0+5WxZvdAiTrhSYJ8iyUKMLAxnbtHLwQIcND44Y8vqdR3Guwx+WMZ+oYuQcNMR4DP/gs6a4brTLQb7m4Ozs/G1QMPVsi9iCDNce6F/wvnrryGwkkb33GXgfjNrspSKqkzdJMG8UatN6EhB870Cx/EibMSExw76Q1el7pqyMvjpSqtv7dDplhPax9DPD/0tZw7RyhQTMMwQTGiNlMyO5GANdL5F8wxYs/bk/5hTEa/MMgPjyXusKqO3lByNFQUXOn1OR2YAFz24AYv6KgEX63aIogi1+/53DUUUFKtJFCmWjs0QzSc3Rc7r4IBVhaJud9UqKIJGk3KKHLkngVclAT8st8Pp/1ZmB6/7q2FvNXzRaz+8W/8ncyv4Orgb3AvCYDN4GHwfHASvAhL8GfwV/B38s3S8ZJd+Xfqtgt68Uet8FTSepT/+A2FgBVY=</latexit>

<latexit sha1_base64="74W8/xXC4SDzzihey31f3YXAPCo=">AAANvniclVfrbts2FFa7W+c1W7v92/4QDQx0gRdITnPDkKFB2nTD0DZrmraA5QUURctEKFIjqSaZIGAPsOfY3+119jY7pORGsq0UE2CLPvrOdw7PTXSUcaaN7/974+YHH3708Se3Pu19dnvl8y/u3P3ylZa5IvSESC7VmwhrypmgJ4YZTt9kiuI04vR1dHZgn79+S5VmUrw0lxkdpzgRbMIINiA6vfN1mGIzJZgXz8vTIpQpTXD1XZ7eWfXXfXehxUVQL1a9+jo6vXv7zzCWJE+pMIRjrUeBn5lxgZVhhNOyF+aaZpic4YSOYClwSvW4cJsoUR8kMZpIBR9hkJM2NQqBTcSisteiyRQTRgp+abUHQk5hjwqCcQbEmCgpLlPQ6LdU4omhFwPU68MK52Yq1V5xqLAISUHKKdPoCAKgOMX5AFk5ARFEEusBegE3EaNjRpNz2FZZs7jA7xVHEpxBB1zmMXpBE8iecmFG+zyRiplpqhETQBJJiIguB72+26bzd4AicFDRyV4GXg6QFbqne1jB1hBhhjZ/54pf/ez1x4XbOuiX/V4rRgUGszamupwX29wvCPVlGrWFFoajC8gREpTGdZrCcxZTMqXkrAWOUouLJI+rRFqoJUC5ZiJBYZS24DqfTNhF256hQtuSgLwpKug5kWkKYQ9FTjjYPATawtUtMza5ac4NU/Ic1fqVSSv8zkoNhmbQ7Q3Zhwgetu1GLEGQs9zADjIl45xQjSJqDFVIA8j6j5XMoQJsv7U5IynPwNRckEGSc6wuy0X7kL0l0gWnlDRQRCJpS7kUidvYorm2BKqTa4IzWqLrrrmKUQpbl13seZ4KOzqKp+UoGBc/FCGBBqfQeEnogLZuNbTHtEyL1aBsd2jD/14fQaqRhu5CEF5wPkW2mzDnkKYQmtLoPNJEscy0KECLm4shnWPGPMk0zWHkyJjaosOzNmNkHljJnQ9Eigkk2hbJrGVmldMLYzoJyRRnsD8YDRKe2SlVHFNiG7msALr6dQ0AtvE+zDunmpD9K0/71YUeP3uEjvYPft5/8vjYhevg+bPDn56soxoA3d5vXdCj56huGY3685cNgXsv6Dq/DliEl9SmwaUYMmwBI5VE48Jf3wmC7a3Ngb++Odx8sOvDwt/ZGm5vli7ZLZKI5xShZST+xu4w2LK62w82A0uytfsgCHaWkMA8Q8tJNne3/O0dSzK0LHYRgKgmgXHRpGEkK0f+uAgxKRihZXhhe5jOGeNZWhau9owpOIsyO8NhtsBgUx0qmFw45ywxGH4H6s1vA8e27t+xzwTLWaEEKEw7kDcUZqKGjT565jpKirkEZqbaLk2zaeG20WEpgzfBHFJ3QXkTCe++rjBmXM8juzh/y3GscANdCWAgXqugl2k0bPT6h7kgdVweUTiAKPoU3hHPocUxdNga5EslKRMljLckHNjVUqDNq9SAIu6NuRyhHY/97nVAYmzwhHEYJGXRWHf55kquLKob6uKEIwV2JRJ3YmRuOLNGZ4sOHAw+eMcW1e06jAsZfLGU/U67kHDSERAzd4PJmuJ60nWD3cnB+tn61aFhe9k1MaQZzrcwP+H9644hYEmj+/Yw8G27atKEiqpNZyfcZq+2oRMFw/cKHEVd2JAJhx0Nx7ZL7TFlbfTrldawLMdWt2hov4Z5fuxOOUuI1uaYgGGBoKE1UTJ9KUcboPM9WubARmmP8f/fnYp4bZEZGE/f41URvqXk5VRRCKHV53RiRnDQgxOwqI8ScLBeDVAYotWh+95AIQXF6iEKFUumZoyWk5s843VywKtcUbu7ygoKYdAknCJL7kjgVklK+/8lmP+3srh4NVwP/PXgF3/14b36n8wt7xvvnnffC7xt76H3o3fknXjE+8P7y/vb+2fl4cpkJV2RFfTmjVrnK691rVz8B4q8/IQ=</latexit> <latexit sha1_base64="fLSe8jYjTulbrhN6N2TrS5Oz92E=">AAANvniclVfrbts2FFa7W+c1a7v92/4QDQx0gRdITnPDkKFB2nTF0DZregMsL6AoWiZCkRpJNckEAXuAPcf+bq+zt9khJTeSbaWYAFv00Xe+c3huoqOMM218/99r1z/6+JNPP7vxee+Lmytf3rp956vXWuaK0FdEcqneRlhTzgR9ZZjh9G2mKE4jTt9Epwf2+Zt3VGkmxUtzkdFxihPBJoxgA6KT29+EKTZTgnnxpDwpQpnSBFff5cntVX/ddxdaXAT1YtWrr6OTOzf/DGNJ8pQKQzjWehT4mRkXWBlGOC17Ya5phskpTugIlgKnVI8Lt4kS9UESo4lU8BEGOWlToxDYRCwqey2aTDFhpOAXVnsg5BT2qCAYp0CMiZLiIgWNfkslnhh6PkC9PqxwbqZS7RWHCouQFKScMo2OIACKU5wPkJUTEEEksR6gF3ATMTpmNDmDbZU1iwv8XnEkwRl0wGUeoxc0gewpF2a0zxOpmJmmGjEBJJGEiOhy0Ou7bTp/BygCBxWd7GXg5QBZoXu6hxVsDRFmaPN3rvjlz15/XLitg37Z77ViVGAwa2Oqy3mxzf2CUF+kUVtoYTg6hxwhQWlcpyk8YzElU0pOW+AotbhI8rhKpIVaApRrJhIURmkLrvPJhJ237RkqtC0JyJuigp4RmaYQ9lDkhIPNQ6AtXN0yY5Ob5twwJc9QrV+ZtMLvrdRgaAbd3pB9iOBh227EEgQ5yw3sIFMyzgnVKKLGUIU0gKz/WMkcKsD2W5szkvIUTM0FGSQ5x+qiXLQP2VsiXXBKSQNFJJK2lEuRuI0tmmtLoDq5JjijJbrqmqsYpbB12cWe56mwo6N4Wo6CcfFjERJocAqNl4QOaOtWQ3tMy7RYDcp2hzb87/URpBpp6C4E4QXnU2S7CXMOaQqhKY3OI00Uy0yLArS4OR/SOWbMk0zTHEaOjKktOjxrM0bmgZXc+UCkmECibZHMWmZWOb0wppOQTHEG+4PRIOGZnVLFMSW2kcsKoKtfVwBgGx/CvHeqCdm/9LRfXejRs4foaP/g5/3Hj45duA6ePzt88ngd1QDo9n7rgh49Q3XLaNSfv2wI3HtB1/l1wCK8oDYNLsWQYQsYqSQaF/76ThBsb20O/PXN4eb9XR8W/s7WcHuzdMlukUQ8pwgtI/E3dofBltXdvr8ZWJKt3ftBsLOEBOYZWk6yubvlb+9YkqFlsYsARDUJjIsmDSNZOfLHRYhJwQgtw3Pbw3TOGM/SsnC1Z0zBWZTZGQ6zBQab6lDB5Nw5Z4nB8HtQb34bOLZ1/559JljOCiVAYdqBvKEwEzVs9NEz11FSzCUwM9V2aZpNC7eNDksZvAnmkLoLyptIePd1hTHjeh7ZxflbjmOFG+hKAAPxSgW9TKNho9c/zAWp4/KQwgFE0afwjngOLY6hw9YgXypJmShhvCXhwK6WAm1epQYUcW/M5QjteOx3rwMSY4MnjMMgKYvGuss3V3JlUd1QFyccKbArkbgTI3PDmTU6W3TgYPDBO7aobldhXMjgi6Xsd9qFhJOOgJi5G0zWFNeTrhvsTg7Wz9avDg3by66JIc1wvoX5Ce9fdwwBSxrds4eB79pVkyZUVG06O+E2e7UNnSgYvpfgKOrChkw47Gg4tl1qjylro18vtYZlOba6RUP7DczzY3fKWUK0NscEDAsEDa2JkulLOdoAnR/QMgc2SnuM///uVMRri8zAePIBr4rwHSUvp4pCCK0+pxMzgoMenIBFfZSAg/VqgMIQrQ7d9wYKKShWD1GoWDI1Y7Sc3OQZr5MDXuWK2t1VVlAIgybhFFlyRwK3SlLa/y/B/L+VxcXr4Xrgrwe/+KsP7tb/ZG5433p3vXte4G17D7yfvCPvlUe8P7y/vL+9f1YerExW0hVZQa9fq3W+9lrXyvl/OuL8fg==</latexit>

<latexit sha1_base64="KxwHmo78qlt3BkBwiV/qc91qwww=">AAANtXicnVfrbts2FFZ37bxma7ef+0M0MNAFXmA5zQ1DhgZp0xVD26zpDbC8gKJomQhFaiTVJCP0BnuF/d2eaW+zQ0ppJNtKgQmwRR995zuH5yY6zjnTZjj898ZHH3/y6Wef3/yi9+Wtla++vn3nm9daForQV0Ryqd7GWFPOBH1lmOH0ba4ozmJO38SnB+75m3dUaSbFS3OR00mGU8GmjGADopPbd6IMmxnB3D4pT2yBivLk9upwfegvtLgI68VqUF9HJ3du/RklkhQZFYZwrPU4HOZmYrEyjHBa9qJC0xyTU5zSMSwFzqieWO97ifogSdBUKvgIg7y0qWEFNjGLy16LJldMGCn4hdMeCDmDrSmIwSkQY6KkuMhAo99SSaaGng9Qrw8rXJiZVHv2UGEREUvKGdPoSGZAQnExQE5OQAQBxHqAXsBNJOiY0fQMtlXWLD7ee/ZIgjPogMsiQS9oCklTPrpon6dSMTPLNGICSGIJEdHloNf32/T+DlAMDio63cvBywFyQv90DyvYGiLM0ObvQvGrn73+xPqtg37Z77ViZDGYdTHV5bzYpXxBqC+yuC10MByfQ46QoDSp0xSdsYSSGSWnLXCcOVwseVIl0kEdASo0EymK4qwF18V0ys7b9gwV2pUE5E1RQc+IzDIIeyQKwsHmIdBaX67MuORmBTdMyTNU61cmnfAHJzUYekC3N+QeInjYthuzFEHOCgM7yJVMCkI1iqkxVCENIOc/VrKACnBt1uaMpTwFU3NBBknBsbooF+1D9pZIF5xS0kARibQt5VKkfmOL5toSqE6uCc5pia675ipGKexc9rHnRSbcxLBPy3E4sT/ZiECDU2i8NPJAV7ca2mNWZnY1LNsd2vC/10eQaqShuxCEF5zPkOsmzDmkKYKmNLqINVEsNy0K0OLmfETnmDFPc00LGDkyoa7o8GWbMTIPrOTeByLFFBLtiuSyZS4rpxcldBqRGc5hfzAaJDxzU8oeU+IauawAuvp1DQC28SHMe6eakP0rT/vVhR49e4iO9g9+2X/86NiH6+D5s8Mnj9dRDYBu77cu6NEzVLeMRv35y4XAvw50nV8PtNEFdWnwKYYMO8BYpfHEDtd3wnB7a3MwXN8cbd7fHcJiuLM12t4sfbJbJDEvKELLSIYbu6Nwy+lu398MHcnW7v0w3FlCAvMMLSfZ3N0abu84kpFjcYsQRDUJjIsmDSN5OR5ObISJZYSW0bnrYTpnjOdZaX3tGWM5i3M3w2G2wGBTHSqYnHvnHDEYfg/qzW8DJ67u37NfCpazQglQmHYgbyhciho2+uiZ7ygp5hKYm2q7NMtn1m+jw1IOb4I5pO6C8iYS3n1dYcy5nkd2cf5e4EThBroSwEC8VkEv02jY6PUPC0HquDykcABR9Cm8I55Di2PosDXIl0ozJkoYb2k0cKulQJdXqQFF/BtzOUJ7Hvfd64Ak2OAp4zBISttYd/nmS6601Q11ccKRAvsSSToxsjCcOaOXiw4cDD54x9rqdh3Ghwy+WMb+oF1IOOkIiJm/wWTNcD3pusH+5OD8bP3q0HC97JsY0gzHWpif8P71xxCwpNE9dxj4vl01WUpF1aaXB9tmr7ahUwXD9wocx13YiAmPHY8mrkvdMWVt/NuV1qgsJ07XNrTfwDw/9qecJURrc0zAsEDQ0Joqmb2U4w3Q+REtc2CjhNP7/3CnIl5bZAbGkw94ZaN3lLycKQohdPqcTs0YDnpwAhb1UQIO1qshiiK0OvLfGyiioFg9RJFi6cxM0HJyU+S8Tg54VSjqdldZQREMmpRT5Mg9CdwqSen+v4Tz/1YWF69H6+FwPfx1uPrgbv1P5mbwXXA3uBeEwXbwIPg5OApeBSQ4C/4K/g7+WdlemawkK9MK+tGNWufboHWtyP8AhpP4ZA==</latexit>

<latexit sha1_base64="9J+Wl4nj0bD8Int0qaURG3Jswzk=">AAANtXicnVfrbts2FFa7W+c1W7v93B+igYEu8ALJaW4YMjRIm24Y2mZNb4DlBRRFy0QoUiOpJhmhN9gr7O/2THubHVJKY9lWCkyALfroO985PDfRScGZNmH4742bH338yaef3fq898XtlS+/unP369dalorQV0Ryqd4mWFPOBH1lmOH0baEozhNO3ySnB+75m3dUaSbFS3NR0HGOM8EmjGADopM7d+McmynB3D6vTmyJyurkzmq4HvoLLS6iZrEaNNfRyd3bf8apJGVOhSEcaz2KwsKMLVaGEU6rXlxqWmByijM6gqXAOdVj632vUB8kKZpIBR9hkJfOaliBTcKSqteiKRQTRgp+4bQHQk5hawpicArEmCgpLnLQ6LdU0omh5wPU68MKl2Yq1Z49VFjExJJqyjQ6kjmQUFwOkJMTEEEAsR6gF3ATKTpmNDuDbVUNi4/3nj2S4Aw64LJM0QuaQdKUjy7a55lUzExzjZgAkkRCRHQ16PX9Nr2/A5SAg4pO9grwcoCc0D/dwwq2hggzdPZ3qfjVz15/bP3WQb/q91oxshjMupjqal7sUr4g1Bd50hY6GE7OIUdIUJo2aYrPWErJlJLTFjjJHS6RPK0T6aCOAJWaiQzFSd6C63IyYedte4YK7UoC8qaooGdE5jmEPRYl4WDzEGitL1dmXHLzkhum5Blq9GuTTvi9kxoMPaDbG3IPETxs201YhiBnpYEdFEqmJaEaJdQYqpAGkPMfK1lCBbg2a3MmUp6Cqbkgg6TkWF1Ui/Yhe0ukC04paaCIRNaWcikyv7FFc20JVCfXBBe0QtddcxWjFHYu+9jzMhduYtin1Sga2x9tTKDBKTReFnugq1sN7TGtcrsaVe0OnfG/10eQaqShuxCEF5zPkesmzDmkKYamNLpMNFGsMC0K0OLmfEjnmDHPCk1LGDkypa7o8GWbMTIPrOXeByLFBBLtiuSyZS4rpxendBKTKS5gfzAaJDxzU8oeU+IauaoBuv51DQC28SHMe6dmIftXnvbrCz1+9ggd7R/8sv/k8bEP18HzZ4c/P1lHDQC6vd+6oEfPUNMyGvXnLxcC/zrQTX490MYX1KXBpxgy7AAjlSVjG67vRNH21uYgXN8cbj7YDWER7mwNtzcrn+wWScJLitAyknBjdxhtOd3tB5uRI9nafRBFO0tIYJ6h5SSbu1vh9o4jGToWt4hA1JDAuJilYaSoRuHYxphYRmgVn7sepnPGeJFX1teeMZazpHAzHGYLDDbVoYLJuXfOEYPh96De/DZw6ur+PfulYDkrlACFaQfyGYVL0YyNPnrmO0qKuQQWpt4uzYup9dvosFTAm2AOqbugfBYJ776uMBZczyO7OH8vcarwDLoWwEC8VkEv05ix0esfloI0cXlE4QCi6FN4RzyHFsfQYWuQL5XlTFQw3rJ44FZLgS6vUgOK+DfmcoT2PO671wFJscETxmGQVHZm3eWbL7nK1jfUxQlHCuxLJO3EyNJw5oxeLjpwMPjgHWvr23UYHzL4Yjn7g3Yh4aQjIGb+BpM1x82k6wb7k4Pzs/WrQ8P1sm9iSDMca2F+wvvXH0PAkkb33WHgu3bV5BkVdZteHmxne7UNnSgYvlfgJOnCxkx47Gg4dl3qjilro9+utIZVNXa6dkb7DczzY3/KWUK0NscEDAsEM1oTJfOXcrQBOj+gZQ5sVHB6/x/u1MRri8zAePIBr2z8jpKXU0UhhE6f04kZwUEPTsCiOUrAwXo1QnGMVof+ewPFFBTrhyhWLJuaMVpObsqCN8kBr0pF3e5qKyiGQZNxihy5J4FbLanc/5do/t/K4uL1cD0K16Nfw9WH95p/MreCb4N7wf0gCraDh8FPwVHwKiDBWfBX8Hfwz8r2ynglXZnU0Js3Gp1vgta1Iv8D1jf4ag==</latexit>

<latexit sha1_base64="6H5hDhdLcjIAFI1rSMWHUNwlL2A=">AAANrnicnVdbb9s2FFa7W+c1W7s97oVoYKALvEBybg6GDA3SphuGtlnTKywvoChaJkKRGkk1yQj9gz3vdftb+zc7pOzGcqwUmABb1NF3Ph6eG6mk4EybMPz3xs2PPv7k089ufd754vbKl1/dufv1Ky1LRehLIrlUbxKsKWeCvjTMcPqmUBTnCaevk9MD9/71O6o0k+KFuSjoKMeZYGNGsAFRHCe5fVud2BKV1cmd1XA92t0Y9DdQuL7ZDweDbRiEg53tcAdF66G/VoPpdXRy9/afcSpJmVNhCMdaD6OwMCOLlWGE06oTl5oWmJzijA5hKHBO9ch6oyvUBUmKxlLBTxjkpfMaVmCTsKTqNGgKxYSRgl847Z6QE1iTgsWfAjEmSoqLHDS6DZV0bOh5D3W6MMKlmUi1Zw8VFjGxpJowjY5kDiQUlz3k5ARE4Dmse+g53ESKjhnNzmBZ1ZTFO3rPHkkwBh1wWaboOc0gWsq7Fe3zTCpmJrlGTABJIsEjuup1un6Z3t4eSsBARcd7BVjZQ07o3+5hBUtDhBk6/1wqfvnY6Y6sXzroV91Ow0cWw7TOp7paFOfYTK4I9UWeNIUOhpNziBESlKbTMMVnLKVkQslpA5zkDpdIntaBdFBHgErNRIYgwxpwXY7H7Lw5n6FCu5SAuCkq6BmReQ5uj0VJOMx5CLQ2dpzMuODmJTdMyTM01a+ndMLvndRgSH7dXJB7ieBlc96EZQhiVhpYQaFkWhKqUUKNoQppADn7sZIlZICrryZnIuUpTLXgZJCUHKuL6ur8EL0l0itGKWkgiUTWlHIpMr+wq9M1JZCdXBNc0Apddy1kjFLYmex9z8tcuFZhn1TDaGR/tDGBAqdQeFnsgS5vNZTHpMrtalQ1K3TO/k4XQaiRhupC4F4wPkeumjDnEKYYitLoMtFEscI0KECLm/M+XWDGPCs0LaHlyJS6pMOzMmNkEVjLvQ1EijEE2iXJrGRmmdOJUzqOyQQXsD5oDRLeuS5ljylxhVzVAF0/XQOAZXwI896oecj+paXd+kKPnj5ER/sHv+w/fnTs3XXw7Onhz4/X0RQA1d5tXFCjZ2haMhp1Fy/nAr8P6Gl8PdDGF9SFwYcYIuwAQ5UlIxuuD6JoZ3urF65v9bc2d8Oe2wO2+ztblQ92gyThJUVoGUm4sduPtp3uzuZW5Ei2dzejaLCEBPoZWk6ytQtbz8CR9B2LG0QgmpJAu5inYaSohuHIxphYRmgVn7sapguT8SKvrM89YyxnSeF6OPQWaGyqRQWTc2+cI4aJ34M6i8vAqcv79+wzwXJWSAEK3Q7kcwoz0dwcXfTUV5QUCwEsTL1cmhcT65fRMlMBO8ECUrdB+TwS9r42NxZcLyLbOH8vcarwHLoWQEO8VkEv05ibo9M9LAWZ+uUhhQOIok9gj3gGJY6hwtYgXirLmaigvWVxz42WAl1cpQYU8TvmcoT2PO6/0wJJscFjxqGRVHZu3GabT7nK1jfUxglHCuxTJG3FyNJw5iadDVpw0Phgj7X17TqMdxn8sZz9QduQcNIR4DN/g86a42mnawf7k4Ozs/HUouFq2RcxhBnOs9A/Yf/1xxCYSaP77jDwXTNr8oyKukzdS4J5o1ab0LGC5nsJTpI2bMyExw77I1el7piyNvztUqtfVSOna+e0X0M/P/annCVEawtMwHCFYE5rrGT+Qg43QOcHtMyAjQpO7//DnJp47SozMJ58wCobv6PkxURRcKHT53RshnDQgxOwmB4l4GC9GqE4Rqt9/7+BYgqK9UsUK5ZNzAgtJzdlwafBAatKRd3q6llQDI0m4xQ5ck8Ct1pSue+X2UcKah+86q9H8Jnza7j64N70S+ZW8G1wL7gfRMFO8CD4KTgKXgYkKIK/gr+Df1bClVcro5WTGnrzxlTnm6BxrUz+A46Q9lg=</latexit>

<latexit sha1_base64="nnm87+ksFszI0xdDavbjiN/Qhlo=">AAANuniclVfrbts2FFa7W+c1W7sB+7M/RAMDXeAZknNzsGVokDbdMLTNmvQyWF5AUbRMhCI1kmqSaXqDPcX+bi+0t9khJTeWY6WYAFvU0Xc+Hp4bqSjjTBvf//fGzffe/+DDj2593Pnk9sqnn925+/lLLXNF6AsiuVSvI6wpZ4K+MMxw+jpTFKcRp6+i0337/tUbqjST4thcZHSc4kSwCSPYgOjkzpdhlBa/lCdFKFOaYFTdypM7q34/2FkfDtaR398Y+MPhFgz84faWv42Cvu+uVa++Dk/u3v4zjCXJUyoM4VjrUeBnZlxgZRjhtOyEuaYZJqc4oSMYCpxSPS7cAkrUBUmMJlLBTxjkpPMahcAmYlHZadBkigkjBb+w2j0hp7A+BY44BWJMlBQXKWh0GyrxxNDzHup0YYRzM5VqtzhQWISkIOWUaXQIDlCc4ryHrJyACLyIdQ89h5uI0RGjyRksq6xZnNN3i0MJxqB9LvMYPacJRE45F6M9nkjFzDTViAkgiSR4RJe9Ttct09nbQxEYqOhkNwMre8gK3dtdrGBpiDBD559zxS8fO91x4ZYO+mW30/BRgWFa61NdLopTbKZXhPoijZpCC8PROcQICUrjOkzhGYspmVJy2gBHqcVFksdVIC3UEqBcM5EgyLUGXOeTCTtvzmeo0DYlIG6KCnpGZJqC20OREw5zHgBtEVpOZmxw05wbpuQZqvWrKa3wGys1GApBNxdkXyJ42Zw3YgmCmOUGVpApGeeEahRRY6hCGkDWfqxkDhlga63JGUl5ClMtOBkkOcfqorw6P0RvifSKUUoaSCKRNKVcisQt7Op0TQlkJ9cEZ7RE110LGaMUtiY73/M8FbZtFE/KUTAuvi9CAgVOofCS0AFt3mooj2mZFqtB2azQOfs7XQShRhqqC4F7wfgU2WrCnEOYQihKo/NIE8Uy06AALW7OB3SBGfMk0zSHliNjapMOz8qMkUVgJXc2ECkmEGibJLOSmWVOJ4zpJCRTnMH6oDVIeGe7VHFEiS3ksgLo6ukaACzjXZi3Rs1D9i4t7VYXevT0ITrc2/9p7/GjI+eu/WdPD3583Ec1AKq927igRs9QXTIadRcv6wK3J+g6vg5YhBfUhsGFGCJsASOVROPC7w+DYHtrs+f3NwebGzt+z+4BW4PtzdIFu0ES8ZwitIzEX98ZBFtWd3tjM7AkWzsbQTBcQgL9DC0n2dyBrWdoSQaWxQ4CENUk0C7maRjJypE/LkJMCkZoGZ7bGqYLk/EsLQuXe8YUnEWZ7eHQW6CxqRYVTM6dcZYYJn4L6iwuA8c279+yzwTLWSEFKHQ7kM8pzERzc3TRU1dRUiwEMDPVcmmaTQu3jJaZMtgJFpC6DcrnkbD3tbkx43oR2cb5W45jhefQlQAa4rUKepnG3Byd7kEuSO2XhxQOIIo+gT3iGZQ4hgpbg3ipJGWihPaWhD07Wgq0cZUaUMTtmMsR2vHY/04LJMYGTxiHRlIWc+M221zKlUV1Q22ccKTALkXiVozMDWd20tmgBQeND/bYorpdh3Eugz+Wst9pGxJOOgJ85m7QWVNcd7p2sDs5WDsbTy0atpZdEUOY4WwL/RP2X3cMgZk0um8PA183syZNqKjK1L4kmDdqtQmdKGi+l+AoasOGTDjsaDC2VWqPKWujXy+1BmU5trrFnPYr6OdH7pSzhGhtgQkYrhDMaU2UTI/laB10vkXLDFgv7Tn+/5tTEa9dZQbGk3dYVYRvKDmeKgoutPqcTswIDnpwAhb1UQIO1qsBCkO0OnD/6yikoFi9RKFiydSM0XJyk2e8Dg5YlStqV1fNgkJoNAmnyJI7ErhVktJ+v8w+UlD74OWgH8Bnzs/+6oN79ZfMLe8r75533wu8be+B94N36L3wiPeH95f3t/fPyncr0QpbOa2gN2/UOl94jWvF/AfzPfrN</latexit>

Fig. 4: Overview of different variables used to estimate the covariance ofICP. Top row: A single example of two cylinders being registered togetherusing ICP and converging to well aligned point clouds P (blue) and Q (red).Middle row: A view of the translation components u of a set of initiationtransformation O before ICP (left) and after ICP (right). Bottom row: Sameview, but for the rotational components ω. For the rotations, the spread alongthe vertical axis is explained by the cylinder being unconstrained around oneaxis. The presence of points in the outer ring is explained by P first turningupside down and then being unconstrained around the same axis.

between P and Q by relying on a prior transformation qT .A typical solution to this registration problem is the ICPalgorithm:

pT “ icppP ,Q, qT q. (2)

The initial transformation qT can be seen as an element ran-domly selected from a distribution of transformations O, forwhich the shape typically depend on odometry computation.Similarly, the estimated value pT comes from a distribution oftransformations I which has a complex shape that dependson both point clouds, O and the configuration of icpp¨q.Estimating the covariance Y of ICP corresponds to makingthe (oversimplifying but tractable) assumption that the I isnormally distributed

I « N pT ,Y q, (3)

where the right hand side is shorthand for

qT “ exppξqT with ξ „ N p0,Y q. (4)

Note the use of T in Equation 3, which supposes that ICP isunbiased. Figure 4 shows an example of those symbols, withtwo cylinders being registered starting with a wrongful initialalignment qT . The distribution O was manually configuredand sampled to feed multiple qT to ICP. The 5000 resultingtransformations pT form a distribution with covariance Ythat approximates I. The covariance approximates the trans-lations well, but some points in rotations were considereddivergent and filtered away.

A. Covariance prediction

Vega-Brown et al. [15] propose a data-driven frameworkfor covariance estimation, in which the estimation function pF

is posed as a weighted average of training examples. The firststep in such an approach is to collect a training dataset D “tpd0,Y0q, ..., pdn,Ynqu composed of point cloud descriptorsdk and sampled covariances Yk. Our formulation differsfrom the original CELLO framework, which has error vectorsin the place of the covariance matrices Yk. This was renderednecessary due to the limited availability of 3D point cloudpairs with associated ground truth: it allows us to extractmore knowledge from existing point cloud pairs.

The descriptors dk are computed by a functiongpaP , abT

bQq that extract features from a registered pointcloud pair. One can then predict a covariance pF pdq for anunseen example d using

pF pdq “1

ř

k s´

ρpd,dkq¯

ÿ

k

s´

ρpd,dkq¯

Yk. (5)

The function ρ is a distance between a pair of point clouddescriptors which is defined as

ρpd,d1q “ pd´ d1qJΘJΘpd´ d1q (6)

where Θ is an upper triangular matrix. The weighing func-tion spxq “ e´x is chosen here, but any decreasing positivefunction is appropriate [15]. Large datasets could motivatea choice of s that completely ignores examples with largedistances to make runtime predictions more efficient [18].For the training of Θ, the loss for an individual covarianceprediction pF pdkq is

Lp pF pdkq|Θq “ det´

pF pdkq¯

` tr´

pF pdkq´1Ykq

¯

(7)

along with a regularization term [15].

B. 3D point cloud descriptor

It is important for a point cloud descriptor dk to containrelevant information to the prediction of the covariance forP , Q, while being small enough to be amenable to machinelearning algorithms. Thus, the descriptor extraction functiongp¨q should capture the geometry of the scene, in a way thattranslates our assumption that this geometry is an importantfactor of the covariance. One consequence of this is thatthe extracted descriptors should not be rotation invariant,so as to capture the correct orientation of the covariance.For instance, the covariance of ICP in a featureless hallwayshould be aligned with its walls, as depicted in Figure 1.There is a wide variety of descriptors that could be used for3D point clouds [19], but a more thorough evaluation of theexisting descriptors is a question that is left for future work.

ICP—at least in its robust version—acts mainly on theoverlapping region of point clouds. Consequently, descriptorsshould only be extracted from this overlapping region. In ourapproach, g first extracts a single point cloud S containingthe subset of points from both P and Q that are overlapping,after registration. Then, descriptors are extracted from S. Theextraction pipeline separates the space into a fixed-size grid,from which all local descriptors are extracted. First, two de-scriptors capture the overall “planarity” p and “cylindricality”c of the entire voxel, from the average values of these two

TABLE I: ICP pipeline used for the sampled covariance computation

Pipeline Element Configuration

Point cloud filters Maximum density, random subsamplingPoint matcher k-d tree, 3 nearest neighbors

Outlier filter Trimmed distance (keep the closest 70%of associations)

Error minimizer point-to-planeTransformation checkers Max. 80 iterations

metrics defined in [20] computed at each point within thevoxel. Then, the orientation of the estimated surface normalsvectors for every point in a voxel are summarized in a 9-histogram th1, ..., h9u [21]. The local descriptor of voxeli, j, k is vijk “ tp, c, h1, ..., h9u. The full descriptor dfor the overlaping point cloud S is the concatenation thelocal descriptors for every voxel. This way, our descriptorpreserves a certain amount of global information, namelythe spatial distribution of the local features.

C. Covariance sampling

We employ a brute-force approach similar to [7] to esti-mate the covariance of ICP for training. We sample the resultof ICP for every point cloud pair in our dataset. Every sampleuses an initial estimate qT drawn from O. The distribution ofresults is then used to compute a sampled covariance Yk forthe point cloud pairs through

Yk “1

n´ 1

ÿ

i

ξiξJi (8)

with ξi “ logpT´1k

pTiq the n perturbations of the sampledtransformations. In some cases, like in the lower row ofFigure 4, ICP converges to many clusters. If we suppose thatwe estimate the covariance of ICP when it converges, it isultimately up to the designer to decide what converged fromwhat did not. We use the DBSCAN clustering algorithm [22]on the set of ξi, and keep only the points in the clusterwhich is closest to ground truth. This filtering method has thebenefit of avoiding unrepresentative sampled covariances Yin the training dataset. It does so without imposing an upperbound to the covariance of the samples. One more look atFigure 4 illustrates the results of this procedure. The centralline of samples is explained by the fact that rotations aroundthe z axis are not constrained on this cylinder. The outerring corresponds to situations where ICP converged upside-down, and then spun freely around the z axis. Our filteringstrategy is able to remove results that converged incorrectly(i.e. the outer ring) while capturing the information about anunderconstrained axis (i.e. the central line).

V. EXPERIMENTS

A. Training datasets

To be realistic, we used datasets that are representativeof a wide variety of environments that a mobile robot canencounter. Consequently, we used a subset of the Challeng-ing data sets for point cloud registration algorithms [23].It comprises point clouds taken in environments rangingfrom structured to unstructured, and indoor to outdoor. EveryChallenging dataset contains a sequence of l point clouds Pi

as well as ground truth positions T for them. To generate alearning dataset D “ tpd0,Y0q, ..., pdn,Ynqu, we consideredpairs of point clouds Pi,Pj for all i, j such that i ă j ă land j ´ i ď 4.

The descriptors dk were generated using gp¨q. We used agrid of 4ˆ4ˆ4 spanning 25 m in the x and y axes (parallelto the ground plane) and 10 m in the z axis (perpendicularto the ground plane) [19]. This grid was chosen because itencapsulates the typical spatial extent of a point cloud fromthe Challenging datasets.

Each sampled covariance Yk was computed from 5000registrations. Every registration had a O that was centeredat the ground truth and a covariance of aI . We set a “0.05 to simulate a reasonable odometry scenario. A typicalICP registration pipeline was used, featuring the point-to-plane error metric. Table I describes the full registrationpipeline, in terms of the framework layed down in [24]. Asdiscussed in Section IV-C, the outliers were filtered fromthe registration transformations to enforce the assumptionthat ICP converged. Point cloud pairs where ICP failedconsistently were removed from the training dataset. To doso we identified the pairs where ICP converged on averagemore than 1m or 1rad away from ground truth. In total, thiswork uses the data from about 5 100 000 registrations on1020 pairs. These registrations were performed on ComputeCanada’s computing clusters using a total of approximately5 CPU-Years.

Data augmentation was performed to extract more knowl-edge from the computationally expensive sampling of theYk. By applying a transformation T on the point cloudsbefore sending them in gp¨q, we obtained new descriptorsdaug. The sampled covariances were transformed similarilyusing the adjoint representation of T such that Yaug “

AdjT ¨Yk ¨ AdjJT . For our application we chose to performdata augmentation only by rotating the frames of referencearound the z axis of the reference point cloud. The z axiswas chosen to preserve the 2.5D aspect of the dataset,while giving some rotation invariance to our algorithm. Otherrotations were expected to create examples that are not closeto the registration pairs encountered online, such as exampleswhere the trees of a forest are sideways. A 6-DOF robotwould warrant a more complete data augmentation.

B. ICP trajectories computation

Once the models were trained, we evaluated our covari-ance estimation algorithm for state estimation on indoorand outdoor trajectories. In that spirit, we computed an ICPodometry from our trajectory datasets. Since the point cloudpairs are in a sequence of length l, we compounded theirICP registration transformations using pTF “

1

0pT

2

1pT ...

l

l´1pT

where pTF is the final pose estimate of the odometry. Thedistribution of initial transformations was O « N pi`1

iT , aIqwith a “ 0.05. In a second step, we computed covarianceestimation pYk for each pTk using a covariance estimationmodel pF . Finally we compounded the covariances with the4th order approximation in Barfoot et al. [25] to obtain afinal covariance estimate pYF. This setup allowed us to assess

the quality of the covariance estimation in context, overtrajectories. VI. RESULTS

We trained on one trajectory dataset, while testing onone or many others that had the same type of environ-ment/structure. Testing on separate (but similar) datasetswas done to obtain a fair evaluation, while avoiding overlyoptimistic result due to overfitting. This correspondence isdetailed in Table II. The weighting matrix Θ was trainedby stochastic gradient descent using a learning rate of 1e´5using the loss from Equation 7. The learning was stoppedafter 100 iterations or until convergence was reached.

A. Single-Pair Covariance Prediction

First, we validated the quality of our approach, for singlepairs of point clouds. We used as quality metric the Kullback-Leibler (KL) divergence between the sampled distributionand our estimated distribution (covariance). This metricmeasures the amount of information lost when using thecovariance pF pdkq instead of Yk to express the distributionof ICP results I. Results reported in Table II are the averageKL divergence over all pairs within a testing group. Forcomparison, we also computed the average KL divergencefor a baseline covariance Ybase “

1n

ř

k Yk, using the Yk

of the training dataset. We made similar computations witha Censi estimate YCensi, for the same point cloud pairs.One should keep in mind that the mere prediction of thescale of the ICP covariance has been historically challenging.As hinted by Figure 3, Censi’s covariance estimate havelarge divergences, since they are orders of magnitude smallerthan the sampled covariances. At worst, our approach makespredictions that have the correct order of magnitude, as seenin Table II.

Trajectory in self-similar environments will have pointclouds (and covariances) that are similar to one another. Forthese self-similar environment, gains over the baseline areexpected to be modest. In this situation, we observe thatour predictor’s parameters Θ converges to nearly uniformweights after training. Consequently, CELLO-3D treats train-ing examples nearly equally, and outputs (something closeto) their mean. This phenomenon is clearly visible in Table II,where gains over the baseline are limited for Wood Autumnand Wood Summer. For the Gazebo environments, they alsoexhibit a certain degree of self-similarity, as measured bythe low KL-divergence of the baseline. Again, gains for ourapproach are modest there. However, for the three indoorenvironments (apt, haupt, and stairs), we can see that theyhave the highest baseline KL-divergence. This indicatesthat the sampled covariance varies largely throughout thetrajectory, in comparison to the average (baseline) one. Asexpected, it is where our algorithm makes the strongest gains.These gains for indoor environments are also explained bytheir structured nature, well-suited for our descriptors.

B. Consistency over Trajectories

We evaluated the consistency of the estimated covarianceswhen computing ICP odometry trajectories in diverse loca-tions. This represents the fundamental situation where our

TABLE II: Loss of the CELLO algorithm in various training scenarios.

Dataset Trained on N.Pairs

Avg. KL Divergence

Baseline Ours Censi

Apartment Haupt. & Stairs 1190 34.1 26.6 9.19e7Hauptgebaude Apt & Stairs 938 34.0 26.7 2.06e8Stairs Apt & Haupt. 798 33.7 27.0 7.65e7Gazebo Summer Gzb. Winter 826 20.8 19.8 2.55e6Gazebo Winter Gzb. Summer 798 20.3 18.9 2.25e6Wood Autumn Wd Summer 812 13.2 11.5 4.94e6Wood Summer Wd Autumn 966 13.6 11.3 3.52e7

TABLE III: Final odometry error and consistency of CELLO-3D.

Length Translation RotationDM

Traj. (m) }u} (m) DM }ω} (rad) DM

Apartment 22 0.115 0.274 0.0331 0.160 0.540Haupt. 24 0.168 0.467 0.00910 0.346 1.15Stairs 12 0.0664 0.0998 0.0127 0.307 0.592Gzb. Smmr 14 0.0396 0.278 0.0165 0.278 0.491Gzb. Wntr 15 0.0311 2.000 0.0144 2.90 2.50Wd Atmn 18 0.217 0.205 0.0178 0.394 0.405Wd Smmr 21 0.332 0.208 0.0299 0.533 0.762

predictor is used within a state-estimation algorithm. Tovisualize the error on the covariance in all dimensions atonce, we resort to computing the Mahalanobis distance DMbetween pTF and the ground truth l

0T

DM “

b

ξJ pY ´1F ξ, (9)

in which ξ “ logp l0T´1

pTFq. The distance DM can be thoughtof as the number of standard deviations between a sampleand the mean, with a value zero meaning that pTF wasexactly on the ground truth. Table III shows the averageDM of 100 trajectories for every covariance model. It alsolists the average Mahalanobis distances of the translation uagainst Yuu and rotation ω against Yωω for the trajectories.The average values of DM indicate that our algorithm isconsistent overall. We consider that an average DM above 3indicates an optimistic covariance estimate, while an averagebelow 1.5 insicates a pessimistic estimate. In that sense, thecovariance estimation algorithm is pessimistic for the Stairsor Apartment datasets. However, not extreme values werefound demonstrating a functional solution.

In Figure 5, we take a closer look at the behaviour ofCELLO-3D over short trajectories, in the Wood Summer(unstructured) and Gazebo Winter (semi-structured) envi-ronments. The figure compares the compounded sampledcovariances Yk with our estimated covariances pF pdkq. Inthere, we sampled 20 ICP odometry trajectories to compareagainst the covariance predictions at each step (the greyellipses). The green dots represent the final pTF for eachtrajectory, with the green ellipses being pYF. This figureshows that our covariance estimates are consistent withthe compounded trajectories, within 2 sigma. Note thatthe CELLO-3D uncertainty estimate grows more steadilyand uniformly than that of the sampled covariances. Thisindicates that, while our algorithm does not seem to modelthe input descriptors in very sharp detail, it is able to extracta consistent knowledge from the training dataset.

0 10

0

5

10

15

y(m

)

Sampled Covariances

0 10

0

5

10

15

Learned Covariances

0.0 2.5 5.0x (m)

−4

−2

0

2

y(m

)

0.0 2.5 5.0x (m)

−4

−2

0

2

Fig. 5: Comparison of covariance estimations for Wood Summer (top) andGazebo Winter (bottom). Every frame as 20 ICP odometry trajectories,although some are not visible because they are overlapping. The groundtruth trajectory is shown as a thick black line. The green dots represent thefinal poses pTF, while the green ellipses represent pYF. The data is projectedon the ground plane for the sake of visualization.

VII. CONCLUSION

In this work, we presented CELLO-3D, an online co-variance estimation algorithm for ICP that works well in3D. CELLO-3D uses the covariances of a learning datasetto predict the covariance of similar point cloud pairs atruntime. It was successfully validated on individual pairs ofpoint clouds and over trajectories, on challenging datasets. Itprovides also a better uncertainty estimate when compared toexisting solutions; Our predicted covariances are neither toooptimistic nor too pessimistic, and represent well sampledparticles over trajectories of several meters.

Throughout the course of this work, some challengesbecame visible with the transition from 2D to 3D. Dueto the curse of dimensionality, generating a dataset forcovariance in 3D requires exponentially more samples inthan in 2D. With this large number of samples, we noticedthat approximating I as a normal distribution is prone tolarger estimation errors in SEp3q, as we observe mainly mul-timodal distributions (see Figure 1 and Figure 4). Moreover,the original CELLO framework proposed the use of weakdescriptors to alleviate the difficulties of descriptor design.However, the quality of the input features is found to becritical to the success of covariance predictions. In futureworks, we intend to meet this challenge using Deep NeuralNetworks (DNNs) as in [26].

REFERENCES

[1] P. Besl and N. McKay, “A Method for Registration of3-D Shapes,” IEEE Transactions on Pattern Analysis

and Machine Intelligence, vol. 14, no. 2, pp. 239–256,Feb. 1992.

[2] C. Yang and G. Medioni, “Object modelling by regis-tration of multiple range images,” in Image and VisionComputing, vol. 10, 1991, pp. 145–155.

[3] F. Pomerleau, F. Colas, and R. Siegwart, “A Reviewof Point Cloud Registration Algorithms for MobileRobotics,” Foundations and Trends in Robotics, vol.4, no. 1, pp. 1–104, 2015.

[4] A. Censi, “An accurate closed-form estimate of ICP’scovariance,” in IEEE International Conference onRobotics and Automation, Apr. 2007, pp. 3167–3172.

[5] Z. Zhang, “Iterative Point Matching for Registration ofFree-Form Curves and Surfaces,” International Jour-nal of Computer Vision, vol. 13, no. 2, pp. 119–152,1994.

[6] O. Bengtsson and A.-J. Baerveldt, “Robot localizationbased on scan-matching—estimating the covariancematrix for the IDC algorithm,” Robotics and Au-tonomous Systems, vol. 44, no. 1, pp. 29–40, Jul. 2003.

[7] T. M. Iversen, A. G. Buch, and D. Kraft, “Prediction ofICP pose uncertainties using Monte Carlo simulationwith synthetic depth images,” in IEEE InternationalConference on Intelligent Robots and Systems, 2017,pp. 4640–4647.

[8] P. Biber and W. Strasser, “The normal distributionstransform: a new approach to laser scan match-ing,” in IEEE/RSJ International Conference on In-telligent Robots and Systems (IROS 2003) (Cat.No.03CH37453), vol. 3, 2003, pp. 2743–2748.

[9] J. Nieto, T. Bailey, and E. Nebot, “Scan-SLAM: Com-bining EKF-SLAM and scan correlation,” SpringerTracts in Advanced Robotics, vol. 25, pp. 167–178,2006.

[10] M. Bosse and R. Zlot, “Map matching and data associ-ation for large-scale two-dimensional laser scan-basedSLAM,” International Journal of Robotics Research,vol. 27, no. 6, pp. 667–691, 2008.

[11] S. M. Prakhya, Liu Bingbing, Yan Rui, and WeisiLin, “A closed-form estimate of 3D ICP covariance,”in Proceedings of the 14th IAPR International Con-ference on Machine Vision Applications, MVA 2015,IEEE, May 2015, pp. 526–529.

[12] E. Mendes, P. Koch, and S. Lacroix, “ICP-basedpose-graph SLAM,” in SSRR 2016 - InternationalSymposium on Safety, Security and Rescue Robotics,IEEE, Oct. 2016, pp. 195–200.

[13] M. Barczyk and S. Bonnabel, “Towards RealisticCovariance Estimation of ICP-based Kinect V1 ScanMatching : the 1D Case,” in Proceedings of the Amer-ican Control Conference, May 2017, pp. 4833–4838.

[14] S. Bonnabel, M. Barczyk, and F. Goulette, “On thecovariance of ICP-based scan-matching techniques,”January 2015, IEEE, 2016, pp. 5498–5503.

[15] W. Vega-Brown, A. Bachrach, A. Bry, J. Kelly, andN. Roy, “CELLO: A fast algorithm for Covariance

Estimation,” in IEEE International Conference onRobotics and Automation, May 2013, pp. 3160–3167.

[16] W. Vega-Brown and N. Roy, “CELLO-EM: Adaptivesensor models without ground truth,” in IEEE Interna-tional Conference on Intelligent Robots and Systems,IEEE, Nov. 2013, pp. 1907–1914.

[17] V. Peretroukhin, L. Clement, M. Giamou, and J. Kelly,“PROBE: Predictive robust estimation for visual-inertial navigation,” in IEEE International Confer-ence on Intelligent Robots and Systems, Sep. 2015,pp. 3668–3675.

[18] V. Peretroukhin, W. Vega-Brown, N. Roy, and J. Kelly,“PROBE-GK: Predictive robust estimation using gen-eralized kernels,” in IEEE International Conference onRobotics and Automation, Jun. 2016, pp. 817–824.

[19] M. Bosse and R. Zlot, “Place Recognition UsingRegional Point Descriptors for 3D Mapping,” in Fieldand Service Robotics, Springer Berlin Heidelberg,2010, pp. 195–204.

[20] J. Demantke, C. Mallet, N. David, and B. Vallet,“Dimensionality based scale selection in 3d lidarpoint clouds,” The International Archives of the Pho-togrammetry, Remote Sensing and Spatial InformationSciences, vol. 38, no. Part 5, W12, 2011.

[21] M. Magnusson, H. Andreasson, A. Nuchter, and A. J.Lilienthal, “Appearance-based loop detection from 3Dlaser data using the normal distributions transform,”in Robotics and Automation, 2009. ICRA’09. IEEEInternational Conference on, vol. 3, 2009, pp. 23–28.

[22] E. Schubert, J. Sander, M. Ester, H. P. Kriegel, andX. Xu, “DBSCAN Revisited, Revisited,” ACM Trans-actions on Database Systems, vol. 42, no. 3, pp. 1–21,Jul. 2017.

[23] F. Pomerleau, M. Liu, F. Colas, and R. Siegwart,“Challenging data sets for point cloud registrationalgorithms,” The International Journal of RoboticsResearch, vol. 31, no. 14, pp. 1705–1711, 2012.

[24] F. Pomerleau, F. Colas, R. Siegwart, and S. Magnenat,“Comparing ICP Variants on Real-World Data Sets,”Autonomous Robots, vol. 34, no. 3, pp. 133–148, Feb.2013.

[25] T. D. Barfoot and P. T. Furgale, “Associating uncer-tainty with three-dimensional poses for use in estima-tion problems,” IEEE Transactions on Robotics, vol.30, no. 3, pp. 679–693, 2014.

[26] C. R. Qi, H. Su, K. Mo, and L. J. Guibas, “PointNet:Deep Learning on Point Sets for 3D Classificationand Segmentation,” in The IEEE Conference on Com-puter Vision and Pattern Recognition (CVPR), 2017,pp. 1578–1584.