3D Scan Registration Using the Normal Distributions Transform with

Ground Segmentation and Point Cloud Clustering

Arun Das,∗ James Servos,∗ and Steven L. Waslander†University of Waterloo, Waterloo, ON, Canada, N2L 3G1

Abstract— The Normal Distributions Transform (NDT) scanregistration algorithm models the environment as a set ofGaussian distributions and generates the Gaussians by discretiz-ing the environment into voxels. With the standard approach,the NDT algorithm has a tendency to have poor convergenceperformance for even modest initial transformation error. Inthis work, a segmented greedy cluster NDT (SGC-NDT) variantis proposed, which uses natural features in the environmentto generate Gaussian clusters for the NDT algorithm. Bysegmenting the ground plane and clustering the remainingfeatures, the SGC-NDT approach results in a smooth andcontinuous cost function which guarantees that the optimizationwill converge. Experiments show that the SGC-NDT algorithmresults in scan registrations with higher accuracy and betterconvergence properties when compared against other state-of-the-art methods for both urban and forested environments.

I. INTRODUCTION

Scan registration algorithms have a significant role in map-ping algorithms for mobile robotics. Many sensors provideinformation about the environment in the form of pointclouds, such as RGBD cameras, LIDAR and stereo cameras.Scan registration algorithms rely on overlapping geometrybetween scans to determine the relative transformation be-tween the two scans. Knowing the transformation parametersallows for the aggregation of point cloud data, which iscommonly required for autonomous operation in unknownenvironments.

The most common scan registration algorithm is the iter-ative closest point (ICP) algorithm. ICP was independentlyintroduced by Besl and McKay [1], Chen and Medioni [2],and Zhang [3]. The ICP algorithm attempts to find transformparameters such that the Euclidean distance between nearestneighbour points are minimized. The ICP algorithm does nottake into account the underlying surface of the point cloud,as it operates at the point level. Furthermore, ICP assumesthat the two point clouds can overlap perfectly, however inpractice this is not the case due to noise and the samplingcharacteristics of the sensor. To address the shortcomingsof ICP, Segal et al. introduced generalized-ICP (G-ICP) [4],which calculates the surface normal at each point, using localneighbourhoods, and only includes correspondences betweenpoints of similar surface structure between the two scans.The main bottleneck of the ICP algorithm is the costlynearest neighbour search needed to identify points in the

* M.A.Sc. Candidate, Mechanical and Mechatronics Engineering, Uni-versity of Waterloo; adas@uwaterloo.ca, jdservos@uwaterloo.ca

† Assistant Professor, Mechanical and Mechatronics Engineering, Uni-versity of Waterloo; stevenw@uwaterloo.ca

local neighbourhoods, which is generally computationallyexpensive.

The normal distributions transform (NDT) is a alternativeapproach, first suggested by Biber and Strasser for scanregistration and mapping in 2D [5]. It was later expandedfor use with 3D point clouds by Magnusson et al. [6], andfurther expanded to a distribution-to-distribution registrationproblem by Stoyanov et al. [7]. The advantage of the NDTalgorithm is that it does not need to explicitly compute thenearest neighbour correspondences because it represents thescan as a set of Gaussian distributions instead of as individualpoints. The NDT algorithm also represents the underlyingscan as a set of Gaussian distributions that locally modelsthe surface as a generative distribution. It is an optimizationbased approach that determines the transform parameterswhich maximize the likelihood of the scan being sampledfrom the NDT distribution.

The standard formulation of NDT results in a nonlin-ear optimization which is susceptible to poor convergenceproperties when starting from initial estimates that deviatesignificantly from the true transformation. The voxel baseddiscretization of NDT generates Gaussian distributions whichdo not necessarily model the environment accurately. Thedistributions only locally model the points within each cell,and may not capture broader features present within the scan.Furthermore, the crossing of points between cell boundariesduring the optimization process results in a cost functionwhich is discontinuous. Previously, the authors [8] proposedmulti scale k-means NDT (MSKM-NDT), which partitionsa 2D laser scan using k-means clustering over multiplescales with a course-to-fine optimization method. Althoughthe MSKM-NDT method avoids local minima, improvesconvergence, and results in a smooth and continuous costfunction, the number of clusters must be known a priori.

Scan partitioning has also been performed in 3D by Moos-mann et al., who used a graph based approach to segmenta laser scan based on local convexity criterion [9]. Thegraph based method computes surface normal informationin order to partition the scan, however good quality surfacenormal information is difficult to generate for sparse laserscans, or when the environment consists of objects suchas trees, brush, and foliage. Douillard et al. demonstratedscan partitioning using Gaussian processes and incrementalsample consensus applied to ICP scan registration, wherecorrespondences between neighbouring points were con-strained to belong to corresponding partitions [10].

The contribution of this work is the application of a seg-

mentation and clustering algorithm to improve the accuracyand convergence of the NDT algorithm, specifically for open,outdoor environments with sparse point-clouds. Ground seg-mentation is performed in order to separate natural features inthe environment which are typically separated by the ground.The natural features which represent the environment areused to generate Gaussian clusters for the NDT algorithm,which is desirable since it allows for accurate modellingof the environment using significantly fewer distributionscompared to the standard NDT algorithm. The applicationof ground segmentation results in the decomposition of theenvironment into ground and obstacle partitions which canbe used for higher level mission execution.

To achieve the decomposition, a ground segmentationalgorithm based on Gaussian process regression [11] andincremental sample consensus [12] is applied to the 3D laserscan. A greedy clustering algorithm which partitions the non-ground points into clusters is then proposed. Assuming thatthe non-ground points are fully separated by the ground,the greedy clustering algorithm generates clusters whichgroup together points belonging to segmented features inthe environment, such as trees, bushes, etc. The segmentedfeature clusters are used to construct Gaussian distributionsfor the NDT algorithm. The NDT algorithm is modified toevaluate the cost function for all generated clusters, whichguarantees that the nonlinear optimization is well definedand all standard nonlinear optimization algorithms will con-verge to a solution which satisfies the necessary conditionsfor optimality [13]. Evaluation at all Gaussian clusters isperformed for both the point-to-distribution and distribution-to-distribution variants of the NDT algorithm. The proposedmethod is evaluated for accuracy and convergence using laserdata obtained from the Ford campus data set [14] and a sparseforested park at Worcester Polytechnic Institute.

II. PROBLEM FORMULATION

Scan registration algorithms seek to find the optimal trans-formation between two point cloud scans, a reference scanand a scene scan. The goal is to determine transformationparameters which best align the two scans such that theyoverlap as much as possible. Denote the reference scan asthe set of points Y = {y1, . . . , yNY

} where yi ∈ R3 for

i ∈ {1, . . . , NY } and the scene scan as X = {x1, . . . , xNX}

where xj ∈ R3 for j ∈ {1, . . . , NX}. A point xj consists

of three components, xj = {xxj , xyj , xzj}, which refer tothe x, y, and z components of the point, respectively. Thetransformation that maps a point x from the scene scan tothe coordinate frame of the reference scan using a parameterestimate, p = [tx, ty, tz, φ, θ, ψ]

T ∈ R3 × SO(3), is given as

T (p, x) =[RφRθRψ

]x+ [tx ty tz]

T

where Rφ is the rotation matrix for a rotation about the xaxis by angle φ, Rθ is the rotation matrix for a rotation aboutthe y axis by angle θ, Rψ is the rotation matrix for a rotationabout the z axis by angle ψ, and [tx, ty, tz]

T is the translationvector between the two frames.

III. PROPOSED METHOD

In order to improve the accuracy and convergence basinof the NDT algorithm, the Segmented Greedy Cluster NDT(SGC-NDT) method is proposed. The SGC-NDT methodsegments the ground points and performs clustering onthe remaining points. Segmenting of the ground point andapplication of the greedy cluster algorithm is applied toboth the point-to-distribution and distribution-to-distributionregistration method for NDT.

The NDT algorithm is modified in three ways:

1) The ground points of the scan are removed usingthe ground segmentation method described in SectionIV. In environments such as a large outdoor field,the collection of ground points are redundant and donot provide as much information for the purpose ofscan registration as other environmental features suchas large trees, fences, etc. Scan point density alsobecomes an issue as the ground points very close tothe laser sensor are far denser than those farther away,which is not desirable for NDT as the high density ofpoints near the vehicle will have a tendency to gen-erate Gaussian distributions biased towards the sensororigin. The biased distribution effect can be somewhatmitigated using a down-sampling voxel based filter,however the filtering adds additional computation tothe scan registration and removes information fromthe entire scan. Although down sampling the scancan mitigate the biased distribution issue for NDT, itdoes not solve the problem of the discontinuous costfunction.

2) The non-ground points are clustered using the greedyclustering method described in Section V. By per-forming the clustering step, natural features in theenvironment, such as trees and bushes, are clusteredto form the Gaussian distributions for SGC-NDT. Incontrast, NDT performs a voxel based discretization,generating truncated Gaussian distributions which donot necessarily correctly model the environment.

3) To accommodate the greedy clustering style of clus-tering of the scan, the NDT cost calculation is mod-ified as described in Section VII. NDT evaluates theGaussian distribution within the cell that a transformedpoint falls into. The SGC-NDT algorithm evaluatesthe transformed point at all the Gaussian distribu-tions for the clusters identified by the greedy clus-tering method. Similarly for distribution-to-distributionmatching NDT, each Gaussian distribution in the scenescan is scored against all Gaussian distributions fromthe reference scan. Evaluation at all Gaussian compo-nents is computationally feasible, since the number ofclusters identified in the scan is typically significantlysmaller than the number of Gaussian distributions thatwould result from voxel based discretization. Calcu-lating the cost based on all Gaussian distributions inthe scans allows for the strongest contributions to comefrom the most likely clusters and provides a continuous

and differentiable cost function for the optimization.

IV. GROUND SEGMENTATION

The basis for the ground segmentation algorithm usedfor this work was first demonstrated by Douillard et al.[12]. Their method performs ground segmentation basedon a 2D Gaussian process (GP) model of the sparse pointclouds, which provides a probabilistic framework to iden-tify ground points using an incremental sample consensus(INSAC) method. A modified version of Douillard’s groundsegmentation algorithm was introduced by Tongtong et al.[15]. To reduce computation, Tongtong divides the scaninto sectors based on a polar grid binning, and applies theground segmentation to each sector separately. The Gaussianprocess regression is formulated as an approximation basedon the points contained in each sector. Tongtong’s methodwas shown to run faster than the 2D method, with compa-rable results in segmentation quality. The 1D approximationmethod for ground segmentation from [15] is used in thiswork, and is briefly summarized in this section.

In order to perform the polar grid binning, the xy planeof the laser scan, in the laser frame, is first segmented intoNs angular sectors, where the ith angular sector is denotedas ξi. Each sector is further sub-divided into Nb linear rangebased bins, which subdivides a sector as ξi = {ξ1i , . . . , ξNb

i }.Finally, the points located within the jth bin of the ith sector,from scan Y , are denoted as (sji )

Y = {y ∈ Y : y ∈ ξji } forall i ∈ {1 . . . Ns} and for all j ∈ {1 . . . Nb}. Denote theset SY as the collection of all individual point sets (sji )

Y ,and the function which accepts a scan Y and returns thecollection of polar binned grid sets, SY , as SY = τ(Y ).

The goal of ground segmentation for the 3D point cloudis to assign each point as either belonging to the ground ornot ground. In this work, ground segmentation is done byperforming Gaussian Process (GP) regression. In the caseof terrain modelling, GP’s are non-parametric tools whichcan be used to learn a model for the ground terrain. For athorough exploration of the usage of GP’s for ground terrainmodelling, the reader is directed to the work of Vasudevan etal. [11]. In this work, the approximate GP regression methodas described in [15] is applied. An example of the groundsegmentation results for a representative laser scan is given inFigure 1. Denote the function which accepts a set of radialbins, SY and returns the non-ground points, Y ′, as Y ′ ⊂Y = χ(SY ).

V. CLUSTERING OF THE NON-GROUND POINTS

Once the ground points have been segmented, the remain-ing non-ground points must be clustered in order to applythe SGC-NDT algorithm, which is achieved using a greedyclustering technique. For a scan Y , denote a cluster as ω ⊂Y , and the set of clusters as ΩY = {ω1 . . . ωNΩ

}, where NΩ

is the total number of clusters, which is initially unknown.The clusters are generated by first randomly sampling aninitial bin sji ∈ SY from a random uniform distribution U ,such that sji ∼ U(SY ). Note that SY is constructed fromthe polar binning of the non ground points from the 3D

Fig. 1. Ground segmentation results for an outdoor scene with large treesand shrubs. Blue points have been classified as ground, and red points areclassified as non-ground.

laser scan. Denote a nearest neighbour function ΘSY, which

returns the set of eight neighbouring bins from sji as the set{sj+qi+p}\sji for all p ∈ {−1, 0, 1} and q ∈ {−1, 0, 1}.

The nearest neighbours for sji are generated and addedto an ordered list of nearest neighbour bins, Qnn. A bin inQnn can be compared against sji to determine if the twocan be combined. Merging of the bins is decided using theΦ(sji , Q

knn) function, where Φ : SY × SY �→ {0, 1}, and

Qknn is the kth element of the list Qnn. The Φ functionreturns 1 if the two bins can be combined, and 0 otherwise.In the Φ function, different metrics can be used to comparethe point bins, such as Euclidean distance between the meanof the points contained in the bins, the L2 distance betweenGaussian distributions within each bin, or a test for Gaussianfit. If the bins can be combined, the neighbour bin is addedto an open list of bins, Qo. The neighbour bin is alsoremoved from the set of all bins, SY , and added to thecurrent cluster, Ccur. The nearest neighbours of the binsfrom Qo are explored and evaluated. When a bin is explored,its nearest neighbours are also added to Qo based on theevaluation function Φ. Once Qo is empty, it means no furthernearest neighbours can be assigned to the current cluster soa new bin from SY is selected, and the process is repeated.The entire process continues until all bins in SY have beenassigned to a cluster. An example of the resulting Gaussiandistributions after the greedy clustering has been completedis presented in Figure 2. Denote the function which acceptsnon ground points, Y ′, and returns the set of clusters ΩY ,as ΩY = β(Y ′).

VI. THE NORMAL DISTRIBUTIONS TRANSFORM

The normal distributions transform is a method by whichsections of a point cloud are mapped and represented asGaussian distributions within a grid structure. The NDT reg-istration algorithm begins by subdividing the space occupiedby the reference scan into a set of grid cells, c. Denotethe collection of reference scan points in cell ci as Y ci ={yci1 , . . . , yciNci

} such that ycik ∈ ci for k ∈ {1, . . . , Nci}.The distribution of points within each cell, ci, are modelledas Gaussian with a mean, μci , and a covariance matrix, Σci .The Gaussian distribution represents a generative process that

Fig. 2. The resulting Gaussian distributions after performing the greedycluster algorithm on the non-ground point laser scan. One-sigma Gaussiandistributions are shown (best viewed in colour).

models the local surface points Y ci within the cell.Since the point cloud is modelled by a piecewise contin-

uous and piecewise differentiable summation of Gaussians,numerical optimization tools can be used in order to registerthe scene scan with the reference scan. A fitness cost, Λ :R

3 × SO(3) → R, can be calculated which quantifies themeasure of overlap between scans X and Y . The originalwork by Magnusson calculates the cost by evaluating eachpoint in the transformed scan at the distribution correspond-ing to the NDT cell it occupies [16]. The score contributionfor each point is given as

ρ(y) = exp(− (y − μci)

TΣ−1ci (y − μci)

2

)

where μci and Σci are the sample mean and covariance forcell, ci. The sum of the score components for all points in thescan is known as the NDT point-to-distribution (p2d) cost,and is given as

Λ(p) = −Nx∑k=1

ρY (T (p, xk)).

Stoyanov et al. introduced a modified version of NDTwhich generates an NDT representation of both scans, thencompares the distributions of the scans in order to align them[7]. The distribution-to-distribution cost function evaluatesall pair-wise Gaussian components for both the scene andthe reference scan. In Stoyanov’s work however, the costfunction is only evaluated at the nearest Gaussian componentin order to reduce computation, which causes discontinuitiesin the overall cost function as the gradient and Hessian to notexist in the transformation space when the correspondencesbetween mean points change. Discontinuities in the costfunction are also present in point-to-distribution NDT, whenscene scan points cross cell boundaries. Magnusson et al.address the cost discontinuity using a tri-linear interpolationscheme [17], however, interpolation does not fully solve theissue, as the cost functions are still discontinuous.

It should be noted that both NDT cost functions haveanalytic expressions available for calculation of the gradient,ηg , and Hessian, ηH . For point to distribution matching,

denote the functions ρg(xk, p), and ρH(xk, p), which usea point in the scene scan and the transformation parametersto calculate an associated gradient and Hessian contributionfor point xk. The optimization is initialized from a parameterestimate, p0, and terminates once the norm of the gradient‖ηg‖ ∈ R is less than a user specified threshold, ε. Stoyanovsolves the optimization using a quasi-Newton approach withline search [7], however other approaches such as a Broyden-Fletcher-Goldfarb-Shanno (BFGS) update method can beused.

VII. SEGMENTED GREEDY CLUSTER NDTUsing the Gaussian distributions generated by the greedy

clustering, the NDT algorithm is modified to evaluate the costfunction using all of the Guassians. A summary of the SGC-NDT-P2D is given in Algorithm 1. For SGC-NDT-D2D, amethod similar to the P2D version is applied, except in theD2D case both laser scans are segmented and clustered, andthe set of resulting Gaussian distributions for each scan arecompared to perform the registration.

Algorithm 1 Register scene scan X with reference scan Yusing SGC-NDT-P2D algorithm

1: SX ← τ(X)2: SY ← τ(Y )3: X ′ ← χ(SX)4: Y ′ ← χ(SY )5: ΩY ← β(Y ′)6: p← p07: while ‖ηg‖ > ε do

8: Λ ← 09: ηg ← 0

10: ηH ← 011: for all xk ∈ X ′ do

12: xk ← T (p, xk)13: for all ωj ∈ Ωy do

14: Λ ← Λ + ρ(xk)ωj

15: ηg ← ηg + ρg(xk, p)ωj

16: ηH ← ηH + ρH(xk, p)ωj

17: end for

18: end for

19: Δp← (ηH)−1(ηg)20: p← p+Δp21: end while

VIII. EXPERIMENTAL RESULTS

The SGC-NDT approach is evaluated using two data setsof different environments and is compared against other scanregistration algorithms. The algorithms used for comparisonare ICP, NDT and G-ICP. For the purposes of comparison,the Point Cloud Library (PCL) implementations of ICP,NDT, and G-ICP are used [18]. The ICP algorithms areimplemented using a maximum correspondence distance of10m and the NDT algorithm was implemented with a gridsize of 3m. For all optimization based approaches, theoptimization is terminated when the norm of the gradientor the norm of the step size falls below 10−6.

A. Accuracy in Outdoor Urban Environments

In order to test the accuracy of SGC-NDT against otherscan registration methods, the Ford Campus Vision and LI-DAR Data Set [14] is used. The data set was generated usinga Velodyne HDL-64E LIDAR with ground truth acquired byintegrating the high quality velocity and acceleration mea-surements produced by a Applanix POS-LV 420 INS withTrimble GPS. When scans were aligned using the groundtruth measurements, the alignment was seen to have on theorder of sub-degree accuracy in orientation and decimeteraccuracy in position. Each algorithm performs pair-wise scanregistration using every 10th scan produced by the LIDAR.The registration is initialized with a parameter estimate ofzero translation and zero rotation. The error in the translationand rotation of registration is then compared against theground truth measurements. The translational and rotationalerror distributions are displayed in Figure 3. It should benoted that the the upper adjacent and lower adjacent whiskersof the box-plots represent 1.5× the interquartile range of thedata.

(a) Absolute translational error

0

0.005

0.01

0.015

0.02

0.025

0.03

0.035

SGC−P2D SGC−D2D NDT ICP GICP

Abs

olut

e R

otat

iona

l Err

or [r

ad]

(b) Absolute rotational error

Fig. 3. Box-plots of the error distributions for each algorithm for theexperiment from the Ford campus data set. (a) Absolute translational error.(b) Absolute rotational error

The error distributions from Figure 3 demonstrate that bothSGC-NDT algorithms produce accurate results when com-pared against the ground truth, as does the G-ICP algorithm.The large translational error distribution for NDT demon-strates the algorithm’s poor convergence characteristics, asthe optimization has a tendency to converge to local minima.

TABLE ISUMMARY OF CRISPNESS MEASURES FOR REGISTRATION TESTS IN

URBAN AND FORESTED.

ICP G-ICP SGC-NDT-P2D SGC-NDT-D2DFord 132507 124911 125767 125221WPI - 44609 41884 40560

The ICP algorithm also shows fairly low translational androtation errors, however, does not appear to be as accurateas SGC-NDT or G-ICP.

To further assess the scan registration quality, the pair-wise registered scans are aggregated into a global map. Themaps are evaluated using a crispness measure, which is alsoused for map evaluation in [10]. The crispness of a mapis evaluated by discretizing the 3D space into voxels, andrecording the number of occupied voxels. For the experiment,a voxel size of 0.1 m is used. A map which evaluates to alower number is said to be crisper than a map which evaluatesto a higher number. Intuitively, the crispness quality measurecaptures the blurring of the map caused by incrementalpair-wise registration errors as the scans are aggregated.The crispness measure is evaluated for SGC-NDT, ICP andG-ICP. A crispness for NDT is not evaluated since theregistration did not converge to the global minimum onnumerous occasions. The crispness measures for SGC-NDT,ICP and G-ICP are presented in Table I.

The crispness measures indicate that both SGC-NDT al-gorithms and G-ICP generate high quality maps, while ICPproduces a lesser quality map due to the accumulation ofregistration error.

B. Accuracy in Sparse Forested Environments

To determine the robustness of the proposed method, anexperiment is performed using a dataset collected from asparse, forested environment. The data was collected as partof the NASA Sample Return Robot Challenge, at WorcesterPolytechnic Institute (WPI). The laser scans were collectedusing a Velodyne HDL-32E LIDAR, mounted to a custommade chassis designed for the challenge. The environmentconsists mainly of trees and other foliage, as well as asingle small gazebo structure. Forested environments arespecifically challenging for registration because foliage, grassand bush tend to create irregular, noisy point clouds with anambiguous local surface, resulting in difficulties determiningcorrect point correspondences and accurate surface normalinformation.

To determine robustness across all types of environments,only the two best performing algorithms, SGC-NDT andG-ICP, from the previous urban scenario experiment arepresented, as the ICP and NDT algorithms were unable toconverge consistently and do not provide any meaningfulresults for comparison. The experiment is performed in asimilar manner to the urban scenario experiment, where scansare pair-wise registered using the competing algorithms. Theregistered scans are then aggregated into a global map.Each algorithm performs pair-wise scan registration usingevery 50th scan produced by the LIDAR, as the robotmoved slowly through the environment. The registration is

initialized with a parameter estimate of zero translation andzero rotation. Visually, the result of aggregating the registeredscans is presented in Figure 4. The experiment demonstratesperformance in situations where odometry estimates arehighly inaccurate, such as in GPS denied environments,or on vehicles where wheel slip or other factors makeaccurate state estimation difficult. Since accurate groundtruth is not available for the WPI data set, the global mapsare evaluated based solely on the crispness measure. Thecrispiness measures for the WPI data set are presented inTable I.

(a) Top-down G-ICP map (b) Top-down SGC-NDT-P2D map

(c) Close-up G-ICP map (d) Close-up SGC-NDT-P2D map

Fig. 4. Top-down and close up views for the resulting global maps usingthe SGC-NDT-P2D and G-ICP algorithms. (Best viewed in colour).

The crispness measure illustrates that G-ICP was not ableto provide accurate scan registration results as compared tothe SGC-NDT algorithms. The difficulty with convergence ismost likely because G-ICP relies on accurate surface normalsto determine the correct point correspondences, which isdifficult to achieve in a forested environment. The SGC-NDT algorithm models environmental features such as treesand shrubs as a mixture of Gaussian distributions, thus goodregistration results are achievable in sparse environmentswith poor local surface structure. Figure 4 shows the mapsfrom top down view of the traversed area, as well as a closeup view of the environmental features. The G-ICP map showssignificant drift and occasionally poor convergence to localminima, while the SGC-NDT map maintains very crisp fea-tures. The crispness of the features can be clearly identifiedby inspecting the alignment of the structural columns seenin the close-up view.

IX. CONCLUSION

This work presents the segmented greedy cluster NDT,a modified version of the NDT algorithm. The proposedmethod segments the ground from the scans, clusters thenon-ground points using a greedy cluster algorithm andmodels each cluster as a Gaussian distribution. The NDTscan registration algorithm is modified to use the clustered

Gaussians, as opposed to the standard NDT algorithm wherethe Gaussian clusters are generated from a voxel grid. TheSGC-NDT approach is applied to both point to distributionand distribution to distribution NDT, and is shown to produceaccurate results in both urban and sparse, forested environ-ments. Future work involves using the Gaussian clusters asfeatures for initial alignment of the scans, optimizing theimplementation for real-time applications, a full analysis ofcomputational complexity, and evaluating the algorithm in alarger set of environments.

REFERENCES

[1] P. Besl and H. McKay, “A method for registration of 3-d shapes,” IEEETransactions on Pattern Analysis and Machine Intelligence, vol. 14,no. 2, pp. 239 –256, Feb 1992.

[2] Y. Chen and G. Medioni, “Object modeling by registration of multiplerange images,” in 1991 IEEE International Conference on Roboticsand Automation (ICRA), vol. 3, Sacramento, CA , USA, Apr 1991,pp. 2724–2729.

[3] Z. Zhang, “Iterative point matching for registration of free-form curvesand surfaces,” INRIA-Sophia Antipolis, Tech. Rep., 1992.

[4] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” in 2009Robotics: Science and Systems (RSS), Seattle, USA, June 2009.

[5] P. Biber and W. Strasser, “The normal distributions transform: anew approach to laser scan matching,” in 2003 IEEE InternationalConference on Robotics and Automation (ICRA), vol. 3, Las Vegas,NV, USA, Oct. 2003, pp. 2743–2748.

[6] M. Magnusson, A. Lilienthal, and T. Duckett, “Scan registrationfor autonomous mining vehicles using 3D-NDT,” Journal of FieldRobotics, vol. 24, no. 10, pp. 803–827, 2007.

[7] T. Stoyanov, M. Magnusson, and A. Lilienthal, “Point set registrationthrough minimization of the l2 distance between 3D-NDT models,”in 2012 IEEE International Conference on Robotics and Automation(ICRA), St. Paul, MN, USA, May 2012, pp. 5196–5201.

[8] A. Das and S. Waslander, “Scan registration with multi-scale k-means normal distributions transform,” in to appear in 2012 IEEEInternational Conference on Intelligent Robots and Systems (IROS),Vilamoura, Algarve, Portugal, Oct 2012.

[9] F. Moosmann, O. Pink, and C. Stiller, “Segmentation of 3d lidar datain non-flat urban environments using a local convexity criterion,” in2009 IEEE Intelligent Vehicles Symposium, June 2009, pp. 215–220.

[10] B. Douillard, A. Quadros, P. Morton, J. Underwood, M. De Deuge,S. Hugosson, M. Hallstrom, and T. Bailey, “Scan segments matchingfor pairwise 3d alignment,” in 2012 IEEE International Conferenceon Robotics and Automation (ICRA), St. Paul, MN, USA, May 2012,pp. 3033–3040.

[11] S. Vasudevan, F. Ramos, E. Nettleton, and H. Durrant-Whyte,“Gaussian process modeling of large-scale terrain,” Journal of FieldRobotics, vol. 26, no. 10, pp. 812–840, 2009.

[12] B. Douillard, J. Underwood, N. Kuntz, V. Vlaskine, A. Quadros,P. Morton, and A. Frenkel, “On the segmentation of 3d lidar pointclouds,” in 2011 IEEE International Conference on Robotics andAutomation (ICRA), Shanghai, China, May 2011, pp. 2798–2805.

[13] D. Bertsekas, Nonlinear Programming. Athena Scientific, 1999.[14] G. Pandey, J. R. McBride, and R. M. Eustice, “Ford campus vision

and lidar data set,” International Journal of Robotics Research, vol. 30,no. 13, pp. 1543–1552, November 2011.

[15] C. Tongtong, D. Bin, L. Daxue, Z. Bo, and L. Qixu, “3d lidar-basedground segmentation,” in 2011 First Asian Conference on PatternRecognition (ACPR), Beijing, China, Nov 2011, pp. 446–450.

[16] M. Magnusson, “The three-dimensional normal-distributions transform— an efficient representation for registration, surface analysis, andloop detection,” Ph.D. dissertation, Orebro University, Dec. 2009,Orebro Studies in Technology 36.

[17] M. Magnusson, T. Duckett, and A. J. Lilienthal, “Scan registrationfor autonomous mining vehicles using 3D-NDT,” Journal of FieldRobotics, vol. 24, no. 10, pp. 803–827, Oct 24 2007.

[18] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),”in 2011 IEEE International Conference on Robotics and Automation(ICRA), Shanghai, China, May 9-13 2011, pp. 1–4.