6D SLAM — PRELIMINARY REPORT ONCLOSING THE LOOP IN SIX DIMENSIONS

Hartmut Surmann Andreas NüchterKai Lingemann Joachim Hertzberg

Fraunhofer Institute for Autonomous Intelligent SystemsSchloss Birlinghoven

D-53754 Sankt Augustin, Germany

Abstract: To create 3D volumetric maps of scenes with autonomous mobile robotsit is necessary to gage several 3D scans and to merge them into one consistent 3Dmodel. This paper provides a new solution to the simultaneous localization andmapping (SLAM) problem with six degrees of freedom. Robot motion on naturalsurfaces has to cope with yaw, pitch and roll angles, turning pose estimation intoa problem in six mathematical dimensions. A fast variant of the Iterative ClosestPoints (ICP) algorithm registers the 3D scans in a common coordinate system andrelocalizes the robot. Finally, consistent 3D maps are generated using closing loopdetection and a global relaxation.

Keywords: autonomous mobile robots, 3D laser scanner, 3D scan matching,simultaneous localization and mapping (SLAM), closing loop.

1. INTRODUCTION

The problem of automatic environment sensingand modeling in 3D is complex, because a numberof fundamental scientific issues are involved. Oneis the control of an autonomous mobile robotand scanning the environment with a 3D sensor.Another question is how to create a volumetricconsistent scene in a common coordinate systemfrom multiple views. This problem is addressedhere: The proposed algorithms allows to digitizelarge environments fast and reliably without anyintervention and solve the simultaneous localiza-tion and mapping (SLAM) problem. Finally, robotmotion on natural outdoor surfaces has to copewith yaw, pitch and roll angles, turning pose es-timation as well as scan matching or registrationinto a problem in six mathematical dimensions.This paper presents a new solution to the SLAMproblem with six degrees of freedom and focuseson closing the loops in 6D. A fast variant of theiterative closest points algorithm registers the 3D

scans in a common coordinate system and relocal-izes the robot. The computational requirementsare reduced by two new methods: First we reducethe 3D data and second we approximate the clos-est point search. Overall consistent 3D maps aregenerated using a global relaxation after a closedloop has been detected.

The paper is organized as follows: The next sec-tion describes the state of the art. Section 3presents the autonomous mobile robot and the3D laser range finder. Section 4 presents the reg-istration algorithms, a ground truth experimentsand solution to the SLAM problem. Furthermorewe show how data reduction and access meth-ods speed up computation and the methods be-come feasible. The next section presents the re-sults. Finally section 7 concludes the paper. Thepaper is accompanied by a video including the3D map. The video is available for download atwww.ais.fraunhofer.de/ARC/3D/6D/.

2. RELATED WORK

Some groups have attempted to build 3D volu-metric representations of environments with 2Dlaser range finders. In (Thrun et al., 2000) two2D laser range finders are used for acquiring 3Ddata. One laser scanner is mounted horizontallyand one is mounted vertically. The latter onegrabs a vertical scan line which is transformedinto 3D points using the current robot pose. Thisapproach has difficulties to navigate around 3Dobstacles with jutting out edges. They are onlydetected while passing them. The published 2Dprobabilistic localization approaches, e.g., Markovmodels or Monte Carlo methods work well in flatand structured 2D environments but an extensionin the third dimension is still missing since thealgorithm do not scale with additional dimensions.

A few other groups use 3D laser scanners (Sequeiraet al., 1999; Allen et al., 2001). A 3D laser scan-ner generates consistent 3D data points within asingle 3D scan. The RESOLV project aimed tomodel interiors for virtual reality and telepresence(Sequeira et al., 1999). They used a RIEGL laserrange finder on a robot, and the ICP algorithm forscan matching. The AVENUE project develops arobot for modeling urban environments using aCYRAX laser scanner and a feature-based scanmatching approach for registration of the 3D scansin a common coordinate system (Allen et al.,2001). The research group of M. Hebert has recon-structed environments using the Zoller+Fröhlichlaser scanner and aims to build 3D models withoutinitial position estimates, like without odometryinformation (Hebert et al., 2001).

3. AUTOMATIC 3D SCANNING

3.1 The AIS 3D Laser Range Finder

The AIS 3D laser range finder (Surmann et al.,2001) is built on the basis of a 2D range finder byextension with a mount and a servomotor. The2D laser range finder is attached to the mount forbeing rotated. The rotation axis is horizontal. Astandard servo is connected on the left side (Fig.1) (Surmann et al., 2001).

The area of 180◦(h) × 120◦(v) is scanned withdifferent horizontal (181, 361, 721) and vertical(128, 256, 512) resolutions. A plane with 181data points is scanned in 13 ms by the 2D laserrange finder (rotating mirror device). Planes withmore data points, e.g., 361, 721, duplicate orquadruplicate this time. Thus a scan with 181 ×256 data points needs 3.4 seconds. In addition tothe distance measurement the AIS 3D laser rangefinder is capable of quantifying the amount of lightreturning to the scanner.

Fig. 1. The robot platform Kurt3D with the 3D scanner.

3.2 The Autonomous Mobile Robot Kurt3D

Kurt3D (figure 1) is a mobile robot platform witha size of 45 cm (length) × 33 cm (width) × 26 cm(height) and a weight of 15.6 kg. Equipped withthe 3D laser range finder the height increases to47 cm and weight increases to 22.6 kg. Kurt3D’smaximum velocity is 5.2 m/s (autonomously con-trolled 4.0 m/s). Two 90W motors are used topower the 6 wheels, whereas the front and rearwheels have no tread pattern to enhance rotating.Kurt3D operates for about 4 hours with one bat-tery (28 NiMH cells, capacity: 4500 mAh) charge.The core of the robot is a Pentium-III-600 MHzwith 384 MB RAM running Linux. An embedded16-Bit CMOS microcontroller is used to controlthe motor.

4. RANGE IMAGE REGISTRATION ANDROBOT RELOCALIZATION

Multiple 3D scans are necessary to digitalize envi-ronments without occlusions. To create a correctand consistent model, the scans have to be mergedinto one coordinate system. This process is calledregistration. If the localization of the robot withthe 3D scanner were precise, the registration couldbe done directly by the robot pose. However, dueto the unprecise robot sensors, the self localiza-tion is erroneous, so the geometric structure ofoverlapping 3D scans has to be considered forregistration.

The matching of 3D scans can either operateon the whole three-dimensional scan point set orcan be reduced to the problem of scan matchingin 2D by extracting, e.g., a horizontal plane offixed height from both scans, merging these 2Dscans and applying the resulting translation androtation matrix to all points of the corresponding3D scan.

Matching of complete 3D scans has the advantageof having a larger set of attributes (either puredata points or extracted features) to compare thescans. This results in higher precision and lowersthe possibility of running into a local minimumof the cost function. Furthermore, using threedimensions enables the robot control software torecognize and take into account changes of heightand roll, yaw and pitch angles of the robot. This6D robot relocalization is essential for robotsdriving cross country.

4.1 Matching as an Optimization Problem

The following method for registration of pointsets is part of many publications, so only a shortsummary is given here. The complete algorithmwas invented in 1992 and can be found, e.g., in(Besl and McKay, 1992). The method is calledIterative Closest Points (ICP) algorithm.

Given two independently acquired sets of 3Dpoints, M (model set, |M | = Nm) and D (dataset, |D| = Nd) which correspond to a single shape,we want to find the transformation consisting ofa rotation R and a translation t which minimizesthe following cost function:

E(R, t) =Nm∑

i=1

Nd∑

j=1

wi,j ||mi − (Rdj + t)||2 . (1)

wi,j is assigned 1 if the i-th point of M describesthe same point in space as the j-th point ofD. Otherwise wi,j is 0. Two things have to becalculated: First, the corresponding points, andsecond, the transformation (R, t) that minimizeE(R, t) on the base of the corresponding points.

The ICP algorithm calculates iteratively a localminimum of equation (1). In each iteration step,the algorithm selects the closest points as corre-spondences wi,j and calculates the transformation(R, t) for minimizing equation (1). Fig. 2 showsthree steps of the ICP algorithm. Besl et al. provesthat the method terminates in a minimum (Besland McKay, 1992). The assumption is that in thelast iteration step the point correspondences arecorrect.

In each ICP iteration, the transformation is cal-culated by the quaternion based method of Horn(Horn, 1987). A unit quaternion is a 4 vectorq̇ = (q0, qx, qy, qz)T , where q20 + q

2x + q2y + q2z =

1, q0 ≥ 0. It describes a rotation axis and an angleto rotate around that axis. The rotation expressedas quaternion that minimizes equation (1) is thelargest eigenvalue of the cross-covariance matrix(Besl and McKay, 1992). This calculation is com-putationally inexpensive. The transformation t iscomputed using the rotation and the two centroids

of the 3D points sets (Horn, 1987). Fig. 2 showstwo 3D scans in their initial, i.e., odometry-basedpose, after 5 iterations, and the final pose. 55iterations are needed to align these two 3D scanscorrectly.

In order to use the ICP algorithm for mobile robotapplications an analysis of the registration processhas been made. To compare the registration withthe ground truth several experiments have to bemade. We have made an experiment in a flat, verystructured environment (Fig. 2) and measured theground truth with a meter rule (scan pose 1: (x, z,θ) = (0 cm, 0 cm, 0◦), scan pose 2 (120 cm, 301 cm,0◦). It turned out, that the automatic alignment ofthe 3D scans (116 cm, 301 cm, 0◦) is close to thatground truth pose. Fig. 3 shows the results of thealignment of two 3D scans with different startingguesses. It turned out that the angle of the robotis more important than the position. Nevertheless,the main parameter of 3D scan matching is theform of the 3D surface.

4.2 Matching Multiple 3D Scans

To digitalize environments, multiple 3D scanshave to be registered. After registration, the scenehas to be globally consistent. A straightforwardmethod for aligning several 3D scans is pairwisematching, i.e., the new scan is registered againstthe scan with the largest overlapping areas. Thelatter one is determined in a preprocessing step.Alternatively, in (Chen and Medoni, 1991) anincremental matching method is introduced, i.e.,the new scan is registered against a so-calledmetascan, which is the union of the previouslyacquired and registered scans. Each scan matchinghas a limited precision. Both methods accumulatethe registration errors such that the registrationof many scans leads to inconsistent scenes and toproblems with the robot localization.

4.2.1. Closing the Loop. After matching mul-tiple 3D scans, errors have accumulated and aclosed loop will be inconsistent (Fig. 4). Our al-gorithm detects a closing loop by registering thelast acquired 3D scan with earlier acquired scans,e.g., the first scan. If a registration is possible, thecomputed error is in a first step divided by thenumber of 3D scans in the loop and distributedover all scans (Fig. 4). A second step minimizesthe global error with the following algorithm.

4.2.2. Diffusing the Error. Pulli presents a reg-istration method that minimizes the global errorand avoids inconsistent scenes (Pulli, 1999). Thismethod distributes the global error while the reg-istration of one scan is followed by registration of

Fig. 2. Left: Initial odometry based pose of two 3D scans. Middle: Pose after five ICP iterations. Right: final alignment.

10000

6000

20000

−200 −100 0 100 200 −200−100

0100

200

e(x,y,−60°)e(x,y,−60°) < 5000

10000

6000

20000

−200 −100 0 100 200 −200−100

0100

200

e(x,y,−20°)

e(x,y,−20°) < 3000e(x,y,−20°) < 4000

e(x,y,−20°) < 2000e(x,y,−20°) < 1000

matchable positions−200

−100

0

100

200−100 0 100 200−200

[−80

,80]

−200 −100 0 100 200 −200−100

0100

200

02000

6000

10000

e(x,y,0°)e(x,y,0) < 1000e(x,y,0) < 500

−200 −100 0 100 200 −200−100

0100

200

02000

6000

10000

e(x,y,20°)e(x,y,20°) < 4000e(x,y,20°) < 3000e(x,y,20°) < 2000e(x,y,20°) < 1000

−80

−40

0

40

80

−200 −100 0 100 200

matchable positions

Fig. 3. Left and middle: Error measurements for 3D scan registration (Fig. 2) against the ground truth pose (120 cm, 301 cm,

-45◦). The Error function is e(x, z, θ) = ||(x, z)− (120, 301)|| + γ∣∣∣∣θ − π

4

∣∣∣∣. Results with θ ∈ {−60,−20, 0, 20} aregiven. Right: The poses are marked in (x, z, θ) from which a correct alignment of two 3D scans is possible.

all neighboring scans. Other matching approacheswith global error minimization have been pub-lished, e.g., (Benjemaa and Schmitt, 1997) and(Eggert et al., 1998).

Based on the idea of Pulli we designed a methodcalled simultaneous matching. Thereby, the firstscan is the masterscan and determines the coor-dinate system. This scan is fixed. The followingthree steps register all scans and minimize theglobal error, after a queue is initialized with thefirst scan of the closed loop:

(1) The current scan is the first scan of thequeue. This scan is removed from the queue.

(2) If the current scan is not the master scan,then a set of neighbors (set of all scans thatoverlap with the current scan) is calculated.This set of neighbors forms one point set M .The current scan forms the data point set Dand is aligned with the ICP algorithms. Onescan overlaps with another if more than 250corresponding point pairs exist.

(3) If the current scan changes its location byapplying the transformation (translation orrotation), then each single scan of the set ofneighbors that is not in the queue is addedto the end of the queue.

In contrast to Pulli’s approach, the proposedmethod is totally automatic and no interactivepairwise alignment has to be done. Furthermorethe point pairs are not fixed (Pulli, 1999). Theaccumulated alignment error is spread over thewhole set of acquired 3D scans. This diffuses thealignment error equally over the set of 3D scans(Fig. 4).

4.3 Data Reduction and Access

The computational expense of the ICP algorithmmainly depends on the number of points. In abrute force implementation the point pairing isin O(n2). Data reduction reduces the time re-quired for matching. The reduction proposed hereconsiders the procedure of the scanning process,i.e., the spherical and continuous measurement ofthe laser. Scanning is noisy and small errors mayoccur. Two kinds of errors mainly occur: Gaussiannoise and so called salt and pepper noise. Thelatter one occurs for example at edges, wherethe laser beam of the scanner hits two surfaces,resulting in a mean and erroneous data value.Furthermore reflections lead to suspicious data.Without filtering, only a few outliers lead to mul-

Fig. 4. Left: Initial poses of 3D scans when closing the loop. Middle: Poses after detecting the loop and equally sharingthe resulting error. Right: Final alignment after error diffusion with correct alignment of the edge structure at theceiling.

tiple wrong point pairs during the matching phaseand results in an incorrect 3D scan alignment.

We propose a fast filtering method to reduceand smooth the data for the ICP algorithm. Thefilter is applied to each 2D scan slice, containingup to 721 data points. It is a combination ofa median and a reduction filter. The medianfilter removes the outliers by replacing a datapoint with the median value of the n surroundingpoints (here: n = 7). The neighbor points aredetermined according to their number within the2D scan slice, since the laser scanner provides thedata sorted in a counter-clockwise direction. Themedian value is calculated with regards to theEuclidian distance of the data points to the pointof origin. The data reduction works as follows:The scanner emits the laser beams in a sphericalway, such that the data points close to the sourceare more dense. Multiple data points located closetogether are joined into one point. This reductionlowers the Gaussian noise. Finally, the data for thescan matching is collected from every third scanslice. This fast vertical reduction yields a goodsurface description. The number of these so calledreduced points is one order of magnitude smallerthan the original one. Data reduction and filteringare online algorithms and run in parallel to the 3Dscanning.

The ICP algorithms spends most of its time dur-ing creating of the point pairs. kD-trees (herek = 3) have been suggested for speed up thedata access (Simon et al., 1994; Zhang, 1992).The kd-trees are a binary tree data structure withterminal buckets. The data is stored in the bucketsand the keys are selected, such that a data space isdivided into two equal parts. This ensures that adata point can be selected in O(log n). In additionthe time spent on creating the tree is important.

Recently Approximate kd-trees (Apr-kd-tree) havebeen introduced (Greenspan and Yurick, 2003).The idea behind this is to return as an approx-imate nearest neighbor pa the closest point pb

�������������������������

�������������������������

����������

����������

������������������������

��������

�����

�����

�����

������������������

�

������� ���

�������������������������

������������������������

������

���������������������������

���������������������������

����������

����������

�������������������

�������������������

������ � �

!!!!!!

""""""

#�##�##�##�##�##�##�##�##�#

$$$$$$$$$

1254

900

800

700

6001 2 3 4 5 6 7 8 9 10 11 12 13 14 15 25 50

approximate kd−treekd−tree

Tim

e [m

s]

Bucket size

Fig. 5. Running time of scan registration using kd-treesearch and approximate kd-tree search.

in the bucket region of the kD-tree where p lies.This value is determined from the depth-firstsearch, thus expensive Ball-Within-Bounds testsand backtracking are not used (Greenspan andYurick, 2003). In addition to these ideas we avoidthe linear search within the bucket. During thecomputation of the Apr-kd-tree the mean valuesof the points within a bucket are computed andstored. Then the mean value of the bucket is usedas approximate nearest neighbor, replacing thelinear search.

The search using the Apr-kd-tree is applied untilthe error function (1) stops decreasing. To avoidmisalignments due to the approximation, the reg-istration algorithm is restarted using the normalkd-tree search. A few iterations (usually 1-3) areneeded for this final corrections. Figure 5 showsthe registration time of two 3D scans in depen-dence to the bucket size using kd-tree or Apr-kd-tree search. Both search methods have their bestperformance with a bucket size of 10 points.

5. RESULTS

To visualize the scanned 3D data, a viewer pro-gram based on OpenGL has been implemented.The task of this program is to project a 3D sceneto the image plane, i.e., the monitor, such thatthe data can be drawn and inspected from everyperspective. Fig. 2, 4 and 6 show rendered 3Dscans.

Table 1. Computing time and number of ICP iterationsto align all 32 3D scans (Pentium-IV-2400). In additionthe computing time for the SLAM algorithm (closed loopdetection and simultaneous matching) is given. About1 min per 3D scan is needed to do error diffusion in 6D.

points & search method time iter.

all pts. & brute force 144 h 5 min 2080all pts. & kD–tree 12 min 23 s 2080all pts. & Apx-kD–tree 10 min 1 s 2080red. pts. & Apx-kD–tree