Robust 3D-Mapping with Time-of-Flight Cameras

Stefan May, David Droscheland Dirk Holz

Fraunhofer IAISstefan may@arcor.de

Stefan FuchsGerman Aerospace Center (DLR)

Inst. of Robotics and Mechatronicsstefan.fuchs@dlr.de

Andreas NuchterJacobs University Bremenandreas@nuechti.de

Abstract— Time-of-Flight cameras constitute a smart and fasttechnology for 3D perception but lack in measurement precisionand robustness. We present a comprehensive approach for 3Denvironment mapping based on this technology. Imprecisionofdepth measurements are properly handled by calibration andapplication of several filters. Robust registration is performedby a novel extension to the Iterative Closest Point algorithm.Remaining registration errors are reduced by global relaxationafter loop-closure and surface smoothing. A laboratory groundtruth evaluation is provided as well as 3D mapping experimentsin a larger indoor environment.

I. INTRODUCTION

The mapping task poses a subset of the well-knownsimul-taneous localization and mapping (SLAM)problem. Thrunet. al. declare mapping as to be one of the “core competenciesof truly autonomous robots” [19]. The purpose of SLAM is tolocate the robot, targets and obstacles, which is fundamentalfor path planning methods. Generating maps is a so-called“chicken-and-egg” problem. If the location of the robot isknown, mapping is a solvable task. In reverse, localizationisstraightforward, if a perfect map is available. But combiningboth turns out to be a challenging problem referred to as theSLAM or concurrent mapping and localization problem.

Because of their high measurement range and precision,laser scanners and stereo camera systems are mostly usedfor SLAM so far. But there are some restrictions: Stereovision requires the matching of corresponding points fromtwo images, and laser scanners only measure sequentiallyline by line. In contrast, ToF cameras can bridge the gapby providing2 1

2D images irrespective of textures or illumi-

nation. ToF cameras also allow for higher frame rates andthus enable the consideration of motion. With their smallmanufactured size and little maintenance requirements ToFcameras are serious competitors with laser scanners in thearea of 3D mapping.

Anyhow, up to now ToF cameras did not really findtheir way into 3D mapping especially due to their complexerror characteristics and high measurement noise. Dependingon external interfering factors (e.g., sunlight) and sceneconfigurations, i.e., distances, orientations and reflectivities,the same scene entails large fluctuations in distance measure-ments from different perspectives.

This paper presents a 3D mapping approach that handlesthese problems by calibration and appropriate filtering. Itrelies only on ToF camera data. No additional sensory infor-mation about the sensor’s motion is needed. The approachshows promising results and highlights essential impacts that

(a) (b)

Fig. 1. a) Scenario used for mapping. b) 3D point cloud registered with datataken from a Swissranger SR-3k device (false color code relates distance toorigin of coordinate system).

have to be considered in ToF camera mapping. In orderto motivate further investigations in 3D mapping with ToFcameras, the underlying data is provided by the authors.

The approach comprises: Depth correction by employingan improved calibration, filtering of remaining inaccuracies,registration w.r.t. to a common coordinate system by a novelextension to the Iterative Closest Point (ICP) algorithm andmap refinement including global relaxation - all combinedyielding a precise and consistent 3D map.

The remainder of this paper is organized as follows: Sec-tion II elaborates 3D mapping approaches and applicationsrelated to ToF cameras. Section III describes ToF cameraerrors, caused by external interfering factors, and the em-ployed depth correction method. In Section IV our mappingapproach including 3D pose estimation, error handling andmapping is represented. Section V illustrates experimentalresults that support our accentuation of employing real-timecapable ToF sensors to pose estimation and mapping tasks.Finally, section VI concludes with an outlook on future work.

II. RELATED WORK

A 3D mapping approach tackling large environments waspresented by Nuchter et al. [13] using a 3D laser scannermounted on a mobile robot. Imprecision of inertial sensorswas handled by an ICP approach, both for registering con-secutive scans and forclosing the loop. Here, 3D scans wereacquired in a stop-scan-go manner by a mobile robot, yield-ing locally consistent 3D point clouds. Other approaches inlocalization and mapping, based on 3D data acquired duringmovement, use either multiple 2D laser range finders facingdifferent orientations, e.g., [18], or a single continuouslyrotating laser scanner [3], [21].

Matching two complete point clouds is often trappedin a local minimum, especially if the sensor’s apex angleis small, such as for ToF cameras (compared with laserscanners). The registration result lacks in precision. In 2006Ohno et al. used a ToF camera for estimating a robot’strajectory and reconstructing the surface of the environment[14]. The registration procedure for 3D captures was similarto the scan registration approach used by Nuchter et al.. Thecalculated trajectory was compared with precise referencedata in order to demonstrate the algorithm’s precision. Theestimation error for the robot pose was up to 15 percentin translation and up to 17 percent in rotation, respectively.Also in 2006, Sheh et al. presented an application of ToFcameras in rescue robotics [17]. Their mapping approachwas assisted by a human operator. In 2007 Prusak et al.presented a joint approach for robot navigation with collisionavoidance, pose estimation and map building employinga ToF camera combined with a high-resolution sphericalcamera. Structure from motion was used to estimate an initialguess for the pose change [16]. By the use of the trimmedICP (TrICP) approach, pose changes were finally determined.The TrICP approach extends the ICP algorithm by employingthe estimated degree of overlap, which was available from theinitial guess. Since ToF cameras also provide amplitude data,2D image processing algorithms are usable. Thus, the pointfeature mapping in monochromatic images can be improvedby the associated depth data obtaining a 3D feature trackingas performed in [16].

Results of above mentioned approaches are difficult tocompare due to different underlying data sets. In contrastto the availability of Computer Vision benchmarks, standarddata sets for ToF camera based registrations are not estab-lished since yet. We provide the data sets of our experimentsin order to motivate benchmarking of registration methodsfor the ToF camera technology.

III. DESCRIPTION OF TOF CAMERA ERRORS

The performance of distance measurements with ToFcameras is limited by a number of errors. Some of themare inherent in the measurement principle and cannot becorrected. Remaining other errors are predictable and cor-rectable by calibration due to their systematic occurrence.The following explanations relate to them asnon-systematicerrors andsystematic errors, respectively.

A. NON-SYSTEMATIC ERRORS

There are three significant non-systematic errors. First, abad signal-to-noiseratio distorts the measurement and cannotbe suppressed. A solution is either carefully increasing theexposure time and amplifying the illumination or intelligentamplitude filtering. Second, due to interreflections in thescene the remitted near infrared (NIR) signal is a superposi-tion of NIR light that has travelled different distances. Thisso-calledmultiple ways reflectionlets hollows and cornersappear rounded off and occluding shapes with a smoothtransition. Third, light scattering occurs in the lenses ofthe ToF camera. Thus, near bright objects may superpose

the measurements from the background objects, which forthat appear closer. The latter two effects are unpredictablebecause the topology of the observed scene is unknown apriori.

B. SYSTEMATIC ERRORS

Furthermore, there are three systematic errors. First, thereis a distance-relatederror. The measurement principle isbased on the assumption that the emitted light is sinusoidal,which is only approximately the case. Second, a so-calledamplitude-relatederror is caused by non-linearities of thepixel’s electronic components. As a result, the measureddistance varies with object reflectivity. Third, there is afixedpattern phase noise. Since the pixels on the sensor chip areconnected in series, the triggering of each pixel depends onthe position on chip. The farther the pixel is located withrespect to a signal generator, the higher is its measurementoffset. These three errors are managable by calibration. In[5] and [6] Fuchs et al. described an appropriate calibrationmethod, which estimates above mentioned errors. It is ap-plied for the mapping experiments in this paper.

IV. 3D MAPPING

The here presented approach acquires a metric map inform of a 3D point cloud, whereby the sensor is simulta-neously localized relative to this point cloud without anyexternal positioning system. The 3D mapping is a four-stageprocess. First, invalid data points are discarded by filtering.Second, the map is generated by registering consecutive 3Dcaptures. Third, accumulated errors are relaxed among allcaptures. Finally, the map is enhanced with refinement filters.

1) Filtering: Errors caused by low illumination or occlu-sion are treated by filtering. A high confidence is related to ahigh amplitude (to be precise: this statement is only a com-promise to what the camera provides; see [7] for a descriptionof error influences). Thresholding the amplitude discardsprimarily data resulting from objects with lower infraredreflectivity, higher distance or from objects which are locatedat the peripheral area of the measurement volume (due toinhomogeneous scene illumination). Mismeasurements alsooccur on jump edges, i.e., when the transition from one toanother shape appears disconnected due to occlusions. Thetrue distance changes suddenly for the transition from oneshape to the other whereas ToF cameras measure a smoothtransition. In order to remove these invalid points, jump edgefiltering has been applied [5]. It is important to mention thatthe proposed filter is sensitive to noise, i.e., besides jumpedges, valid points are removed, if noise reduction filtersare not being applied first. The subsequent application ofmedian and jump edge filtering achieved the best results inour experiments.

2) Map Generation: Pose changes are assumed to besmall due to the high frame rate. For that, no additionalsensory data as initial guess is needed (e.g., inertial sen-sors). The estimation is performed relying completely onthe registration of 3D data by the use of the well-knownICP algorithm [1]. It aims at finding a rigid transformation

between a model point setM and a scene point setD byperforming an iterative least square minimization scheme.In each iteration step corresponding closest points are de-termined. Denoting corresponding point pairs as a set ofN tuples{(mi,di)|i = 1 . . .N} and the transformation ascomposition of a rotation matrix and a translation vector(R, t) the error function reads:

E(R, t) =

N∑

i=1

||mi − (Rdi + t)||2. (1)

The solution can be determined by several closed-formalgorithms. A detailed description and evaluation of thesealgorithms is summarized in [11].

The original formulation of the ICP approach assumesthat the scene point set is completely covered by the modelpoint set [1]. If the scene includes points which are not partof the model, wrong correspondences are assigned whichskew the result [4]. The simplest solution to discard scenepoints from a non-overlapping area is the employment ofa distance threshold. Corresponding tupels are rejected, iftheir Euclidean distance exceeds this value. Several strategiesare possible to determine suitable thresholds, e.g., a gradualdecreasing threshold with respect to the iteration step. Ingen-eral, these thresholds increase the registration performancesignificantly on only partially overlapping point clouds. Forconvenience, the original formulation of the ICP approachincluding a distance threshold is calledVanilla ICP approachin the following.

A plain threshold has limitations with respect to robustnessand accuracy. Several approaches have been proposed to im-prove registration results for an unknown degree of overlap.Fusiello et al. employed the X84 rejection rule, which usesrobust estimates for location and scale of a corrupted Gaus-sian distribution [4]. It estimates a suitable rejection thresholdconcerning the distance distribution between correspondingpoints. Niemann et al. proposed a rejection rule that considersmultiple point assignments(Picky ICP algorithm)[12] . Ifmore than one point from the scene point set is assignedto a corresponding model point, only the point with thenearest distance is accepted. Pajdla and Van Gool proposedthe inclusion of a reciprocal rejection rule(Iterative ClosestReciprocal Point algorithm - ICRP)[15]. For a correspondingpoint pair(mi,di), which has been determined by searchingthe nearest neighbor ofdi in M , the search is reversedsubsequently, i.e., formi the nearest neighbor inD isdetermined. This needs not to be the same scene point and isdenoted withd′

i. The point correspondence is rejected ifdi

andd′

i have a distance being larger than a certain threshold.The method proposed here to overcome an unknown

degree of overlap addresses the problem formulation moreprecisely and intuitively. It stems from 3D computer graphicsand is calledfrustum culling [8]. A frustum defines thevolume that has been in the range of vision while acquiringthe model point set (cf. Fig. 2). Luck et al. used frustumculling for pre-filtering based on an initial pose estimate [9].The iterative registration process was then performed on the

Lower clipping plane

Right clipping plane

Upper clipping plane

Left clipping plane Far clipping plane

Focal pointf

d c

ba

Fig. 2. Definition of frustum clipping planes.

reduced data set. Contrary, we employ no pose estimationand thus cannot pre-filter data in order to remove the non-overlapping area. Frustum culling is therefore embedded inthe iteration process by employing the pose estimate of theprevious iteration step. Scene points outside of the modelfrustum are filtered by testing against clipping planes beforeperforming nearest neighbor searching.

Let f = 0 be the focal point and{a,b, c,d} vectors fromf to the four edge points of a far clipping plane. Lateralclipping planes are spanned by each two of adjacent vectors(cf. Fig. 2). The normal vectorsn = (nx, ny, nz)

T of thefour lateral clipping planes are then used to check if a pointx = (x, y, z)

T is inside the frustum by

xnx + znz < 0 (2)

for the left and right clipping planes and

yny + znz < 0 (3)

for the upper and lower clipping planes. The approach isnon-parametric and removes iteratively scene points fromnon-overlapping areas by evaluating their visibility fromthemodel’s viewpoint. During the iteration process scene pointsare clipped as soon as they leave the visibility frustum, whichaddresses exactly the problem formulation. This extensioniscalledFrustum ICPapproach in the following.

3) Error relaxation: Errors sum up due to the limitedsensor precision and accumulation of registration errors frompairwise ICP matching of consecutive frames. However,when the field of view is overlapping with an earlier one, aloop can be closed. This enables SLAM algorithms to boundthe error and to compute a consistent map.

We use a GraphSLAM approach [20], [19] that extendsEq. (1). The graph contains links of all overlapping 3D scansand therefore the loop closing. Instead of minimizing theEuclidean distance between two point pairs, the Euclideandistance between 3D point clouds connected by a graph edge(j, k) is minimized:

E(R, t)=∑

j→k

N∑

i=1

||(Rjpj,i + tj) − (Rkpk,i + tk)||2 (4)

To solve the equation above, we have to fix the first acquired3D point cloud, i.e., setR1 = I and t1 = (0, 0, 0).Unfortunately Eq. (4) cannot be solved in closed form. Usingthe small angle assumption the linearizationsin x ≈ x and

(a) (b)

Fig. 3. a) 3D map before refinement (bird’s view of scene in Fig. 1). b) 3Dmap after refinement. Sparse points are removed and surfacesare smoothed.

cosx ≈ 1 yields an approximation of a rotation matrix:

R =

1 −θz θy

θyθx + θz 1 − θyθxθz −θx

−θy + θxθz θyθz + θx 1

. (5)

Furthermore, we assume that the multiplication of smallvalues results in even smaller values which thus can beomitted. The rotation matrixR then simplifies to

R =

1 −θz θy

θz 1 −θx

−θy θx 1

. (6)

Using these approximations, Eq. (4) is transformed into aquadratic form and solved. Note: This approximation is valid,if we apply the centroid trick, i.e., subtracting the centerofmass calculated from corresponding points in Eq. (1) and (4).This is commonly used for solving the ICP error function [1].A detailed derivation of this global relaxation formula anda solution of the resulting minimization problem is givenin [11].

4) Refinement:The refinement of a 3D map comprisesfiltering of sparse points and approximation of plain patchesto the neighborhood of each point. Removing the set ofsparse pointsS is done by determining the mean distanceof k-nearest neighbors as a density measure,

d(pi) =1

k

k∑

n=1

||(pi − pi,n)||, (7)

S = {pi ∈ P | d(pi) < dth}, (8)

wheredth is a constant threshold. The set of point candidatesQ for the resulting 3D map includes all remaining pointsafter filtering:

Q = P\(J ∪ S). (9)

Compared with laser scanners, the noise level of ToFcameras is much higher. Smooth surfaces appear with acertain thickness of some centimeters. This effect can beseen in Fig. 3(a). For the refinement of a composed 3D pointcloud a principle component analysis (PCA) is performed todetect surface normals. Related pixels are shifted along thesevectors towards the detected surfaces. The resulting 3D mapafter applying the proposed refinement is contrasted in Fig.3(b).

V. EXPERIMENTS AND RESULTS

The following experiments demonstrate the accuracy androbustness of the proposed 3D mapping approach. Therefore,a SR-3k ToF camera with a resolution of176×144 pixels isused. The standard deviation of this camera varies from 20mm to 50 mm, the horizontal and vertical apex angles are45 ◦ and 35 ◦ respectively. At first, laboratory experimentsdemonstrate the reachable accuracy and influences of thelight scattering effect. For this purpose, a ground truthevaluation is provided by an industrial robot arm (KUKA KR16) to which the ToF camera has been attached. The robot’spositioning system has an accuracy of1 mm and 0.1 ◦. Atsecond, the robustness of the 3D mapping approach is shownin a larger environment.

A. EVALUATION MEASURES

The quality of an 3D mapping approach can be ratedeither by comparing the estimated path (ego motion) of thesensor with a ground truth or by comparing the created3D map with a ground truth. The absolute comparison ofposes against this ground truth measure provides only aweak objectivity since registration errors can be compensatedby other registration errors. Therefore, incremental measuresare more suitable in terms of validity, i.e., an integrationof angular and translational error increments induced byregistration of subsequent frames. The translational errormeasure is defined by

einc,∆t =∑

||∆tinc,i||, (10)

where∆tinc,i is the translational registration error of framei. In order to make rotational errors comparable, the rotationaxis has to be considered:

einc,∆θ =∑

|| |∆θr,i|ar,i − |∆θe,i|ae,i||, (11)

where∆θr,i is the ground truth angle around the ground truthrotation axisar,i of frame i, whereas∆θe,i and ae,i con-stitute the estimated counterparts of ICP registration. Thesedefinitions provide an uniform measure for the accumulationof translational and rotational errors, Eq. (11) is similartoEq. (10).Accurate ego motion does not inevitably result in a perfect3D map. Even with the exact sensor localization, inaccuracycan be induced by corrupted depth measurements. Even ifthe sensor is localized exactly, corrupted depth data willresult in an inaccurate map. On this account, the secondmeasure is constituted by theisometry of the resultingmap. Characteristic opposing walls within the scene weremeasured with a tape (cf. Fig. 4).

B. CALIBRATION AND LIGHT SCATTERING

First, the achievable accuracy in ego motion estimationwas investigated in applying two different trajectories. Fig.5 and Fig. 6 depict scenes and performed paths. In thefirst scene (SI, cf. Fig. 5) the camera moves around a basicgeometric Styrofoam object. Contrary, the object was movedaround the camera in the second scene (SII, cf. Fig. 6). Datatakes for both trajectories were performed twice: Primary,

hor. dist. vert. dist./ mm / mm

Ground truth(meas. tape) 1715 1405DefaultCalibration 1800 1570ImprovedCalibration 1750 1425

Fig. 4. Isometry of resulting 3D map of the laboratory scene (SIII). Thedistances of opposing walls were manually measured and assumed to be theground truth. The improved calibration reduces deviationsfrom the groundtruth and provide a more accurate mapping result.

trans. error rot. err./ mm / ◦

DefaultCalibration 39.7 4.8ImprovedCalibration 28.2 2.4

Fig. 5. Experimental setup (SI) for identifying the accuracy and demon-strating the impact of calibration. The camera is rotated by90 deg aroundan object in a distance of600 mm while keeping it in the center of the fieldof view. Thus, a distance of950 mm is covered. Both, the translational errorand the rotational error decrease due to the proper calibration.

the scenes were captured with the default (manufacturer’s)calibration. This calibration considers fixed pattern phasenoise. Then, the scenes were captured with an improvedcalibration. The camera was calibrated according to Fuchset. al. [6] in order to additionally consider distance-relatederrors. This approach employs a spline for estimating a pixel-wise depth measurement correction value. Fig. 8 depicts thecomputed correction spline.

In both scenes the calibration reduces the error in egomotion estimation by≈ 25 %. But, compared to the length ofboth trajectories, the translational error inSII is significantlylarger (20 %) compared to the error resulting fromSI (3 %).We conclude that the type of movement is crucial for theresult. Especially inSII, the observed object is moving atthe margin of the field of view where the low resolution andsmall apex angle handicap the ego motion estimation.

In addition, the light scattering has to be considered, asthe second investigation shows. Here, we induced scatteringeffects by adding an Styrofoam object toSII at severaldistances. Fig. 6 demonstrates the results. The nearer thedisturbing object moves to the camera the more the egomotion estimation results degrade. Obviously, the light scat-tering affects only the translation. Strong influences arenoticable when high reflective objects come into the fieldof view or when distances to objects in the scene areheterogeneous. Hence, we considered this fact in designingthe next experiment.

C. 3D MAPPING OF LABORATORY SCENE

Second, the laboratory scene was enlarged. The Styro-foam objects were assembled in a square, which measuredapproximately1800 mm. This scene (cf. Fig. 7) was cir-cumferentially captured by moving the camera on a circularpath with a diameter of300 mm. In total,180 captures were

trans. error rot. err./ mm / ◦

AD 12.8 1.2AI 9.9 1.3B 35.4 2.1C 36.0 2.1D 42.1 1.8

Fig. 6. Experimental setup (SII) for identifying the accuracy and the impactof light scattering. The camera is stepwise moving (50mm) and rotating(22 deg) from C0 to C1. Initially the scene consists of two Styrofoamcuboids standing on top of each other (case A). The improved calibration(AI) shows to reduce the translational error from12.8mm to 9.9mm. Then(cases B, C, D), an additionally Styrofoam cuboid was put into the scene.The ego motion estimation results degrade to42.1 mm. The rotational errornearly stays constant.

(a) (b)

Fig. 7. a) Laboratory environment used for ground truth evaluation. TheToF camera is mounted on an industrial robot arm (KUKA KR 16).b)Bird’s view of the 3D map created in this environment. The camera wasstepwise moved on a circular path with a diameter of 300 mm (green line).

taken. In order to reduce the measurement noise, the camera’sautomatic integration time controller was activated, whichadjusted the integration time between8000 µs and12000 µs.

Again, two test series with the default and improvedcalibration were performed. Here, the improved calibrationreduces the pose-estimation error only slightly. We assumethe light scattering effect completely adumbrates the results.However, the major benefit concerns the isometry of theresulting map. The deviation from the ground truth (mea-suring tape) reduces from85 mm for the horizontal and165 mm for the vertical distance measure to35 mm and20 mm (cf. Fig. 4).

-175-150-125-100-75-50-25

0255075

1000 2000 3000 4000 5000

err

or/

mm

measured distance / mm

Correction SplineMeasurement Error

Fig. 8. Distance calibration result: The identified distance-related erroris plotted by subtracting the measured distance from the real distance. Anoverall sinusoidal curve of the distance-related error is apparent. Altogether,the camera measures a greater distance. Thus a spline is fitted into the errorfor correction of distance measurements.

(a) (b)

0

20

40

60

0 30 60 90 120 150 180

ein

c,∆

θ/◦

Vanilla ICPFrust. ICP

Frust. ICP + relax.

(c)

0

0.5

1

1.5

2

2.5

0 30 60 90 120 150 180

ein

c,∆

t/m

Vanilla ICPFrust. ICP

Frust. ICP + relax.

(d)

Fig. 9. a,b) 3D Map reconstruction: The estimated trajectories are drawn in green color. a) Frustum ICP approach. b) Frustum ICP approach with errorrelaxation. Remaining trajectory distortions are caused by non-systematic errors. c,d) Ground truth comparison of ICP registration. The proposed FrustumICP extension is benchmarked against the Vanilla ICP approach. Global relaxation further improves the accuracy. c) Incremental angular error measure.Please note the large error induced for the Vanilla ICP approach between frame 160 and 170 in (c), which were caused by convergence to wrong localminima. The Frustum ICP approach provided more robustness.d) Incremental translational error measure.

Furthermore, we compared the performances of VanillaICP, Frustum ICP and Frustum ICP with error relaxationapplying them to the calibrated data. In Fig. 9(a) and 9(b)the resulting maps are illustrated. The green line representsthe estimated ego motion of the camera. Both paths onlypartially agree with the circle in Fig. 7. An experimentemploying the ground truth poses as initial guess for theICP registration provided convergence to nearly the sameminima. Thus, errors in ego motion estimation primarily arisefrom unsystematic errors, i.e., light scattering and multiple-ways-reflection. The effect is more noticeable for the secondhalf of the circle, where near objects appeared in the fieldof view. Those influences are dependent on the scene andthus difficult to model or compensate. Fig. 9(c) and 9(d)contrasts the performance of the employed ICP approaches.The Frustum ICP approach provided more robustness andhigher accuracy as the Vanilla ICP approach. The registrationerror could be additionally reduced by error relaxation.

D. 3D MAPPING OF LARGER ENVIRONMENTS

In a further experiment the robustness of the proposedmapping approach was tested in a larger environment, therobotic pavilion at the Fraunhofer Institute IAIS (cf. Fig.1).It sizes19.4 m in the longest distance. In total, 325 captureshave been taken on a closed trajectory. Since the unambiguityinterval is limited to 7.5 m, measurements appear closer(modulo 7.5 m) than they are, if this value is exceeded.Mostly, the amplitude value can be used to discard thosemeasurements, but not in all cases. These mismeasurementsoccur especially when surfaces with specular reflectivity arepresent, e.g., mirrors, window panes and metallic surfaces.Some objects have been covered or removed from the scenebefore the exploration was started. Future technologies willaddress this problem on the sensor level, e.g., by the employ-ment of multiple frequencies or binary coded sequences.

The scene features more dynamic range compared to thelaboratory scene, i.e., a larger working range and variabil-ity of infrared reflectivity. The automatic integration timecontroller adjusted the exposure time between21000 µs and65000 µs.

Ground truth data, in terms of an external positioningsystem, was not available. Therefore, the evaluation of

3DOF 6DOFCalib. method ||∆t|| / m ∆θ / ◦ ||∆t|| / m ∆θ / ◦

Default 0.27 26.10 1.23 31.58Improved 0.30 0.81 1.74 9.03

TABLE ICOMPARISON OF POSE ESTIMATION EMPLOYING DIFFERENT

CALIBRATION METHODS.

Fig. 10. 3D map of the whole robotic pavilion (perspective view of scenein Fig. 1). The trajectory (estimated poses of each frame) isdrawn in green.

precision in reconstruction is specified with the distancebetween two opposing walls and the pose estimation errorafter closing the loop. The start-to-end frame registrationbefore relaxation provided an accumulated translational errorof ||∆t|| = 1.74 m and an accumulated rotational error of||∆θ|| = 9.03◦, which is small enough to perform loop-closure. The calculated distance of two opposing walls fromthe 3D map compared with the distance determined with ameasuring tape deviates0.4 m (Measuring tape vs. 3D cloud:10.8 m / 11.2 m; cf. Fig. 10). Influences of the improvedcalibration approach were more noticeable in this experiment(cf. Table I). A video showing the performance of mapcreation in this environment can be found in [10].

VI. CONCLUSIONS AND FUTURE WORK

This paper presented a method for precise 3D environmentmapping. It employed only a 3D time-of-flight (ToF) cameraand no additional sensors. The achieved precision for a 3Dmap composed of 180 single captures was calculated againstground truth data provided by an industrial robot. Severalmeasures were necessary for achieving the presented resultsin 3D map creation. First, a calibration method has beenapplied to reduce distance measurement errors. Second, theremovement of mismeasurements has been achieved by thedevelopment of suitable filters. Third, robust pose estimationwas achieved by an improved ICP algorithm. Finally, theconsistency of a 3D map has been enhanced in a refinementstep, relaxing accumulated errors and smoothing surfaces.The robustness of the entire approach has been demonstratedwhile registering 325 single captures obtained from a largerindoor environment. By providing the underlying data set ofthe laboratory scene on our website1, we want to motivatefurther investigations in 3D mapping based on ToF cameradata and enable comparable results and benchmarks.

Future work will concentrate on the improvement ofcalibration, e.g., the consideration of fluctuation in depthmeasurements caused by exposure time control, on theimprovement of 3D map creation, e.g., by enhancing thesemantic information, and on fusing ToF camera data withadditional sensory information, e.g., with inertial measure-ment units.

REFERENCES

[1] P. Besl and N. McKay. “A method for Registration of 3–D Shapes”,IEEE Trans. Pattern Anal. Machine Intell., vol. 14, no. 2, pp. 239–256,1992.

[2] D. Chetverikov, D. Svirko, D. Stepanov and P. Krsek. “Thetrimmed it-erative closest point algorithm”.Proceedings of the 16th Internationalconference on pattern recognition (ICPR), vol. 3, pp. 545–548, 2002.

[3] D. M. Cole and P. M. Newman. “Using Laser Range Data for 3DSLAM in Outdoor Environments”,Proceedings of the IEEE Interna-tional Conference on Robotics and Automation (ICRA), 2006.

[4] A. Fusiello, U. Castellani, L. Ronchetti and V. Murino. “Model acqui-sition by registration of multiple acoustic range views”,Proceedingsof the 7th European Conference on Computer Vision (ECCV), pp.805–819, 2002.

[5] S. Fuchs and S. May. “Calibration and registration for precise surfacereconstruction with ToF cameras”.Proceedings of the Dynamic 3DImaging Workshop in Conjunction with DAGM (Dyn3D), vol. I, 2007.

[6] S. Fuchs and G. Hirzinger. “Extrinsic and Depth Calibration of ToF-cameras”.Proceedings of the IEEE Conference on Computer Visionand Pattern Recognition (CVPR), 2008.

[7] R. Lange. “3D time-of-flight distance measurement with customsolid-state image sensors in CMOS/CCD-technology”, Dissertation,University of Siegen, 2000.

[8] E. Lengyel. Game Programming Gems, chapter 4.5: A Fast Cylinder-Frustrum Intersection Test. Charles River Media, 2000.

[9] J. Luck, C. Little and W. Hoff. “Registration of Range Data Using aHybrid Simulated Annealing and Iterative Clostest Point Algorithm”,Proceedings of the IEEE International Conference on Robotics andAutomation (ICRA), 2000.

[10] S. May, D. Droeschel, D. Holz, C. Wiesen and S. Fuchs. Video:“3D Pose Estimation and Mapping with Time-of-flight Cameras”,http://www.iais.fraunhofer.de/3325.html, 2008.

[11] A. Nuchter.3D Robotic Mapping, Springer STAR series, 2008.[12] H. Niemann, T. Zinßer and J. Schmidt. “A refined ICP algorithm

for robust 3-D correspondence estimation”,Proceedings of the IEEEInternational Conference on Image Processing, 2003.

1http://www.robotic.de/(242/

[13] A. Nuchter, K. Lingemann, J. Hertzberg and H. Surmann.“6D SLAM- 3D Mapping Outdoor Environments”,Journal of Field Robotics, vol.24, no. 8–9, pp. 699–722, 2007.

[14] K. Ohno, T. Nomura and S. Tadokoro. “Real-Time Robot TrajectoryEstimation and 3D Map Construction using 3D Camera”.Proceedingsof IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS), 2006.

[15] T. Pajdla and L. V. Gool. “Matching of 3-D curves using semi-differential invariants”,Proceedings of the Fifth International Con-ference on Computer Vision (ICCV), pp. 390–395, 1995.

[16] A. Prusak, O. Melnychuk, H. Roth, I. Schiller and R. Koch. “Pose Esti-mation and Map Building with a PMD-Camera for Robot Navigation”.Proceedings of the Dynamic 3D Imaging Workshop in Conjunctionwith DAGM (Dyn3D), vol. I, 2007.

[17] R. Sheh, M. W. Kadous and C. Sammut. “On building 3D maps usinga Range camera: Applications to Rescue Robotics”, Technical ReportUNSW-CSE-TR-0609, School of Computer Science and Engineering,The University of New South Wales, Sydney, Australia, 2006.

[18] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron, J.Diebel, et al., “Stanley: The Robot that Won the DARPA GrandChallenge”,Journal of Field Robotics, vol. 23, no. 9, pp. 661–692,2006.

[19] S. Thrun, W. Burgard and D. Fox,Probabilistic Robotics, The MITPress, 2005

[20] S. Thrun and M. Montemerlo. “The GraphSLAM Algorithm With Ap-plications to Large-Scale Mapping of Urban Structures”,InternationalJournal on Robotics Research, vol. 25, no. 5/6, pp. 403–430, 2005.

[21] O. Wulf, D. Lecking and Bernardo Wagner. “Robust Self-Localizationin Industrial Environments based on 3D Ceiling Structures”, Proceed-ings of the IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS), 2006.