Generalized-ICPAleksandr V. SegalStanford University

Email: avsegal@cs.stanford.edu

Dirk HaehnelStanford University

Email: haehnel@stanford.edu

Sebastian ThrunStanford University

Email: thrun@stanford.edu

Abstract— In this paper we combine the Iterative ClosestPoint (’ICP’) and ‘point-to-plane ICP‘ algorithms into a singleprobabilistic framework. We then use this framework to modellocally planar surface structure from both scans instead of justthe ”model” scan as is typically done with the point-to-planemethod. This can be thought of as ‘plane-to-plane’. The newapproach is tested with both simulated and real-world data andis shown to outperform both standard ICP and point-to-plane.Furthermore, the new approach is shown to be more robust toincorrect correspondences, and thus makes it easier to tune themaximum match distance parameter present in most variants ofICP. In addition to the demonstrated performance improvement,the proposed model allows for more expressive probabilisticmodels to be incorporated into the ICP framework. Whilemaintaining the speed and simplicity of ICP, the Generalized-ICPcould also allow for the addition of outlier terms, measurementnoise, and other probabilistic techniques to increase robustness.

I. INTRODUCTION

Over the last decade, range images have grown in popularityand found increasing applications in fields including medicalimaging, object modeling, and robotics. Because of occlusionand limited sensor range, most of these applications requireaccurate methods of combining multiple range images into asingle model. Particularly in mobile robotics, the availabilityof range sensors capable of quickly capturing an entire 3Dscene has drastically improved the state of the art. A strikingillustration of this is the fact that virtually all competitors in theDARPA Grand Challenge relied on fast-scanning laser rangefinders as the primary input method for obstacle avoidance,motion planning, and mapping. Although GPS and IMUs areoften used to calculate approximate displacements, they are notaccurate enough to reliably produce precise positioning. In ad-dition, there are many situation (tunnels, parking garages, tallbuildings) which obstruct GPS reception and further decreaseaccuracy. To deal with this shortcoming, most applicationsrely on scan-matching of range data to refine the localization.Despite such wide usage, the typical approach to solving thescan-matching problem has remained largely unchanged sinceits introduction.

II. SCANMATCHING

Originally applied to scan-matching in the early 90s, theICP technique has had many variations proposed over thepast decade and a half. Three papers published around thesame time period outline what is still considered the stateof the art solution for scan-matching. The most often citedanalysis of the algorithm comes from Besl and McKay[1]. [1]directly addresses registration of 3D shapes described either

geometrically or with point clouds. Chen and Medioni[7]considered the more specific problem of aligning range datafor object modeling. Their approach takes advantage of thetendency of most range data to be locally planar and intro-duces the ”point-to-plane” variant of ICP. Zhang[5] almostsimultaneously describes ICP, but adds a robust method ofoutlier rejection in the correspondence selection phase of thealgorithm.

Two more modern alternatives are Iterative Dual Correspon-dence [15] and Metric-Based ICP [16]. IDC improves thepoint-matching process by maintaining two sets of correspon-dences. MbICP is designed to improve convergence with largeinitial orientation errors by explicitly putting a measure ofrotational error as part of the distance metric to be minimized.

The primary advantages of most ICP based methods aresimplicity and relatively quick performance when imple-mented with kd-trees for closest-point look up. The draw-backs include the implicit assumption of full overlap of theshapes being matched and the theoretical requirement that thepoints are taken from a known geometric surface rather thanmeasured [1]. The first assumption is violated by partiallyoverlapped scans (taken from different locations). The sec-ond causes problems because different discretizations of thephysical surface make it impossible to get exact overlap ofthe individual points even after convergence. Point-to-plane,as suggested in [7], solves the discretization problem by notpenalizing offsets along a surface. The full overlap assumptionis usually handled by setting a maximum distance thresholdin the correspondence.

Aside from point-to-plane, most ICP variations use a closedform solution to iteratively compute the alignment from thecorrespondences. This is typically done with [10] or similartechniques based on cross-correlation of the two data sets. Re-cently, there has been interest in the use of generic non-linearoptimization techniques instead of the more specific closedform approaches [9]. These techniques are advantageous inthat they allow for more generic minimization functions ratherthen just the sum of euclidean distances. [9] uses non-linearoptimization with robust statistics to show a wider basin ofconvergence.

We argue that among these, the probabilistic techniquesare some of the best motivated due to the large amount oftheoretical work already in place to support them. [2] appliesa probabilistic model by assuming the second scan is generatedfrom the first through a random process. [4] Applies raytracing techniques to maximize the probability of alignment.

[8] builds a set of compatible correspondences, and thenmaximizes probability of alignment over this distribution. [17]introduces a fully probabilistic framework which takes intoaccount a motion model and allows estimates of registrationuncertainty. An interesting aspect of the approach is that asampled analog of the Generalized Hough Transform is usedto compute alignment without explicit correspondences, takingboth surface normals into account for 2D data sets.

There is also a large amount of literature devoted to solvingthe global alignment problem with multiple scans ([18] andmany others). Many approaches to this ([18] in particular) usea pair-wise matching algorithm as a basic component. Thismakes improvements in pairwise matching applicable to theglobal alignment problem as well.

Our approach falls somewhere between standard IPC andthe fully probabilistic models. It is based on using MLEas the non-linear optimization step, and computing discretecorrespondences using kd-trees. It is unique in that it providessymmetry and incorporates the structural assumptions of [7].Because closest point look up is done with euclidean distance,however, kd-trees can be used to achieve fast performanceon large pointclouds. This is typically not possible with fullyprobabilistic methods as these require computing a MAPestimate over assignments. In contrast to [8], we argue thatthe data should be assumed to be locally planar since mostenvironments sampled for range data are piecewise smoothsurfaces. By giving the minimization processes a probabilisticinterpretation, we show that is easy to extend the technique toinclude structural information from both scans, rather then justone as is typically done in ”point-to-plane” ICP. We show thatintroducing this symmetry improves accuracy and decreasesdependence on parameters.

Unlike the IDC [15] and MbICP [16] algorithms, ourapproach is designed to deal with large 3D pointclouds. Evenmore fundamentally both of these approaches are somewhatorthogonal to our technique. Although MbICP suggests analternative distance metric (as do we), our metric aims totake into account structure rather then orientation. Since ourtechnique does not rely on any particular type (or number) ofcorrespondences, it would likely be improved by incorporatinga secondary set of correspondences as in IDC.

A key difference between our approach and [17] is thecomputational complexity involved. [17] is designed to dealwith planar scan data – the Generalized Hough Transform sug-gested requires comparing every point in one scan with everypoint in the other (or a proportional number of comparisonsin the case of sampling). Our approach works with kd-treesfor closest point look up and thus requires O(n log(n) explicitpoint comparisons. It is not clear how to efficiently generalizethe approach in [17] to the datasets considered in this paper.Furthermore, there are philosophical differences in the models.

This paper proceeds by summarizing the ICP and point-to-plane algorithms, and then introducing Generalized-ICPas a natural extension of these two standard approaches.Experimental results are then presented which highlight theadvantages of Generalized-ICP.

A. ICP

The key concept of the standard ICP algorithm can besummarized in two steps:

1) compute correspondences between the two scans.2) compute a transformation which minimizes distance

between corresponding points.

Iteratively repeating these two steps typically results in conver-gence to the desired transformation. Because we are violatingthe assumption of full overlap, we are forced to add a max-imum matching threshold dmax. This threshold accounts forthe fact that some points will not have any correspondence inthe second scan (e.g. points which are outside the boundary ofscan A). In most implementations of ICP, the choice of dmaxrepresents a trade off between convergence and accuracy. Alow value results in bad convergence (the algorithm becomes“short sighted”); a large value causes incorrect correspon-dences to pull the final alignment away from the correct value.Standard ICP is listed as Alg. 1.

input : Two pointclouds: A = {ai}, B = {bi}An initial transformation: T0

output: The correct transformation, T , which aligns Aand B

T ← T0;1

while not converged do2

for i← 1 to N do3

mi ← FindClosestPointInA(T · bi);4

if ||mi − T · bi|| ≤ dmax then5

wi ← 1;6

else7

wi ← 0;8

end9

end10

T ← argminT{∑i

wi||T · bi −mi||2};11

end12Algorithm 1: Standard ICP

B. Point-to-plane

The point-to-plane variant of ICP improves performance bytaking advantage of surface normal information. Originallyintroduced by Chen and Medioni[7], the technique has comeinto widespread use as a more robust and accurate variant ofstandard ICP when presented with 2.5D range data. Insteadof minimizing Σ||T · bi −mi||2, the point-to-plane algorithmminimizes error along the surface normal (i.e. the projectionof (T · bi − mi) onto the sub-space spanned by the surfacenormal). This improvement is implemented by changing line11 of Alg. 1 as follows:

T ← argminT{∑i

wi||ηi · (T · bi −mi)||2}

where ηi is the surface normal at mi.

III. GENERALIZED-ICP

A. Derivation

Generalized-ICP is based on attaching a probabilistic modelto the minimization step on line 11 of Alg. 1. The techniquekeeps the rest of the algorithm unchanged so as to reducecomplexity and maintain speed. Notably, correspondences arestill computed with the standard Euclidean distance rather thena probabilistic measure. This is done to allow for the use ofkd-trees in the look up of closest points and hence maintainthe principle advantages of ICP over other fully probabilistictechniques – speed and simplicity.

Since only line 11 is relevant, we limit the scope of thederivation to this context. To simplify notation, we assumethat the closest point look up has already been performedand that the two point clouds, A = {ai}i=1,...,N and B ={bi}i=1,...,N , are indexed according to their correspondences(i.e. ai corresponds with bi). For the purpose of this section,we also assume all correspondences with ||mi−T ·bi|| > dmaxhave been removed from the data.

In the probabilistic model we assume the existence ofan underlying set of points, A = {ai} and B = {bi},which generate A and B according to ai ∼ N (ai, CAi )and bi ∼ N (bi, CBi ). In this case, {CAi } and {CBi } arecovariance matrices associated with the measured points. Ifwe assume perfect correspondences (geometrically consistentwith no errors due to occlusion or sampling), and the correcttransformation, T∗, we know that

bi = T∗ai (1)

For an arbitrary rigid transformation, T, we define d(T)i =

bi − Tai, and consider the distribution from which d(T∗)i

is drawn. Since ai and bi are assumed to be drawn fromindependent Gaussians,

d(T∗)i ∼ N (bi − (T∗)ai, CBi + (T∗)CAi (T∗)T )

= N (0, CBi + (T∗)CAi (T∗)T )

by applying Eq. (1).Now we use MLE to iteratively compute T by setting

T = argmaxT

∏i

p(d(T)i ) = argmax

T

∑i

log(p(d(T)i ))

The above can be simplified to

T = argminT

∑i

d(T)i

T(CBi + TCAi TT )−1d

(T)i (2)

This defines the key step of the Generalized-ICP algorithm.The standard ICP algorithm can be seen as a special case

by setting

CBi = I

CAi = 0

In this case, (2) becomes

T = argminT

∑i

d(T)i

Td(T)i

= argminT

∑i

||d(T)i ||

2(3)

which is exactly the standard ICP update formula.With the Generalized-IPC framework in place, however, we

have more freedom in modeling the situation; we are freeto pick any set of covariances for {CAi } and {CBi }. As amotivating example, we note that the point-to-plane algorithmcan also be thought of probabilistically.

The update step in point-to-plane ICP is performed as:

T = argminT

{∑i

||Pi · di||2} (4)

where Pi is the projection onto the span of the surface normalat bi. This minimizes the distance of T · ai from the planedefined by bi and its surface normal. Since Pi is an orthogonalprojection matrix, Pi = Pi

2 = PiT . This means ||Pi · di||2

can be reformulated as a quadratic form:

||Pi · di||2 = (Pi · di)T · (Pi · di)= dTi ·Pi · di

Looking at (4) in this format, we get:

T = argminT

{∑i

dTi ·Pi · di} (5)

Observing the similarity between the above and (2), it canbe shown that point-to-plane ICP is a limiting case ofGeneralized-ICP. In this case

CBi = Pi−1 (6)

CAi = 0 (7)

Strictly speaking Pi is non-invertible since it is rank defi-cient. However, if we approximate Pi with an invertible Qi,Generalized-ICP approaches point-to-plane as Qi → Pi. Wecan intuitively interpret this limiting behavior as bi beingconstrained along the plane normal vector with nothing knownabout its location inside the plane itself.

B. Application: plane-to-plane

In order to improve performance relative to point-to-planeand increase the symmetry of the model, Generalized-ICP canbe used to take into account surface information from bothscans. The most natural way to incorporate this additionalstructure is to include information about the local surface ofthe second scan into (7). This captures the intuitive natureof the situation, but is not mathematically feasible since thematrices involved are singular. Instead, we use the intuition ofpoint-to-plane to motivate a probabilistic model.

The insight of the point-to-plane algorithm is that our pointcloud has more structure then an arbitrary set of points in3-space; it is actually a collection of surfaces sampled bya range-measuring sensor. This means we are dealing with

Fig. 1. illustration of plane-to-plane

a sampled 2-manifold in 3-space. Since real-world surfacesare at least piece-wise differentiable, we can assume that ourdataset is locally planar. Furthermore, since we are samplingthe manifold from two different perspectives, we will not ingeneral sample the exact same point (i.e. the correspondencewill never be exact). In essence, every measured point onlyprovides a constraint along its surface normal. To model thisstructure, we consider each sampled point to be distributedwith high covariance along its local plane, and very lowcovariance in the surface normal direction. In the case of apoint with e1 as its surface normal, the covariance matrixbecomes ε 0 0

0 1 00 0 1

where ε is a small constant representing covariance along thenormal. This corresponds to knowing the position along thenormal with very high confidence, but being unsure about itslocation in the plane. We model both ai and bi as being drawnfrom this sort of distribution.

Explicitly, given µi and νi – the respective normal vectors atbi and ai – CBi and CAi are computed by rotating the abovecovariance matrix so that the ε term represents uncertaintyalong the surface normal. Letting Rx denote one of therotations which transform the basis vector e1 → x, set

CBi = Rµi·

ε 0 00 1 00 0 1

·RTµi

CAi = Rνi ·

ε 0 00 1 00 0 1

·RTνi

The transformation, T, is then computed via (2).Fig. 1 provides an illustration of the effect of the algorithm

in an extreme situation. In this case all of the points along thevertical section of the green scan are incorrectly associatedwith a single point in the red scan. Because the surfaceorientations are inconsistent, plane-to-plane will automaticallydiscount these matches: the final summed covariance matrixof each correspondence will be isotropic and will form a verysmall contribution to the objective function relative to the thinand sharply defined correspondence covariance matrices. Analternative view of this behavior is as a soft constraint for eachcorrespondence. The inconsistent matches allow the red scan-point to move along the x-axis while the green scan-points are

free to move along the y-axis. The incorrect correspondencesthus form very weak and uninformative constraints for theoverall alignment.

Computing the surface covariance matrices requires a sur-face normal associated with every point in both scans. Thereare many techniques for recovering surface normals from pointclouds, and the accuracy of the normals naturally plays animportant role in the performance of the algorithm. In ourimplementation, we used PCA on the covariance matrix of the20 closest points to each scan point. In this case the eigen-vector associated with the smallest eigenvalue correspondswith the surface normal. This method is used to computethe normals for both point-to-plane and Generalized-ICP. ForGeneralized-ICP, the rotation matrices are constructed so thatthe ε component of the variance lines up with the surfacenormal.1

IV. RESULTS

We compare all three algorithms to test performance of theproposed technique. Although efficient closed form solutionsexist for T in standard ICP, we implemented the minimizationwith conjugate gradients to simplify comparison. Performanceis analyzed in terms of convergence to the correct solution aftera known offset is introduced between the two scans. We limitour tests to a maximum of 250 iterations for standard ICP, and50 iterations for the other two algorithms since convergencewas typically achieved before this point (if at all).

Both simulated (Fig. 3) and real (Fig. 4) data was used in or-der to demonstrate both theoretical and practical performance.The simulated data set also allowed tests to be performed ona wider range of environments with absolutely known groundtruth. The outdoor simulated environment differs from thecollected data primarily in the amount of occlusion presented,and in the more hilly features of the ground plane. The real-world outdoor tests also demonstrate performance with moredetailed features and more representative measurement noise.

Simulated data was generated by ray-tracing a SICK scannermounted on a rotating joint. Two 3D environments werecreated to test performance against absolute ground truth bothin the indoor (Fig. 2(a)) and an outdoor (Fig. 2(b)) scenario.The indoor environment was based on an office hallway,while the outdoor setting reflects a typical landscape around abuilding. In both cases, we simulated a laser-scanner equippedrobot traveling along a trajectory and taking measurements atfixed points along the path. Gaussian noise was added to makethe tests more realistic.

Tests were also performed on real data from the logs ofan instrumented car. The logs included data recorded by aroof-mounted Velodyne range finder as the car made a loopthrough a suburban environment and were annotated with GPSand IMU data. This made it possible to apply a pairwiseconstraint-based SLAM technique to generate ground truth

1In our implementation we compute these transformations by consideringthe eigen decomposition of the empirical covariance of the 20 closest points,Σ = UDUT . We then use U in place of the rotation matrix (in effectreplacing D with diag(ε, 1, 1) to get the final surface-aligned matrix).

(a) indoor scene

(b) outdoor scene

Fig. 2. simulated 3D environments

(a) indoor scene

(b) outdoor scene

Fig. 3. ray-traced scans – scan A is shown in green, scan B in red

Fig. 4. Velodyne scans – scan A is shown in green, scan B in red

positioning. Although (standard) ICP itself was used in thepairwise matching to generate the ground truth, the spacing ofscans used for the SLAM approach was an order of magnitudesmaller. In contrast, the scan pairs used for testing wereextracted with much higher spacing (15-20+ meters) in orderto pose a much more challenging problem. This is not aperfect method to generate ground truth, but we believe itprovides a reasonable baseline to make comparisons betweenthe algorithms.

To measure performance, all algorithms were run on pairsof scans from each of the three data sets. For each scanpair, the initial offset was set to the true offset with auniformly generated error term added. The error term wasset within ±1.5m and ±15◦ along all axes. Performance wasmeasured by averaging positioning error over all scan pairsfor a particular algorithm. In all cases tested, rotational errorwas negligible.

As mentioned before, selection of dmax plays an importantrole in the convergence of ICP. Fig. 5 shows the averageerror for different values of dmax; the plot shows averageperformance across all scan pairs. Fig. 9 shows the averagesfor individual scan pairs based on ideal values of dmax; itdemonstrates the distribution of error across the range of scanpairs. In contrast to Fig. 5, the large number off random initialoffsets averaged into each data point of Fig. 9 serves to samplethe space of possible offsets. For Fig. 5, the algorithms wererun on each scan pair with 10 randomly generated startingpositions. For the plots in Fig. 9, each data point was generatedwith 50 random initial poses using best-case values for dmax.In all cases, error bars were computed as σ√

N.

The plots in Fig. 5 show that the proposed algorithm ismore robust to choice of the matching threshold and demon-strates better performance in general. This is to be expectedsince it more completely models the environment and willautomatically discount many incorrect matches based on thestructure of the scene. In particular, Fig. 5 shows that in thesimulated environments, the accuracy of the algorithm is notsensitive to overestimated values of dmax. For the real data,Generalized-ICP is still shown to be less sensitive due to thesmaller slope of average error as dmax →∞. The discrepancybetween simulated and real data can be explained by thedifference in their respective frequency profiles. Whereas thesimulated environments only have high-level features modeled

0

0.2

0.4

0.6

0.8

1

1.2

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

aver

age

erro

r [m

]

max. match distance [m]

Simulated Hallway with 15,000 points, 1cm noise

Generalized ICPPoint-to-plane ICP

Standard ICP

(a) simulated indoor

0

0.2

0.4

0.6

0.8

1

1.2

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

aver

age

erro

r [m

]

max. match distance [m]

Simulated Outdoor with 15,000 points, 1cm noise

Generalized ICPPoint-to-plane ICP

Standard ICP

(b) simulated outdoor

0

0.5

1

1.5

2

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

aver

age

erro

r [m

]

max. match distance [m]

Velodyne Data with 15,000 points

Generalized ICPPoint-to-plane ICP

Standard ICP

(c) Velodyne data

Fig. 5. average error as a function of dmax

(a) Initial alignment (b) Point-to-plane (c) Generalized-ICP

Fig. 6. Example of results for velodyne scan pair #31

(a) Initial alignment (b) Point-to-plane (c) Generalized-ICP

Fig. 7. Example of results for velodyne scan pair #45

(a) scan pair #31, view 1 (b) scan pair #31, view 2 (c) scan pair #45, view 1 (d) scan pair #45, view 2

Fig. 8. Velodyne scan pairs #31 and #45 shown in perspective to illustrate scene complexity

0

0.1

0.2

0.3

0.4

0.5

1 2 3 4 5 6

aver

age

erro

r [m

]

Scan pair #

Simulated Hallway with 15,000 points, 1cm noise

Generalized ICP, dmax=5.0mPoint-to-plane ICP, dmax=1.6m

Standard ICP, dmax=1.4m

(a) simulated indoor

0

0.1

0.2

0.3

0.4

0.5

2 4 6 8 10 12 14av

erag

e er

ror

[m]

Scan pair #

Simulated Outdoor with 15,000 points, 1cm noise

Generalized ICP, dmax=1.6mPoint-to-plane ICP, dmax=1.0m

Standard ICP, dmax=1.8m

(b) simulated outdoor

0

0.2

0.4

0.6

0.8

1

1.2

1.4

5 10 15 20 25

aver

age

erro

r [m

]

Scan pair #

Velodyne Data with 15,000 points

Generalized ICP, dmax=1.2m Point-to-plane ICP, dmax=1.0m Standard ICP, dmax=1.0m

(c) Velodyne data #1 - #26

0

0.2

0.4

0.6

0.8

1

1.2

1.4

30 35 40 45 50

aver

age

erro

r [m

]

Scan pair #

Velodyne Data with 15,000 points

Generalized ICP, dmax=1.2m Point-to-plane ICP, dmax=1.0m Standard ICP, dmax=1.0m

(d) Velodyne data #27 - #51

Fig. 9. average error with ideal values of dmax which minimize Fig. 5

by hand, the real world data contains much more detailed,high-frequency data. This increases the chances of incorrectcorrespondences which share a common surface orientation –a situation which is not taken into account by our algorithm.Nonetheless, even when comparing worst-cast values of dmaxfor Generalized-ICP with best-case values for point-to-plane,Generalized-ICP performs roughly as good.

As mentioned in Section II, the dmax plays an importantrole in the performance of ICP. Setting a low value decreasesthe chance of convergence, but increases accuracy. Setting avalue which is too high increases the radius of convergence,but decreases accuracy since more incorrect correspondencesare made. The algorithm proposed in this paper heavilyreduces the penalty of picking a large value of dmax by dis-counting the effect of incorrect correspondences. This makes iteasier to get good performance in a wide range of environmentwithout hand-picking a value of dmax for each one.

In addition to the increased accuracy, the new algorithmgives equal consideration to both scans when computing thetransformation. Fig. 6 and Fig. 7 show two situations whereusing the structure of both scans removed local minima whichwere present with point-to-plane. These represent top-downviews of velodyne scans recorded approximately 30 metersapart and aligned. Fig. 8 shows some additional views of thesame scan pairs to better illustrate the structure of the scene.The scans cover a range of 70-100 meters from the sensorin an outdoor environment as seen from a car driving on theroad.

Because this minimization is still performed within the ICPframework, the approach combines the speed and simplicityof the standard algorithm with some of the advantages offully probabilistic techniques such as EM. The theoreticalframework also allows standard robustness techniques to beincorporated. For example, the Gaussian kernel can be mixedwith a uniform distribution to model outliers. The GaussianRVs can also be replaced by a distribution which takesinto account a certain amount of slack in the matching toexplicitly model the inexact correspondences (by assigning thedistribution of d(T)

i a constant density on some region around0). Although we have considered some of these variations,none of them have an obvious closed form which is easilyminimized. This makes them too complex to include in thecurrent work, but a good topic for future research.

V. CONCLUSION

In this paper we have proposed a generalization of theICP algorithm which takes into account the locally planarstructure of both scans in a probabilistic model. Most of theICP framework is left unmodified so as to maintain the speedand simplicity which make this class of algorithms popularin practice; the proposed generalization only deals with theiterative computation of the transformation. We assume allmeasured points are drawn from Gaussians centered at the truepoints which are assumed to be in perfect correspondence.MLE is then used to iteratively estimate transformation foraligning the scans. In a range of both simulated and real-world

experiments, Generalized-ICP was shown to increase accuracy.At the same time, the use of structural information from bothscans decreased the influence of incorrect correspondences.Consequently the choice of maximum matching distance as aparameter for the correspondence phase becomes less criticalto performance. These modifications maintain the simplicityand speed of ICP, while improving performance and removingthe trade off typically associated with parameter selection.

ACKNOWLEDGMENT

This research was supported in part under subcontractthrough Raytheon Sarcos LLC with DARPA as prime sponsor,contract HR0011-04-C-0147.

REFERENCES

[1] P. Besl, N. McKay. ”A Method for Registration of 3-D Shapes,” IEEETrans. on Pattern Analysis and Machine Intel., vol. 14, no. 2, pp. 239-256,1992.

[2] P. Biber, S. Fleck, W. Strasser. ”A Probabilistic Framework for Robustand Accurate Matching of Point Clouds,” Pattern Recognition, LectureNotes in Computer Science, vol. 3175/2004, pp. 280-487, 2004.

[3] N. Gelfan, L. Ikemoto, S. Rusinkiewicz, M. Levoy. ”Geometrically StableSampling for the ICP Algorithm,” Fourth International Conference on 3-DDigital Imaging and Modeling, p. 260, 2003.

[4] D. Haehnel, W. Burgard. ”Probabilistic Matching for 3D Scan Registra-tion,” Proc. of the VDI-Conference Robotik, 2002.

[5] Z. Zhang. ”Iterative Point Matching for Registration of Free-FormCurves,” IRA Rapports de Recherche, Programme 4: Robotique, Imageet Vision, no. 1658, 1992.

[6] D. Hahnel, W. Burgard, S. Thrun. ”Learning compact 3D models ofindoor and outdoor environments with a mobile robot,” Robotics andAutonomous Systems, vol. 44, pp. 15-27, 2003.

[7] Y. Chen, G. Medioni. ”Object Modeling by Registration of MultipleRange Images,” Proc. of the 1992 IEEE Intl. Conf. on Robotics andAutomation, pp. 2724-2729, 1991.

[8] L. Montesano, J. Minguez, L. Montano. ”Probabilistic Scan Matching forMotion Estimation in Unstructured Environments,” IEEE Intl. Conf. on.Intelligent Robots and Systems, pp. 3499-3504, 2005.

[9] A. Fitzgibbon. ”Robust registration of 3D and 3D point sets,” Image andVision Computing, vol. 21, no. 13-14, pp. 1145-1153, 2003.

[10] B. Horn. ”Closed-form solution of absolute orientation using unitquaternions,” Journal of the Optical Society of America A, vol. 4, pp.629-642, 1987.

[11] S. Rusinkiewicz, M. Levoy. ”Efficient Variants of the ICP Algorithm,”Third International Conference on 3-D Digital Imaging and Modeling, p.145, 2001.

[12] G. Dalley, P. Flynn. ”Pair-Wise Range Image Registration: A Study inOutlier Classification,” Computer Vision and Image Understanding, vol.87, pp. 104-115, 2002.

[13] S. Kim , Y. Hwang , H. Hong , M. Choi. ”An Improved ICP AlgorithmBased on the Sensor Projection for Automatic 3D Registration,” LectureNotes in Computer Science, vol. 2972/2004 pp. 642-651, 2004.

[14] J.-S. Gutmann, C. Schlegel, ”AMOS: comparison of scan matchingapproaches for self-localization in indoor environments,” eurobot, p.61,1st Euromicro Workshop on Advanced Mobile Robots (EUROBOT),1996.

[15] F. Lu, E. Milos. ”Robot Pose Estimation in Unknown Environments byMatching 2D Range Scans,” Journal of Intelligent Robotics Systems 18:pp. 249-275, 1997.

[16] J. Minguez, F. Lamiraux, L. Montesano. ”Metric-Based Scan MatchingAlgorithms for Mobile Robot Displacement Estimation,” Robotics andAutomation, Proceedings of the 2005 IEEE International Conference on,pp. 3557-3563, 2005.

[17] A. Censi, ”Scan matching in a probabilistic framework,” Robotics andAutomation, Proceedings of the 2006 IEEE International Conference on,pp. 2291-2296, 2006.

[18] K. Pulli, ”Mutliview Registration for Large Data Sets,” 3-D DigitalImaging and Modeling, 1999. Proceedings. Second International Con-ference on, pp. 160-168, 1999.