Scan registration for autonomous mining vehicles using 3D-NDTeprints.lincoln.ac.uk/1615/1/mm_al_td_jfr2007.pdf · Scan Registration for Autonomous Mining Vehicles Using 3D-NDT Martin

Scan Registration forAutonomous Mining Vehicles

Using 3D-NDT

Martin Magnusson and Achim LilienthalAASSDepartment of TechnologyÖrebro UniversitySE-701 82 Örebro, Swedene-mail: [email protected],[email protected]

Tom DuckettDepartment of Computing and InformaticsUniversity of LincolnBrayford PoolLincoln LN6 7TS UKe-mail: [email protected]

Received 24 September 2006; accepted 28 May 2007

Scan registration is an essential subtask when building maps based on range finder datafrom mobile robots. The problem is to deduce how the robot has moved between con-secutive scans, based on the shape of overlapping portions of the scans. This paper pre-sents a new algorithm for registration of 3D data. The algorithm is a generalization andimprovement of the normal distributions transform �NDT� for 2D data developed byBiber and Strasser, which allows for accurate registration using a memory-efficient rep-resentation of the scan surface. A detailed quantitative and qualitative comparison of thenew algorithm with the 3D version of the popular ICP �iterative closest point� algorithmis presented. Results with actual mine data, some of which were collected with a newprototype 3D laser scanner, show that the presented algorithm is faster and slightly morereliable than the standard ICP algorithm for 3D registration, while using a more memory-efficient scan surface representation. © 2007 Wiley Periodicals, Inc.

1. INTRODUCTION

The main application considered in this paper is tun-nel profiling �that is, measuring and building three-

dimensional models� by using a range sensormounted on drill rigs that are commonly used fortunnel excavation �see Figure 1�. Profiling of minetunnels is necessary to check that new tunnels have

• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • •

Journal of Field Robotics 24(10), 803–827 (2007) © 2007 Wiley Periodicals, Inc.Published online in Wiley InterScience (www.interscience.wiley.com). • DOI: 10.1002/rob.20204

the desired shape, to measure the volume of materialremoved, to survey old tunnels and investigatewhether they are still safe, and to build three-dimensional maps that can be used for autonomousoperation of drill rigs and other mining vehicles.

Today’s tools for tunnel profile scanning are ei-ther very slow or very expensive, and profiling cur-rently needs to be performed separately from anyother activity in the tunnel. The rock drill industryhas been searching for tools that give a fast and cheapsolution to this problem for a long time.

The long-term goal of this work is to make it pos-sible for mining vehicles to operate with minimal hu-man intervention, or completely autonomously. If un-derground operations could be performed byautonomous vehicles, the lives and health of thou-sands of mine workers could be saved in the future.

The paper is structured as follows. Section 2briefly covers the basic algorithms for scan registra-tion that provided the foundation for this work. Sec-tion 3 describes the three-dimensional normal distri-butions transform, a novel algorithm for registrationof 3D surfaces, and Section 4 presents some variantsand improvements to 3D-NDT. Section 5 gives the re-sults of experiments performed using scan data froman underground mine, and shows a detailed com-parison of the algorithms presented in the paper. Fi-nally, Section 6 concludes and summarizes the paper.

2. EXISTING SCAN REGISTRATIONALGORITHMS

Pairwise scan registration is the process of aligningtwo overlapping scans, given an estimate of the rela-tive transformation needed to match one with theother. When the scans are properly aligned, they aresaid to be in registration. Several algorithms for thispurpose exist, the most common and well known ofwhich is the ICP �iterative closest point� algorithm�Besl & McKay, 1992; Chen & Medioni, 1992�. Follow-ing the nomenclature of Besl & McKay, the scan thatserves as the reference is called the model and the scanthat is moved into alignment with the model is calledthe data scan.

2.1. ICP

ICP works by iteratively searching for pairs ofnearby points in the two scans and minimizing thesum of all point-to-point distances.

Two main problems of ICP are that it is point-based, and as such does not make use of the localsurface shape around each point, and that thenearest-neighbor search in the central loop is rathertime consuming. One way to speed up the search isto use an efficient search data structure, such as akd-tree with approximate nearest-neighbor search�Greenspan & Yurick, 2003�, but the search pass isstill the main bottleneck for the algorithm’s runningtime.

If the point pairs that are found in the first stepof the algorithm indeed correspond to the samepoint on the scanned surface, the computed transfor-mation will be exact. However, since the closestpoint is used as a guess for the corresponding point,it is desirable to detect and filter bad correspon-dences and keep only the best ones. One strategy isto assign different weights to the pairs, as a kind of“soft” outlier rejection �Rusinkiewicz, 2001�. Thestrategy is to assign more weight to point pairs thatare likely to contribute more to the end result andless weight to pairs that are more likely to be incor-rect correspondences. One example of a weightingcriterion is to use the relative distance between thepoints. The weight w of the correspondence betweenpoints x and y can be set proportional to the pointpair with the largest distance so that

Figure 1. An Atlas Copco drill rig in its natural environ-ment. The vehicle is equipped with rock drills mounted ontelescopic booms and is used for drilling holes in the rockface before blasting.

804 • Journal of Field Robotics—2007

Journal of Field Robotics DOI 10.1002/rob

w = 1 −�x − y�

max�xi − yj�. �1�

We found that linear weighting based on distancedid not improve the results on our data. For tunnelor corridor data, distance-based weighting can infact degrade performance. Because most pointsalong the walls and ceiling will generally be well-aligned, their influence will overwhelm point pairswith larger distances, which may correspond to cor-ners and other features that are important. Thereforewe weighted all point pairs equally.

2.2. 2D-NDT

The normal distributions transform �NDT� is a morerecent method for registration developed for two-dimensional scan data �Biber & Strasser, 2003�. Thekey element in this algorithm is its representation ofthe model. Instead of using the individual points ofthe model, it is represented by a combination of nor-mal distributions, describing the probability of find-ing a surface point at a certain position. The normaldistributions give a piecewise smooth representationof the model point cloud, with continuous first andsecond order derivatives. Using this representation,it is possible to apply standard numerical optimiza-tion methods for registration. Numerical optimiza-tion is a problem that has been studied for centuries,and fast and reliable methods for optimizing func-tions such as a sum of normal distributions havebeen developed and tested over time. Because thepoints in the model are not used directly for match-ing, there is no need for the computationally expen-sive nearest-neighbor search that is done in the cen-tral loop of ICP. Storing the NDT representation ofscans instead of storing the point clouds themselvesalso requires much less memory. This is beneficialfor all large maps, where storing the complete pointcloud data is uneconomical. Another applicationwhere a compact map representation is needed iswhen using multiple time scales for mapping dy-namic environments, where multiple copies of thesame area are stored, representing different timescales. Computing the normal distributions is aquick one-off task that is done during a single passthrough the points of the model.

The first step of the NDT algorithm is to subdi-vide the space occupied by the model into regularlysized cells �squares in the 2D case, or cubes in 3D�.

Then, for each cell b that contains more than someminimum number of points, the mean vector q ofthe points in the cell and the covariance matrix C arecalculated as

q =1n�

k=1

n

xk, �2�

C =1

n − 1�k=1

n

�xk − q��xk − q�T, �3�

where xk=1,. . .,n are the points contained in the cell.The probability that there is a point at position x

in cell b can then be modeled by the normal distri-bution N�q ,C�. The probability density function�PDF� is formulated as

p�x� =1c

exp�−�x − q�TC−1�x − q�

2 � , �4�

where q and C are the mean vector and covariancematrix for the cell that contains point x, and c is anormalizing constant that can be set to one for prac-tical purposes. Setting the limit for which cells areconsidered occupied to five points per cell is reason-able, in order to get a sensible covariance matrix. A2D laser scan and its corresponding normal distribu-tions are shown in Figure 2.

The parameters to be optimized—that is, the ro-tation and translation of the current pose estimate—can be encoded in a vector p. For 2D registration,there are three transformation parameters to opti-mize. Let p= �tx , ty ,�� be the parameter vector, wheretx and ty are the translation parameters and � is therotation angle. Using counter-clockwise rotation, the2D transformation function is

T3�p,x� = cos � − sin �

sin � cos �x + tx

ty . �5�

The algorithm measures the fitness of a particularpose by evaluating the PDFs at all points of the datascan. Since optimization problems are generally for-mulated as minimization problems, the score func-tion is defined so that good parameters yield a largenegative number.

Magnusson et al.: Scan Registration for Autonomous Mining Vehicles Using 3D-NDT • 805


Given a set of points X= �x1 , . . . ,xn�, a pose p,and a transformation function T�p ,x� to transform apoint in space, the score s�p� for the current set ofparameters is defined as

s�p� = − �k=1

n

p�T�p,xk�� . �6�

In other words, the score is the negated sum of prob-abilities that the transformed points of the data scanare actually lying on the model surface.

Given the transformation parameters p, New-ton’s algorithm can be used to iteratively solve theequation H�p=−g, where H and g are the Hessianand gradient of s. The increment �p is added to thecurrent estimate of the parameters in each iteration,so that p←p+�p.

For brevity, let

x� T�p,x� − q . �7�

In other words, x� is the transformed point x, rela-tive to the center of the point distribution of the cellto which it belongs. The entries for the gradient of

the score function can be written as

gi =�s�pi

= �k=1

n

xk�TC−1�xk�

�piexp�− xk�

TC−1xk�

2� . �8�

The entries of the Hessian are

Hij =�s

�pi�pj= �

k=1

n

exp�− xk�TC−1xk�

2��xk�

TC−1�xk�

�pi�

��− xk�TC−1�xk�

�pj� + xk�

TC−1 �2xk�

�pi�pj+

�xk�T

�pjC−1�xk�

�pi� .

�9�

The first- and second-order partial derivatives of x�in Eqs. �8� and �9� depend on the transformationfunction. Using the 2D transformation function fromEq. �5�, the first-order derivative �x�/�pi is given bycolumn i of the Jacobian matrix

J3 = 1 0 − x1� sin � − x2� cos �

0 1 x1� cos � − x2� sin � , �10�

and the second-order derivatives are

�2x�

�pi�pj= ��

− x1� cos � + x2� cos �

− x1� sin � − x2� cos �� if i = j = 3,

�0

0� otherwise.�

�11�

The NDT algorithm for registering two point sets Xand Y �finding the pose p that moves the data scan Xinto registration with the model Y� is given in Algo-rithm 1.

In recent work carried out independently, asemi-3D version of NDT was used to register largehigh-resolution outdoor scans �Ripperda & Brenner,2005�. In the work of Ripperda and Brenner, each 3Dscan was divided into several horizontal slices and2D-NDT was applied on each pair of slices. Using N

Figure 2. A 2D laser scan of part of a room and the NDTrepresentation describing the surface shape. The originalpoint cloud is shown with small squares, and the roundedshapes show the normal distributions of the occupied gridcells. Each cell is a square with 1 m side length. Brighterareas represent a higher probability.



slices, and denoting the score for slice n by sn�p�, thescore function was defined as the sum over all slicepairs

s�p� = �n=1

N

sn�p� . �12�

The approach used by Ripperda and Brenner canonly perform registration in the horizontal plane,and only works under the assumption that the localcoordinate systems of all scans are aligned in theplane, meaning that the scanner must have the sameorientation at each scan pose. This assumption doesnot hold for the majority of mobile robot applica-tions.

Algorithm 1 Register data scan X with model Y usingNDT

Build cell structure Bfor all points yi�Y do

Find the cell bk that contains yi

Store yi in bk

end for

for all cells bi�B do

Y�= �y1� , . . . ,yn��←all points in bi

qi←1n

�j=1n yj�

Ci←covariance of all points in Y�

end for

while not converged do

score←0

g←0

H←0

for all points xi�X do

Find the cell bk that contains T�p ,xi�

xi�←T�p ,xi�

score←score−p�xi�� see Eq. �4��

Update g �see Eq. �8��

Update H �see Eq. �9��end for

Solve H�p=−g

p←p+�p

end while

2.3. Registration with Approximants to theDistance Function

Mitra et al. presented an approach to 3D scan regis-tration that is similar to NDT �Mitra, Gelfand, Pott-mann & Guibas, 2004�. The idea behind their algo-rithm is to describe the model surface implicitly,using quadratic approximants to the squared dis-tance function from the target surface, instead of thenormal distributions used by NDT or the originalpoint cloud data used by ICP. Registration then be-comes the task of minimizing the sum of the dis-tance functions when evaluated at the points of thedata scan. Because the approximants used in theiralgorithm are second-order approximations of thelocal surface shape that are valid within an intervalaround each point, and not just at the points wherethey are computed, it is possible to use Newton it-eration to solve the registration problem with thissurface representation, too. One way to use the ap-proximants is to compute them on demand for eachpoint in the model, using the normal vector and thetwo principal curvature directions at that point. Thenormal and principal curvature vectors are com-puted in a preprocessing step, and the distance func-tions are computed at each step of the registrationprocess. The other method presented by the authorsis to subdivide the space occupied by the model intoa grid. For each grid cell �both cells that are occupiedby the surface and empty cells�, a quadratic patch isfitted to the squared distance to the scan surface. Thesecond method is quite similar to the NDT versionsdescribed in this paper. For any point in the datascan, the algorithm queries the cell structure for thecorresponding approximant to the squared distancefunction to the surface and uses these values as the“score” of the current pose.

The squared distance function used by Mitra etal. is in fact a generalization of the error metrics usedby the most common versions of ICP: the point-to-point distance mentioned in Section 2.1, and thepoint-to-plane distance, which measures the dis-tance from a point in the data scan to the closestpoint on the tangential plane of its closest neighborin the model. In their paper, they showed that thesuggested functions lead to more reliable registra-tion from a larger number of initial pose estimatesthan point-to-plane ICP. The algorithm behaves likepoint-to-point ICP �stable with regard to the initialerror, but slower� when the scans are far from each



other, and like point-to-plane ICP �faster, but lessstable with regard to the initial error� when the scansare almost registered.

The quadratic patches approximate both the po-sition and the curvature of the surface, while thenormal distributions used in NDT only give an esti-mate of the position. As long as the surface issmooth and the cells are small enough so that thesurface is approximately uni-modal within each cell,quadratic patches are a more descriptive representa-tion of the surface than the normal distribution ofpoints within the cell. Mitra et al. use the fitting errorof the quadratic patch to deal with the problem ofchoosing a good cell size, by building an octree cellstructure that has small cells where required andlarge cells where that is sufficient. Neighboring cellsare merged if a patch fitted to the surface in thelarger cell has an acceptable fitting error. A similarmethod has also been implemented for NDT �seeSection 4.3.2�.

For very noisy data, we hypothesize that surfacepatches are an inappropriate model of the scan data,compared to the more context-free normal distribu-tion representation. The quadratic patches assumethat the scan points are sampled from a piece-wisesmooth surface, which is not always the case. In themine mapping application, the walls of the tunnelsare quite rough, and the the sample spacing is at alarger scale than the surface roughness for areas ofthe tunnel far away from the scanner. Using only thescan points or an approximated surface fitted to thescan points is likely to lead to misalignment of scansproportional to the roughness of the walls. The un-even walls will in this case behave like noisy mea-surements. Smoothing the surface with the proposednormal distributions is a good alternative in thatcase. Mitra et al. did not report the execution timesof their algorithm, but it would be interesting tocompare the speed and accuracy of their approach tothat of NDT. We did not compare the two algorithmsfor the work presented here, because of time con-straints and the lack of a publicly available imple-mentation. Though the storage requirements for thequadratic fit representation are smaller than storingthe point clouds themselves—at least for denselysampled point clouds—they are somewhat largerthan for NDT, because distance approximants arestored for all cells �requiring nine parameters percell�, and not just the occupied ones.

3. 3D-NDT

The main difference between 2D and 3D registrationwith NDT lies in the spatial transformation functionT�p ,x� and its partial derivatives. In two dimensions,rotation is represented with a single value for theangle of rotation around the origin, and the most ob-vious transformation function is the one from Eq. �5�.General rotation in 3D is more complex. A robust 3Drotation representation requires both an axis and anangle. A straightforward way to represent a general3D transformation is to use seven parameters �threeparameters for the translation, three for the rotationaxis, and one for the rotation angle�. Using a right-handed coordinate system and counter-clockwise ro-tations, the transformation of a 3D point x using a pa-rameter vector p can then be formulated as

T7�p,x� = � erx2 + c erxry − srz erxrz + sry

erxry + srz ery2 + c eryrz − srx

erxrz − sry eryrz + srx erz2 + c

�x + �tx

ty

tz� ,

�13�

where p= �t�r��, t= �tx , ty , tz� is the translation, r= �rx ,ry ,rz� is the axis of rotation, s=sin �, c=cos �,e=1−cos �, and � is the rotation angle.

A common way to represent 3D rotation in com-puter graphics is to use quaternions, which are a gen-eralization of complex numbers. Quaternions havefavorable properties when used for rotation, most no-tably when composing several rotations. A normal-ized quaternion always represents a valid rotation. Acombination of rotation matrices, on the other hand,may become nonorthogonal as rounding errors in-crease over time, and using a nonorthogonal transfor-mation matrix for rotation has undesired effects. Theaxis-angle rotation r, � can be represented by thequaternion cos �+ �rx cos ��i+ �ry cos ��j+ �rz cos ��k.

The partial derivatives that are needed for Eqs.�8� and �9� when using T7 can be found in the Jacobianand Hessian matrices �17� and �18�. The Hessian ispresented as a block matrix with 7�7 blocks, whereeach block is a three-element vector. Similarly to Eq.�7�, define

x� T7�p,x� − q , �14�

where x is a 3D scan point, q is the mean vector of thecell in which it lies, and p is a vector of transformationparameters. Then



�x�

�pi= the ith column of J7, �15�

�2x�

�pi�pj= Hij, �16�

J7 = �1 0 0

0 1 0

0 0 1

e�2rxx1 + ryx2 + rzx3� eryx1 − sx3 erzx1 + sx2

erxx2 + sx3 e�rxx1 + 2ryx2 + rzx3� erzx2 − sx1

erxx3 − sx2 eryx3 + sx1 e�rxx1 + ryx2 + 2rzx3�sA − cB sC − cD sE − cF

�T

�17�

A = �rx2 − 1�x1 + rxryx2 + rxrzx3, B = rzx2 − ryx3,

C = rxryx1 + �ry2 − 1�x2 + ryrzx3, D = − rzx1 + rxx3,

E = rxrzx1 + ryrzx2 + �rz2 − 1�x3, F = ryx1 − rxx2

H7 = �0 0 0 0 0 0 0

0 0 0 0 0 0 0

0 0 0 0 0 0 0

0 0 0 a b c d

0 0 0 b e f g

0 0 0 c f h i

0 0 0 d g i j

� , �18�

a = �2ex1

0

0�, b = �ex2

ex1

0�, c = �ex3

0

ex1� ,

d = �s�2rxx1 + ryx2 + rzx3�sryx1 − cx3

srzx1 + cx2� ,

e = � 0

2ex2

0�, f = � 0

ex3

ex2�, g = � srxx2 + cx3

s�rxx1 + 2ryx2 + rzx3�srzx2 − cx1

� ,

h = � 0

0

2ex3�, i = � srxx3 − cx2

sryx3 + cx1

s�rxx1 + ryx2 + 2rzx3�� ,

Figure 3. The probability functions used by 3D-NDT fora tunnel section. Brighter, denser parts represent higherprobabilities. The cells have a side length of 1 m.



j = �cA + sB

cC + sD

cE + sF� .

In �17� and �18�, xn denotes the scalar nth componentof the 3D vector xk. Figure 3 illustrates the 3D normaldistributions for a mine tunnel scan.

The equations above were implemented for theexperiments in Section 5. However, the angle param-eter in the seven-element parameter vector is redun-dant. The angle can also be encoded implicitly in thethree axis parameters, so that the length of the rota-tion axis corresponds to the angle of rotation, insteadof maintaining a normalized rotation axis separately.In that case, only six parameters are needed.

4. ALTERNATIVE METHODS IMPLEMENTED

Several choices need to be made for a practical imple-mentation of 3D-NDT. This section describes differ-ent methods and parameters that were tested, andtheir influence on the basic algorithm.

4.1. Sampling Method

When using 3D-NDT, the model is converted to a setof normal distributions. The points of the data scanare then aligned to these functions. Usually, a largenumber of scan points are redundant for the pur-pose of describing the scanned surface shape. There-fore, it is normally desirable to subsample the datascan in order to improve running time. In manycases, not least when scanning in corridors and tun-nels, as well as in unstructured outdoor environ-ments, the distribution of points is very muchdenser near the scanner location than farther out. Ifpoints are sampled in a uniformly random manner,the sampled subset will have a similar distribution.Consequently, parts that are further from the scan-ner contribute less to the registration. This is not spe-cific to NDT, but is common to all point-based reg-istration methods.

Spatially distributed sampling is a sensible alter-native method, that is, making sure that the spatialdistribution of points in the subsample is as even aspossible. This can be done by grouping the pointsinto equally sized cells, similar to what is done whenthe normal distributions are generated for themodel. Then, a number of points are drawn from

each cell. If the distribution of cells is adequate, thisstrategy will give an even distribution of points.

It is also possible to implement subsamplingmethods that consider the normals as well as posi-tions of points, either making the distribution of nor-mals as spread out as possible or primarily choosingpoints with “unusual” normals �Rusinkiewicz,2001�. The normal at each surface point must then becomputed from a sufficient number of its neighbors.Gelfand et al. developed an improved samplingmethod for ICP, mainly for cases when the data con-sist of mostly planar regions with a few important“lock and key” features �Gelfand, Ikemoto, Rusink-iewicz & Levoy, 2003�. Such data are notoriously dif-ficult to register correctly, since the scans can “slide”along the planar regions without any big changes inthe error function. The stable sampling method ofGelfand et al. requires that normals are computedfor all sample points. They reported that the algo-rithm takes about three times longer to execute thanICP with uniform subsampling. In the work coveredby this paper, point clouds without normal or con-nectivity information have been used, so these kindsof sampling methods have not been investigated indetail. Though it was not tested, we believe thatmost of the mine tunnel scans do not have the kindof shape that the stable ICP sampling method wasdesigned for. While many of the scan pairs used inSection 5 are difficult to register for the same rea-sons, namely that the large-scale features are notenough for accurate registration, the importantsmall-scale features are not generally as distinct as inthe incised plane data sets used by Gelfand et al.,but are more evenly distributed over the rough sur-face and have characteristics similar to Gaussiannoise.

4.2. Cell Size

Choosing a good cell size is important when usingNDT. Any feature that is much smaller then the sizeof a cell will be blurred out by the PDF that de-scribes the local surface shape around it. Choosing acell size that is too large therefore often leads to lessaccurate registration. On the other hand, the regionof influence of a cell only extends as far as its bound-aries. That is, the cell will only contribute to thescore function for scan points within its bounds. Theconsequence of this is that if the cells are too small,the two scans must be close together before registra-tion for the algorithm to succeed. Using smaller cells



also requires more memory. The optimal size anddistribution of cells depend on the shape of the in-put data and on the application.

4.3. Discretization Methods

Using a fixed lattice of square or cubic cells burdensthe user with the task of choosing a good cell size. Amore adaptive cell structure would be preferable, us-ing finer subdivision in places where a single normaldistribution cannot describe the surface satisfyingly.This section presents a number of alternative meth-ods for handling the cells and their PDFs.

4.3.1. Fixed Subdivision

The benefit of using a fixed lattice of cells is that theoverhead for initializing the cell structure is verysmall. Only one set of PDF parameters needs to becomputed for each cell, and the positioning of eachcell is straightforward. Even more important for theperformance of the algorithm is that point-to-celllook-up is also a very quick operation that can bedone in constant time, as the cells can be stored in asimple array.

4.3.2. Octree Subdivision

An octree is a tree structure that can be used to storea hierarchical discretization of 3D space. In an oc-tree, each node represents a bounded partition of thespace. Each internal node has eight children thatrepresent congruent and non-overlapping subdivi-sions of the space partition that corresponds to theirparent node. When creating an octree, the root nodeis sized to encompass the whole model. The tree isthen built recursively, splitting all nodes containingmore than a threshold number of points. All datapoints are contained in the leaf nodes of the octree.

The “octree” version of 3D-NDT starts withfixed regular cells, as described before, with the dif-ference that each cell is the root node of an octree.Each cell in which the spread of the distribution islarger than a certain threshold is then recursivelysplit, thus making a forest of octrees. It is importantfor the efficiency of the algorithm that the point-to-cell look-up is fast, and this is the main reason whya forest of octrees was implemented, rather thanhaving a single octree with a root node that spansthe whole scan. For many types of scan data, a rea-sonable cell size can be specified, so that only a few

cells in parts where the scan surface is particularlyuneven need to be split. Thus, for most points, find-ing the correct cell only needs a single array access,while traversing a large octree once for each pointwould take more time. Using a forest gives a veryslight increase in memory consumption, since a fewunnecessary cells need to be stored, but the effect ofthis is negligible.

When traversing the cell structure looking forthe corresponding cell to a point in the data scan, theleaf node that contains the point is chosen and itsPDF is used to compute the score function.

4.3.3. Additive Subdivision

Using octree subdivision gives a better representa-tion of the surface shape in areas where large cellswould hide many details, while keeping large cellswhere the surface is largely planar and further sub-division is unnecessary. However, the problem thatsmall cells have a smaller region of influence re-mains: if corresponding points of the two scans arenot within the same cell, the extra fidelity is of nouse.

A slight change to the octree subdivision schemecan mitigate this limitation. Instead of using onlyone leaf of the octrees, each point from the data scanhas its score function evaluated for all of the distri-butions in the leaf cells. This effectively increases thesupport size of the leaf cells to that of their root cell,without sacrificing the extra refinement of the sur-face description that they give. This is illustrated inFigure 4.

4.3.4. Iterative Subdivision

Another option is simply to perform a number ofNDT runs with successively finer cell resolution, sothat the start pose for each iteration other than thefirst one is the end pose of the previous run. The firstruns are good for bringing badly aligned scanscloser together, and later runs improve the roughinitial match.

If the different cell structures are computed fromthe smallest cell size to the largest and the cell sizesare changed by a factor 2 in each iteration, the largercells do not need to be computed from scratch, butcan be updated efficiently using the data from theirsubcells. This method is a potential improvement tohow the implementation shown in Section 5.2 wasdone.



During the preparation of this paper, Takeuchiand Tsubouchi presented another way of using NDTfor 3D scan registration �Takeuchi & Tsubouchi,2006�. Their implementation is rather similar to theversion described in this paper in that they also usean iterative subdivision scheme similar to that de-scribed here. An important difference is that theyused smaller cells in the space that is near the sensorlocation and larger cells farther away in the earlyiterations, and used only the smaller size in the lateriterations, when the scans were almost aligned. Thereasoning behind this is that error in the rotationestimate caused larger displacements further fromthe sensor location, so larger cells are needed thereto make sure that more points from the data scan areused. The linked cells strategy described in Section4.3.5 is another solution to the same problem. Takeu-chi and Tsubouchi tested their algorithm on datafrom a computer lab with good results, though theydid not make a direct comparison of their algorithmwith other registration algorithms.

4.3.5. Linked Cells and Infinite Outer Bounds

Using the discretization methods described so far,points from the data scan lying in unoccupied cellsare discarded, thus rendering large parts of the inputspace “dead.” Instead of doing so, the PDF from theclosest occupied cell can be used for those points.

This increases the region of influence of cells and isillustrated in Figure 5. Even though the value of thePDF of many cells is almost zero outside the cellbounds, so that it makes no substantial contributionto the score anyway, for cells with a very elongatedpoint distribution, the influence outside the cell canalso make a difference.

The same idea can also be applied to points fall-ing outside of the cell lattice altogether. The score forthose points can be computed using the closest cellon the edge of the lattice, so that the outer cells ineffect have infinite outer bounds. However, doing sointroduces a certain “drag” bias, as points from non-overlapping regions of the data scan will be at-tracted to border regions of the model.

Linked cells can be implemented either by let-ting each cell store a pointer to the nearest occupiedcell, or by storing only occupied cells and puttingthem in a kd-tree. The latter should be preferable ifthere are many unoccupied cells.

5. EXPERIMENTS

This section covers experiments performed with un-derground mine data to compare the performance ofdifferent varieties of 3D-NDT and ICP.

Figure 4. Comparing octree and additive subdivision. Asubdivided grid cell is shown on the left, and the treestructure is shown on the right. The PDF of cell a has alarge spread, because the points within the cell are notaligned along a planar region. Therefore it is split, and thePDFs of eight subregions b– i are computed instead. Pointx is within cell a, and, more specifically, within subcell g.Using octree subdivision, x’s contribution to the scorefunction is computed from g alone. Using additive subdi-vision, the score is a sum computed from nodes b– i. Inthis example, nodes b–e are empty and will not add any-thing to the score.

Figure 5. Matching two 2D scans of a tunnel section. Thedotted scan is being registered to the solid scan. Occupiedcells are shaded. If linked cells are not used, the parts ofthe scan that are in unshaded cells will be skipped. Other-wise the linked cell �shown with arrows� will be used. Ifusing infinite outer bounds, the outer cells extend asshown with dashed lines.



There are many parameters that can be changed,both for ICP and 3D-NDT. To avoid a combinatorialexplosion in the number of possibilities, a default“baseline” combination of variants was chosen thatincorporates the following features:

1. ICP parameters• Euclidean point-to-point distance error

metric,• outlier rejection using a 1 m fixed distance

threshold,• least squares optimization �Besl & McKay,

1992�,• approximate kd-tree search structure with

10 points per leaf node in the tree �to mini-mize the amount of back-tracking needed�and 1 cm error bound �for each search, apoint that is no more than 1 cm from thetrue nearest neighbor is returned�,

• constant weighting of point pairs.

2. NDT parameters• fixed cells with 1 m side length,• Newton’s method with line search for op-

timization, with a maximum step length of0.05 ��p � =0.05� so that the maximumchange in the pose vector is 5 cm or0.05 rad at each iteration,

• neither linked cells nor infinite outerbounds.

3. Common parameters• convergence threshold when the change of

�p� is less than 0.0001.

The times reported in this paper include all requiredpreprocessing: creating a kd-tree �for ICP�, buildingthe cell structure and computing all PDFs �for NDT�,and subsampling the data scan �for both algorithms�.

Moderate effort was made to optimize the effi-ciency of the programs. The algorithms were imple-mented in C++. The ICP implementation uses thequite efficient approximate nearest neighbor libraryANN. The numerical optimization code used in 3D-NDT makes use of the C linear algebra library new-mat. This library claims to be most efficient for largematrices, but the matrices involved in the computa-tions for 3D-NDT are no larger than 7�7. It is likelythat the numerical optimization can be performed

faster. The experiments were run on a computer withan AMD Athlon processor running at 1950 MHz and512 MB of memory.

5.1. Data

Three mine data sets were used in the comparisonand evaluation of the registration algorithms. Theywere collected in the Kvarntorp mine, south of Öre-bro in Sweden. This mine is no longer in production,but was once used to mine sandstone. The mine con-sists of more than 40 km of tunnels, all in one plane.Parts of the mine are currently used as archives andstorage facilities, while others are used as a test bedfor mining equipment.

Because of the natural layers of sandstone, thetunnels have a rather characteristic shape, with flatceilings and relatively straight walls. Even thoughthe floor and ceiling are flat compared to many othermines and natural environments, the unevenness ofthe floor makes a wheeled vehicle tilt considerablywhile driving over it. The roughness is comparableto that of a gravel road, and if scans were being reg-istered with only three degrees of freedom �disre-garding tilt and changes in floor height�, therewould be large discrepancies between many scans.Figure 6 shows a photo from one of the tunnels.

The JUNCTION data set �Figure 7� consists of twoscans from the end section of a tunnel. At the far endof the tunnel, there is a flat cast concrete structure,and on one of the side walls there is a passage to aneighboring tunnel. Both the end face and this pas-sage are salient and large-scale features. These twoscans were taken from the same pose, and only dif-fer in resolution. In other words, the ground truthpose for the data scan with respect to the model ist=0, R= ��0,0 ,1� ,0�. The data scan contains 139 642points and the model contains 72 417 points.

The TUNNEL data set �Figure 8� was collected fur-ther down the same tunnel. Two scans were taken

Figure 6. One of the tunnels in the Kvarntorp mine.



approximately 4 m apart. The scans contain around27 500 points each. The scans in this set have muchless obvious features. The only large-scale features—the walls and ceiling—are not enough to give accu-rate registration, as the scans can “slide” along thedirection of the tunnel, and still have a large amountof overlap and close proximity of all surfaces, whichare the usual criteria for a good match. The small-scale features, such as bumps on the walls and lightfixtures in the ceiling, need to be matched in order toproperly register these scans. For these two scans,the ground truth was determined visually, by run-ning a number of registration attempts and picking

one that looked like the closest match. When collect-ing this data set, we tried to measure the relativedisplacement between the scans using a so-called to-tal station �that is, a tripod mounted laser measure-ment device�. The total station can be seen in Figure6 �on a yellow tripod near the left wall�. Three pointswere marked on the scanner, and the total stationwas set up at a fixed position further down the tun-nel. The distances to the three points on the scannerwere measured from each scanner pose, and thetransformation from each scanner pose to the nextwas determined from these data. The resulting mea-surements were not accurate enough to use as aground truth measurement, but they were goodenough to provide an initial estimate for the regis-tration algorithms.

Both the JUNCTION and TUNNEL data sets werecollected with an early prototype of a 3D laser rangefinder, built by Optab Optronikinnovation AB. TheOptab scanner is based on a modulated infra-red la-ser that is projected onto a rotating mirror. The rangeis measured by investigating the phase-shift of thereflected laser beam. The configuration of the scan-ner was changed between the two data sets. For theTUNNEL data set, the scanner was oriented so thatthe first scan plane was horizontal. The scanner wasthen tilted upwards. This is a so-called pitchingscan. Because of this configuration, the floor is notvisible in the TUNNEL data. For the JUNCTION dataset, the scanner was mounted so that each scanplane was vertical, and the scanner was rotatedaround the vertical axis. This is known as a yawingscan �Wulf & Wagner, 2003�. The Optab scanner isshown in Figure 9.

A larger data set, KVARNTORP-LOOP, was col-lected at a later date, using a SICK LMS 200 laserscanner mounted on our mobile robot platformcalled Tjorven �shown in Figure 10�. The SICK scan-ner is a 2D scanner, but was mounted on a pan-tiltunit in order to collect 3D data sets. For theKVARNTORP-LOOP data set, the robot was drivenmanually along two tunnels, forming a loop, with3D scans being taken about 4 to 5 m apart. The ro-bot was kept stationary during the scans, so that allpoints in each scan were taken at the same physicallocation. The first 65 scans from this set are shown inFigure 11. The scans contain around 95 000 pointseach. The scanner on Tjorven is configured for pitch-ing scans.

The scans of the KVARNTORP-LOOP data set aremore accurate than those of the JUNCTION and TUN-

Figure 7. The data scan scan from the JUNCTION data set,seen from above.

Figure 8. The two scans of the TUNNEL data set. The free-floating points in the middle of the tunnel are noise.



NEL sets, which show some disturbances due to thesomewhat unstable experimental state of the scan-ner.

5.2. Results

5.2.1. Results with Single Scan Pairs

To test the performance of the algorithms with re-spect to different parameter values the two scan

pairs of the JUNCTION and TUNNEL data sets wereused. A number of registration attempts were runfrom a set of start poses evenly distributed aroundthe ground truth pose. The magnitudes of the trans-lation and rotation components of the initial poseestimates were kept constant for each batch of tests,but the directions were different for each run. Inother words, the translation displacement for eachtest run was a point on a sphere with a fixed radius.The added rotation error had its axis pointing in arandom direction for each run and the angle �that is,the amount of rotation� was fixed for each batch ofruns. The pose offsets were taken from a set ofpoints evenly distributed on the unit sphere. Thetranslation error of the initial pose estimate is de-noted et and the rotation error is denoted er.

For these experiments, the following settingswere used in the “baseline” setup:

• 10% of the points were sampled from the datascan with even spatial distribution,

• no subsampling of the model �all points wereused�,

Figure 9. The Optab scanner lab prototype.

Figure 10. Tjorven, our mobile robot platform. In addi-tion to the laser scanner used for 3D mapping, it is alsoequipped with a digital camera, an array of sonars, anomnidirectional camera �not shown here�, and a differen-tial GPS system.

Figure 11. The first 65 scans from the KVARNTORP-LOOPdata set, seen from above, after registration with manualintervention where the registration algorithms failed andfor the scans without odometry information. The mapmeasures approximately 55 m by 155 m, and is around6 m high. The traversed distance around the loop isaround 330 m. The top left corner shows the accumulatederror after coming back to a previously visited locationafter completing the loop. The error there is about 2.7 m.To the right of this section is a clear “offset” in the tunnel.This is not a registration error, but shows the actual shapeof the tunnel. That shape is probably due to a mistake onpart of the excavation crew when they were trying tophysically “close the loop.”



• initial translation error of 1 m,• initial rotation error of 0.1 rad,• 100 tests for each set of parameters.

Table I shows the parameters that were manipulatedin the experiment. The results of these experimentsare presented with box plots, with a line connectingthe median values of each set of runs. The box ex-tends to the upper and lower quartile of the data, andthe “whiskers” extend to the maximum and mini-mum values of the sequence. The limits for what isconsidered a “good match” are shown with dashedhorizontal lines. These are not hard limits, but werechosen according to what was considered acceptablefor the application and the accuracy with which theground truth pose could be estimated. If only the me-dian registration errors were shown, 3D-NDT wouldappear to be far superior to ICP in all cases. Eventhough the median error was lower when using 3D-NDT for scan registration, there were problems withsome outlier poses for which the algorithm did notconverge. The box plots show a more complete de-scription of the distribution of the results, showing

both how the majority of the runs behaved and theextreme values.

On a similar note, the mean squared point-to-point error is commonly used as a measure of thequality of registration. We did not include thesenumbers here, as they are not an objective measureof the registration accuracy. The mean squaredpoint-to-point error is exactly the objective functionthat ICP tries to minimize, and, if that were indeedthe best measure of registration accuracy, ICP wouldbe an optimal algorithm and would never fail. Wechose instead to determine a ground truth pose foreach scan pair and measure the deviance from thatpose, with some allowance for what is a closeenough match, as described above. The ground truthpose for the JUNCTION data set was zero rotation andtranslation, since the scanner did not move betweenthe two scans. For the TUNNEL data set the groundtruth was determined by running and inspecting anumber of registration attempts, and an average ofthe best matches was used as the ground truth pose.

Sample ratio: To test the sensitivity to the num-ber of samples being used for registration, a number

Figure 12. Sample ratio tests for the JUNCTION set.



of test sequences were run with increasing numbersof samples. From 0.1% up to 50% of the points in thedata scan were sampled and used for matching to-gether with all of the points in the model. Figures 12and 13 show the results of tests where all other pa-rameters were set according to the baseline setup.

The conclusion is that ICP is less error-pronewhen using very low sample ratios �less than a fewpercent�, and that the execution time is around threetimes longer than for 3D-NDT. Even though 3D-NDT succeeds at registering the two scans frommost of the start poses tried, it fails for some poseswhen using a very low sample ratio. Around 10% ofthe total number of points is enough to give reliableresults for the JUNCTION data set when the initial er-ror is moderate. The median error is lower for 3D-NDT in all cases with larger sampling ratios, butthere are some outlier cases where the error is muchlarger. There were failed registrations at up to 12%sample ratio. ICP gives acceptable results down toaround 8% for the same data and initial error.

As can be seen from Figure 13, the TUNNEL dataset is much more challenging than the JUNCTION set,

both for ICP and 3D-NDT. The median error is stillsmaller for 3D-NDT than for ICP, but, with an initialtranslation error of 1 m and a rotation error of0.1 rad, the algorithms fail to register the scans froma rather large number of the initial pose estimates.

Figure 12 shows that the rotation error actuallyincreases for ICP as the sample ratio goes above 20%for the JUNCTION data set. The reason for this couldbe that more of the scan noise is used, in otherwords, overfitting. A similar effect can be seen for3D-NDT on the TUNNEL data set in Figure 13. Be-cause the two scans in this data set are only partiallyoverlapping, ICP tends to move the source scan a bittoo much towards the center of the target scan tomaximize the amount of overlap. The pose that 3D-NDT converges to when using a high sample ratio issimilar to the one that ICP converges to.

If both the data scan and the model are sub-sampled using the same ratio, and not just the datascan, the required sample ratio is much higher.

Sampling method: Spatially distributed sam-pling is generally more robust than uniformly ran-dom sampling. The results of using a uniform prob-

Figure 13. Sample ratio tests for the TUNNEL set, using spatially distributed sampling.



ability distribution when selecting the subset formatching is shown in Figure 14. As discussed earlier,using uniformly random sampling will preserve thegeneral distribution of points in the scan, and that isnot optimal for tunnel scans, where the concentra-tion of points is much higher near the sensor loca-tion than further away.

Comparing Figures 13 and 14, it can be seen thatthe registration errors are larger when using uniform

random sampling for 3D-NDT. Using spatially dis-tributed sampling with ICP with this data meantthat more non-overlapping points were selected.Therefore, it makes sense not to use this samplingmethod for ICP. For 3D-NDT, the interquartile rangewas rather large for both sampling methods, but themedian translation and rotation errors were signifi-cantly lower when using spatially distributed sam-pling, because the more evenly distributed samplinggives a more representative view of the scan. For theJUNCTION set, uniformly random sampling works al-most equally well compared to spatially distributedsampling, however, since the overlap of the datascan and model is 100%.

Cell size: To show the effect of different cellsizes for 3D-NDT, registration with sizes rangingfrom 0.5 m up to 3 m are shown in Figure 15. Eachbox plot shows the results of 50 test runs.

The running times are shorter when the cells arelarger �and fewer�. The translation error is at itssmallest within a certain cell size range, and in-creases with both smaller and larger cells. For

Figure 14. Sample ratio tests for the TUNNEL set with uniform random subsampling instead of spatially distributedselection. The other settings are the same as in Figure 13.

Table I. The parameters that were manipulated for ICPand NDT on the JUNCTION and TUNNEL and data sets.

Parameter ICP 3D-NDT

Sample ratio • •

Sampling method • •

Initial translation error • •

Initial rotation error • •

Cell size – •

Discretization method – •



smaller cells, the algorithm fails to register the twoscans from many start poses, because of small re-gions of influence. This can be seen in Figure 15,where the upper quartile of the tests with JUNCTIONand 0.75 m cells is comfortably below the acceptablethreshold, but the error of the worst few runs ismuch larger. This result is due to the fact that, de-pending on the direction of the initial pose error, forsome test runs the small cells will not be able to“attract” enough points. For larger cells, the accu-racy decreases because of loss of surface shape infor-mation. Based on these results, a cell size of around1 to 2 m is most suitable for the given environment.

Because simple arrays were used for cell storage�storing both occupied and unoccupied cells�,memory usage increased drastically for the testswith the smallest cells. This also led to slower per-formance because of memory swapping, particularlyfor the JUNCTION data set. The times reported herewere measured with the ANSI C clock�� function,which only measures CPU time. The actual time waslarger for the tests with 0.5 m cell size. A straightfor-

ward way to fix this problem would be to store thecells in a data type more suitable for sparsely popu-lated data �for example, run-length encoded lists�.For all other tests, where the NDT cells were notpathologically small, memory allocation was not aproblem and the reported time and wall clock timewere the same.

Initial error: The sensitivity of the algorithmswith respect to the amount of error in the initial poseestimate was also tested, both for the translationaland rotational components. The results are shown inFigures 16 and 17. For the translation error tests, theinitial rotation error was set to zero, and the transla-tion error was set to zero when testing the sensitivityto the initial rotation error.

Again, 3D-NDT shows a smaller median error inmost cases, although failed registrations start to oc-cur at smaller values for the initial error than is thecase for ICP. The reason that the median error issmaller for 3D-NDT is probably because the PDFsare a better surface description than point clouds,which have no information about the surface be-

Figure 15. Comparing the effect of 3D-NDT registration with different cell sizes, using fixed cells. Each test sequence is50 runs. The initial error is �et�=1 and �er�=0.1.



tween points. Without infinite outer bounds, fewerpoints from the data scan are used when the initialpose error is large. With the baseline settings for ICP,an initial translation error of up to 2.5 m �when theerror in rotation is zero� or a rotation error of up to0.35 rad �when the translation error is zero� can behandled reliably for the JUNCTION data set. Using3D-NDT, failed registrations start to occur at 2 mtranslation error or 0.3 rad rotation error. The resultsfor the TUNNEL set show the same tendencies.

The time taken by ICP increases with the mag-nitude of the initial pose error, while 3D-NDT takesabout the same amount of time for all of the runs.

Discretization methods: Test results for 3D-NDT with different discretization methods on theJUNCTION and TUNNEL data sets are shown in Figure18. For these tests, cell sizes varying between 2 and1 m were used. The results for fixed 2 m cells areshown for comparison. The fixed cell plots are la-belled F �without infinite bounds� and FI �with infi-nite bounds�. With the initial error set according tothe baseline setup, all methods performed equally

well on the JUNCTION set. To show the differences inthe methods’ efficiency, the initial pose error was in-creased a little for the tests on the JUNCTION set sothat er=0.2. The different discretization methods aredescribed in Section 4.3.

• Octree subdivision �O, OI� did not lead to anoticeable improvement for the JUNCTION

data set. A probable reason for this is that theadded detail was not needed for this data set,as it has clear and large features. Octree sub-division did improve the result for the TUN-

NEL data set, approximately halving the me-dian error compared to using fixed cells.

• Additive octree subdivision �A, AI�—computing the score for each point by sum-ming all leaves in the octree where it belongsinstead of using a single leaf—improved theresult of the TUNNEL set slightly, at the cost ofa minor increase in execution time, becausemore cells were investigated for each point.However, for an unknown reason, 3D-NDT

Figure 16. Comparing the sensitivity to the initial error in the translation estimate for the JUNCTION set. The initialrotation error was 0 for these tests.



with additive octree subdivision failed fortwo of the initial pose estimates when run-ning on the JUNCTION data set. The results forthe other 98 poses were still satisfactory.

• Iterative subdivision with varying cell size �I,II�—the more “brute-force” method—removed all of the failed registrations for theJUNCTION data set, at the cost of longer execu-tion times. Iterative subdivision and additivesubdivision with infinite outer bounds werethe only methods that succeeded in accu-rately registering the TUNNEL data set from atleast 75% of the inital poses. For the testsshown here, the first iteration used 2 m cells.For each subsequent iteration, the cell sizewas multiplied by 0.75, and the registrationwas stopped when the size was smaller than1 m. In other words, the cell sizes used were2, 1.5, and 1.125 m, respectively.

• Using linked cells led to a slight improve-ment for the TUNNEL data set, especially forthe rotation component of the pose. Interest-

ingly, it did not lead to an improvement forthe JUNCTION data set. The likely reasons forthis are that, firstly, the error in the initialpose estimate was not large enough for theouter cells to have any significant effect, and,secondly, that the scans overlap completely.

Based on these results, the best performance was ob-tained using iterative subdivision with infinite outerbounds, at a slightly higher computational cost thanthe noniterative variants of 3D-NDT, though it wasstill faster than ICP.

5.2.2. Results with Mobile Robot Data

The KVARNTORP-LOOP data set contains scans col-lected by a mobile robot, together with pose esti-mates for each scan, derived from the robot’s two-dimensional odometry. This is more like the actualsituation that can be expected in the mine mappingapplication than the artificial �but more complete�experimental setup used for the other two data sets.

Figure 17. Comparing the sensitivity to the initial error in the rotation estimate for the JUNCTION set. The initial transla-tion error was 0 for these tests.



The more artificial setup can be considered morecomplete because, for those experiments, the algo-rithms were tested from a larger set of possible start-ing poses, and the properties of the algorithms wereinvestigated more thoroughly.

For the results presented here, 8000 randomsample points �around 8%� from the data scan andall points from the model were used. Infinite outerbounds were used for 3D-NDT, but not linked cells.The following text covers the effects of using differ-ent cell sizes and discretization methods.

Because of some practical problems during thedata collection in the Kvarntorp mine, the odometryhad to be reset at three points �after scans number11, 16, and 66�. These results are for the longest con-secutive scan sequence �scans 17–66�.

The pose error from odometry was up to around1.5 m and 0.2 rad from one scan to the next. Giventhat the size of each scan is around 10 by 30 m, arotation error of 0.2 rad is quite large. An example of

how bad the odometry can be when driving ongravel with a small mobile robot is shown in Figure19. Scan 49 is severely rotated with respect to theprevious scan, which was taken just 5 m earlier.Measuring the turn angle from odometry is alwaysproblematic, and especially so when driving over asurface with loose rocks.

The results are presented as histograms in Fig-ures 20–23. Two limits were chosen for each compo-nent of the error of the pose estimate after registra-tion. Because of the difficulty of finding a realground truth pose, all registrations that came withina certain limit of the manually determined true posewere considered successful. The ground truth poseswere determined by running and inspecting a num-ber of registration attempts, and an average of thebest matches was used as the ground truth pose foreach scan pair. A second limit was also picked. Reg-istrations that came inside this limit are not exactmatches, but “acceptably” close for the application.

Figure 18. Comparing different discretization methods for 3D-NDT on the JUNCTION and TUNNEL data sets. For theJUNCTION tests, et=1 m and er=0.2 rad. For TUNNEL, et=1 m and er=0.1 rad. Baseline ICP is on the left. The next two plots�F and FI� show 3D-NDT with fixed cells, O and OI show octree subdivision, A and AI show additive subdivision, and Iand II show iterative subdivision, The rightmost plot in each NDT plot pair ��I� uses infinite outer bounds but not linkedcells �not applicable for ICP�.



The limits for this data set were chosen to be 0.10 mand 0.005 rad for “good” matches, and 0.20 m and0.010 rad for “acceptable” matches. Registrationswhere any of the pose components are outside ofthis limit were regarded as failures. The most impor-tant feature of the plots to judge the quality of eachregistration algorithm is the height of the leftmosthistogram box, showing the number of successfulregistrations. The histogram boxes that only haveone entry are labelled with the corresponding scannumber, to make it clearer which scans fail to beregistered. Also included in the plots are box plotsshowing the distribution of the results.

The results from using 3D-NDT with fixed cellswith different sizes are shown in Figure 20. Whenthe cells are too small �0.5 m�, scans where theodometry pose is too far from the actual pose fail.When the cells are too large, features that are neededfor accurate registration are smoothed out, also mak-ing registration fail in more cases. Looking at Figure20, a cell size of around 2 m seems to be the prefer-able choice for this data set.

The orientation was generally easier to get rightthan the position, because the large-scale features ofthe tunnel scans were sufficient to get the correctrotation angle.

Figure 21 shows the results of different adaptivesubdivision methods; starting with 2 m cells, andusing cells with 1 and 0.5 m side length as needed.Octree subdivision improves the registration of anumber of the scans, compared to using fixed 2 mcells, resulting in 40 successful registrations. Using

additive subdivision instead of standard octree sub-division did not lead to an additional improvementfor the KVARNTORP-LOOP data set. Iterative subdivi-sion, however, registered 45 of the 50 scan pairs withvery high accuracy, and only failed with two scans—the difficult scans number 49 and 41. It is interestingto note that only the rotation component of scan 41’spose and only the translation component of scan 49’spose were wrong. The time needed for 3D-NDT withiterative subdivision was longer than for the othersubdivision methods, because two extra runs of thealgorithm were performed for each scan. However,the increase is not linearly proportional to the num-ber of iterations. Iterative 3D-NDT took about twiceas long as a single iteration of the other versions ofthe algorithm, even though three passes were per-formed for each scan. The reason for this is that inmost cases, the scans are already in registration atthe last pass, so that the last iteration is very fast.

Figure 22 shows the results of registering thesame data set with iterative 3D-NDT, with and with-out infinite outer bounds. Using linked cells did notgive an improvement for these scans. When not us-ing infinite outer bounds, the translation componentof scan 41 and the rotation component of scan 48’sfinal pose estimate were worse than when using in-finite bounds. This shows that using infinite boundsfor the outer cells helps in some cases. Apart fromthat, the results were very similar to when using in-finite bounds.

The KVARNTORP-LOOP data set was also regis-tered with ICP. For this experiment, a decreasing dis-tance threshold was used, starting at 2 m and de-creasing to zero, instead of the fixed 1 m thresholdfrom the baseline setup. The results are shown inFigure 23. The number of successful registrationswas comparable to that of 3D-NDT, though ICP hada few more failures. The main difference lies in therunning time of the two algorithms. 3D-NDT wastypically almost three times faster than ICP whenusing the same sampling ratio.

6. SUMMARY AND CONCLUSIONS

A new method for registration of 3D range scans, 3D-NDT, has been presented. A detailed analysis of thealgorithm with respect to different methods and pa-rameters based on real-world experiments in a mine

Figure 19. Scans 48 �light, yellow� and 49 �dark, blue�before registration, seen from above.



has also been presented, along with a comparison toICP, the most common registration algorithm used to-day. The main reason why 3D-NDT is faster is be-cause it avoids the computationally challengingnearest-neighbor search, which is central to the ICP

algorithm. Using iterative subdivision for buildingNDT’s model surface description overcomes theproblems associated with discretizing the scan vol-ume into fixed grid cells. It has been shown that 3D-NDT with iterative subdivision and infinite outer

Figure 20. NDT with fixed cells, ranging from 0.5 m �top� to 3 m �bottom�. In order to make the plots easier to read, thescan labels are not shown in these plots.



Figure 22. NDT with �top row� and without �bottom row� infinite outer bounds. Using linked cells did not give anoticeable improvement for this data set, but increased the time substantially.

Figure 21. 3D-NDT with different discretization methods. Octree split top, iterative split at bottom. Cells with sizes 2; 1;and 0.5 m were used. Iterative subdivision is clearly the best choice here, as it has only two failed registrations �theposition of scan 49 and the orientation of scan 41� and three “acceptable” matches. The time taken is about twice that ofoctree subdivision. Additive octrees and standard octrees had very similar performance for this data set.



bounds consistently leads to accurate registration ofdifficult scan data, requiring less time than ICP.

With the experimental setup used in the workpresented here, the alignment speed is not critical.Since the vehicle is stopped and moved betweenscans, the few seconds saved from using a faster reg-istration algorithm are not very important. In the realapplication, however, the plan is to collect 3D scandata while the vehicle is moving, possibly using a 3Dlidar camera that collects a full-frame range image atvide frame rates. Also, the computer hardware on theintended platform �Atlas Copco’s drill rigs� is slowerthan the system used for these tests. In such a sce-nario, the need for fast scan alignment will be muchhigher.

One of the other main advantages of 3D-NDT isthat the scanned surface can be stored much more ef-ficiently using the combined normal distributionsthan if the point clouds themselves, or even sparsesubsamples of the point clouds, are stored. This is im-portant for any large 3D map. In a scenario where adynamic map is maintained over a long time, thestorage requirements for 3D point cloud data wouldalso soon grow uncomfortably large. Storing the NDT

representations of the scans requires only a smallfraction of the space required by ICP, and the NDTrepresentation is still powerful enough for registeringnew scans to the collected data, as has been shown inthis paper.

ACKNOWLEDGMENTS

Thanks to Atlas Copco Rock Drills AB for financialsupport and access to the mine and equipment, andto Optab Optronikinnovation AB for providing thelaser scanner used for some of the experiments.

REFERENCES

Besl, P. J., & McKay, N. D. �1992�. A method for registra-tion of 3-D shapes. IEEE Transactions on PatternAnalysis and Machine Intelligence, 14�2�, 239–256.

Biber, P., & Strasser, W. �2003�. The normal distributionstransform: A new approach to laser scan matching. InProceedings of the IEEE International Conference onIntelligent Robots and Systems �IROS�, Vol. 3, pp.2743–2748.

Figure 23. Results of using ICP on the KVARNTORP-LOOP data set, using a decreasing distance threshold of 2 m to 0 m.The results using iterative 3D-NDT are shown above for comparison.



Chen, Y., & Medioni, G. �1992�. Object modelling by regis-tration of multiple range images. Image and VisionComputing, 10�3�, 145–155.

Gelfand, N., Ikemoto, L., Rusinkiewicz, S., & Levoy, M.�2003�. Geometrically stable sampling for the ICP al-gorithm. In Proceedings of the 4th International Con-ference on 3-D Digital Imaging and Modeling, pp.260–267, Banff, October 2003.

Greenspan, M., & Yurick, M. �2003�. Approximate k-d treesearch for efficient ICP. Paper presented at Fourth In-ternational Conference on 3-D Digital Imaging andModeling �3DIM ’03�.

Mitra, N. J., Gelfand, N., Pottmann, H., & Guibas, L.�2004�. Registration of point cloud data from a geo-metric optimization perspective. In Proceedings of the2004 Eurographics/ACM SIGGRAPH Symposium onGeometry Processing, pp. 23–31, ACM Press, NewYork, NY.

Ripperda, N., & Brenner, C. �2005�. Marker-free registra-

tion of terrestrial laser scans using the normal distri-bution transform. In Proceedings of the ISPRS Work-ing Group V/4 Workshop 3D-ARCH 2005, Mestre-Venice, Italy, 22-24 August, 2005.

Rusinkiewicz, S. M. �2001�. Efficient variants of the ICPalgorithm. In Proceedings of the 3rd InternationalConference on 3-D Digital Imaging and Modeling, pp.145–152.

Takeuchi, E., & Tsubouchi, T. �2006�. A 3-D scan matchingusing improved 3-D normal distributions transformfor mobile robotic mapping. In Proceedings of theIEEE International Conference on Intelligent Robotsand Systems �IROS�, October 2006, pp. 3068–3073,Beijing, China.

Wulf, O., & Wagner, B. �2003�. Fast 3D-scanning methodsfor laser measurement systems. Paper presented atthe International Conference on Control Systems andComputer Science �CSCS14�.



Scan registration for autonomous mining vehicles using 3D-NDTeprints.lincoln.ac.uk/1615/1/mm_al_td_jfr2007.pdf · Scan Registration for Autonomous Mining Vehicles Using 3D-NDT Martin

Documents