An E ffi cientExtensionofElevation Mapsfor Outdoor Terrain Mapping PatrickPfaff andWolframBurgard DepartmentofComputer Science, University of Freiburg,Germany, { pfaff,burgard} @informatik.uni-freiburg.de Summary. Elevationmapsare apopular data structurefor representingthe environmentofa mobile robot operatingoutdoorsoronnot-flat surfaces.Elevationmapsstore in each cellofa discrete grid theheight of thesurface thecorresponding place in theenvironment. Theuse of this 2 1 2 -dimensionalrepresentation, however, is disadvantageous when it is used formapping with mobile robotsoperatingonthe ground, sincevertical or overhanging objectscannot be representedappropriately.Suchobjectsfurthermore can lead to registrationerrors when two elevationmapshavetobematched.Inthispaper we proposeanapproach that allows amobile robot to deal with vertical andoverhanging objectsinelevationmaps. We classify thepointsin theenvironmentaccordingtowhether they correspond to such objectsornot.Wealsodescribe avariant of theICP algorithmthatutilizes theclassificationofcells duringthe data association. Experiments carried out with areal robot in an outdoor environmentdemonstratethatthe scan matchingprocessbecomes significantly more reliableand accurate when our classificationis used. 1Introduction Theproblem of learning maps with mobile robotshas been intensivelystudied in thepast. In theliterature,di ff erenttechniquesfor representingthe environment of a mobile robot prevail. Topological maps aimatrepresentingenvironmentsbygraph- likestructures, whereedgescorrespond to places,and arcs to pathsbetween them. Geometric models, in contrast,use geometric primitivesfor representingthe environ- ment.Whereas topological maps have theadvantage to betterscaletolarge environ- ments, they lack theability to representthe geometric structureofthe environment. Thelatter, however, is essentialinsituations,inwhich robotsare deployed in poten- tially unstructuredoutdoor environmentswhere theability to traverse specific areas of interest needstobeknown accurately.However,fullthree-dimensionalmodels typically have toohighcomputationaldemands foradirect applicationonamobile robot. Elevationmapshavebeen introduced as amorecompact 2 1 2 -dimensionalrepre- sentation. An elevationmap consists of atwo-dimensionalgridinwhich each cell stores theheight of theterritory.Thisapproach,however,can be problematic when a P. Corke and S. Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp. 195–206, 2006. © Springer-Verlag Berlin Heidelberg 2006 196 P. Pfa ff and W. Burgard Fig. 1. Scan (point set) of abridge recorded with amobile robot carryingaSICK LMSlaser range findermounted on apan/ tilt unit. robot hastoutilizethese maps for navigationorwhenithas to register twodi ff erent maps in ordertointegrate them.For example, consider thethree-dimensionaldata pointsshown in Figure1.Theyhavebeen acquiredwith amobile robot standing in front of abridge.The resultingelevationmap,which is computedfromaverag- ingoverall scan pointsthatfallintoacellofahorizontal grid (given avertical projection),isdepictedinFigure2.Ascan be seen fromthe figure, theunderpass hascompletelydisappeared andthe elevationmap showsanon-traversableobject. Additionally,whenthe environment contains vertical structures,wetypically obtain varyingaverage height values depending on howmuchofthisvertical structureis containedinascan.Accordingly, if one registerstwo such elevationmaps, one ob- tainsincorrect alignments. Fig. 2. Standard elevationmap computed forthe outdoor environmentdepictedinFigure1. Thepassage underthe bridge hasbeen convertedintoalargeun-traversableobject. In this paperwepresent asystemfor mapping outdoor environmentswith el- evationmaps. Ouralgorithmtransforms range scansintolocal elevationmapsand combinesthese local elevationmapsusing avariant of theICP algorithm[3].Inour elevationmaps, we classify locations in theenvironment into four classes, namely locations sensed fromabove,vertical structures,vertical gaps,and traversablecells. Theadvantage of this classificationistwofold.First,the robot can representobsta- cles corresponding to vertical structures likewalls of buildings.Italsocan deal with An E ffi cient Extension of Elevation Maps for Outdoor Terrain Mapping197 overhanging structures like branches of trees or bridges. Furthermore, the classifi- cation can be utilized in the ICP algorithm to more robustly match local elevation maps. We present experimental results illustrating the advantages of our approach regarding the representation aspect as well as regarding the robust matching. This paper is organized as follows. After discussing related work in the follow- ing section, we will describe our extension to the elevation maps in Section 3. In Section 4 we then describe how to incorporate our classification into the ICP algo- rithm used for matching elevation maps. Finally, we present experimental results in Section 5. 2 Related Work The problem of learning three-dimensional representations has been studied inten- sively in the past. One of the most popular representations are raw data points or tri- angle meshes [1, 7, 12, 15]. Whereas these models are highly accurate and can easily be textured, their disadvantage lies in the huge memory requirement, which grows linearly in the number of scans taken. An alternative is to use three-dimensional grids [9] or tree-based representations [13], which only grow linearly in the size of the environment. Still, the memory requirements for such maps in outdoor environ- ments are high. In order to avoid the complexity of full three-dimensional maps, several re- searchers have considered elevation maps as an attractive alternative. The key idea underlying elevation maps is to store the 2 1 2 -dimensionalheight informationofthe terrain in atwo-dimensionalgrid. Baresetal. [2]aswellasHebertetal. [4]use ele- vationmapstorepresent theenvironment of aleggedrobot.Theyextract pointswith high surface curvatures andmatch thesefeatures to alignmapsconstructedfromcon- secutive range scans. Parra et al.[11]represent theground floor by elevationmaps anduse stereo vision to detect andtrack objectsonthe floor.Singh andKelly [14] extract elevationmapsfromlaser range data anduse thesemapsfor navigatingan all-terrain vehicle. Ye andBorenstein[16]proposeanalgorithmtoacquire elevation maps with amovingvehicle equippedwith atiltedlaser range scanner. They pro- posespecial filteringalgorithms to eliminatemeasurementerrors or noise resulting fromthe scannerand themotions of thevehicle.Lacroixetal. [6]extract elevation maps from stereo images.Theyuse atwo-dimensionalgridand storeineach cell of this grid theaverage height.Hygounenc et al.[5] constructelevationmapsinan autonomous blimpusing 3d stereo vision. They proposeanalgorithmtotrack land- marksand to matchlocal elevationmapsusing theselandmarks. Olson[10]describes aprobabilistic localizationalgorithmfor aplanetary roverthatuseselevationmaps forterrain modeling. Comparedtothese techniquesthe contributionofthispaper liesintwo aspects. First,weclassify thepointsinthe elevationmap into horizontal pointsseen from above,vertical points, andgaps. This classificationisimportantespecially when a roverisdeployedinanurban environments. In such environments, typical structures likethe walls of buildings cannot be representedinstandard elevationmaps. Second, we describe how this classification can be used to enhance the matching of diff erent elevation maps. 3 Extended Elevation Maps As already mentioned above, elevation maps are 2 1 2 -dimensionalrepresentationof theenvironment.The maintain atwo-dimensionalgridand maintain in everycell of this grid an estimateabout theheight of theterrain at thecorresponding point of theenvironment.Tocorrectly reflect theactualsteepness of theterrain,acommon assumptionisthatthe initialtilt andthe roll of thevehicle is known. When updatingacellbased on sensoryinput,wehavetotakeintoaccount,that theuncertainty in ameasurementincreases with thedistancemeasured due to errors in thetiltingangle. In our current system,weaapplyaKalman filter to estimatethe parameters µ 1:t and σ 1:t about theelevationinacelland its standard deviation. We applythe following equations to incorporateanewmeasurement z t with standard deviation σ t at time t [8]: µ 1:t = σ 2 t µ 1:t − 1 + σ 2 1:t − 1 z t σ 2 1:t − 1 + σ 2 t (1) σ 2 1:t = σ 2 1:t − 1 σ 2 t σ 2 1:t − 1 + σ 2 t (2) Note that theapplicationofthe Kalman filter allows us to take into account the uncertainty of themeasurement. In our current system,weapplyasensor model, in whichthe variance of theheight of ameasurementincreases linearly with the distance of thecorresponding beam.Thisprocessisindicated in Figure3. Fig. 3. Variance of aheight measurements depending on thedistanceofthe beam. In additionweneed to identifywhich of thecells of theelevationmap correspond to vertical structures andwhich onescontaingaps. In ordertodetermine theclass of acell, we first consider thevarianceofthe height of allmeasurements fallinginto this cell. If this valueexceedsacertain threshold, we identifyitasapoint that hasnot 198 P. Pfa ff and W. Burgard An E ffi cient Extension of Elevation Maps for Outdoor Terrain Mapping199 been observed from above. We then check, whether the point set corresponding to a cell contains gaps exceeding the height of the robot. When a gap has been identified, we determine the minimum traversable elevation in this point set. Fig. 4. Labelingofthe data pointsdepictedinFigure2accordingtotheir classification. The diff erentcolors / grey levels indicatethe individualclasses. Figure4showsthe same data pointsalreadydepictedinFigure2.The classes of theindividualcells in theelevationmap areindicated by thedi ff erentcolors / grey levels.The blue/ dark pointsindicatethe data pointsabove agap.The red / medium greyvaluesindicatecells that areclassifiedasvertical.The green/ light grey values, however, indicatetraversable terrain.Notethatthe not traversablecells arenot shown in this figure. Fig. 5. Extendedelevationmap forthe scenedepictedinFigure1. Amajor part of theresultingelevationmap extracted fromthisdataset is shown in Figure5.Ascan be seen fromthe figure, theareaunderthe bridge can nowbe representedappropriately by ignoringdatapointsabove thelowestsurface. This in turn enablesthe robot to plan apaththrough thepassage underthe bridge. 200 P. Pfa ff and W. Burgard 4 E ffi cient Matching of Elevation Maps in 6 Dimensions To integrate several local elevation maps into a single global elevation map we need to be able register two maps relative to each other. In our current system, we apply the ICP algorithm for this purpose. The goal of the matching process is to minimize an error function defined over two point sets X = { x 1 , , x L } and Y = { y 1 , , y L } , where each pair x i and y i is assumed to be the points that corresponding to each other. We are interested in the rotation R and the translation t that minimizes the following cost function: E ( R , t ) = 1 n L  l = 1 ||x l − Ry l − t || 2 , (3) where || ·|| is adistancefunctionthattakes into account thevarianceofthe Gaussians corresponding to each pair x i and y i . In principle, one coulddefine this functiontodirectly operate on theheight val- uesand theirvariancewhenaligning twodi ff erentelevationmaps. Thedisadvantage of this approach,however,isthatinthe caseofvertical objects, theresultingheight seriously depends on theviewpoint.The same vertical structuremay lead to varying heightsinthe elevationmap when sensed fromdi ff erentpoints. In practical experi- mentsweobservedthatthisintroduces serious errors andoften prevents theICP al- gorithmfromconvergence. To overcome this problem,weseparateEquation(3) into four componentseach minimizing theerroroverthe individualclassesofpoints. The first twoclassesconsistofthe cells corresponding to vertical objectsand gaps.The lattertwo classescontainonlycells whosepointshavebeen sensed fromabove.To increasethe e ffi ciency of thematchingprocess, we onlyconsider asubset of these cells.Inpractical experimentswefound out that traversablecells andedge cells yield thebestregistrationresults.The traversablecells arethosecells forwhich theele- vationofthe surface normalobtainedfromaplanefittedtothe local neighborhood exceeds83degrees.Additionally,weconsider edge cells,i.e., cells whichlie more than 20cmabove theirneighboringpoints. Letusassume that α 1 , ,α N α and α  1 , ,α  N α arethe corresponding vertical points, β 1 , ,β N β and β  1 , ,β  N β arethe vertical gaps, γ 1 , ,γ N γ and γ  1 , ,γ  N γ arethe edge points, and δ 1 , ,δ N δ and δ  1 , ,δ  N δ arethe traversablecells.The resultingerrorfunctionthenis E ( R , t ) = N α  n = 1 d ( α n ,α  n )   vertical objects + N β  n = 1 d ( β n ,β  n )   vertical gaps + N γ  n = 1 d ( γ n ,γ  n )   edge cells + N δ  n = 1 d ( δ n ,δ  n ) ,   traversablecells (4) where d ( x , y ) =  x − Ry − t  2 . Figure6illustrateshow twoelevationmapsare alignedoverseveral iterations of theminimizationprocess. Whereas theleftcolumn showsthe point clouds theright column showsthe cells in theelevationmap used forminimizingEquation(4).Inour An E ffi cient Extension of Elevation Maps for Outdoor Terrain Mapping201 current implementation, each iteration of the ICP algorithm usually takes between one and five seconds on a 2.8GHz Pentium 4. The time necessary to acquire a scan by tilting the laser is 5 seconds. Fig. 6. Incrementalregistrationoftwo elevationmaps. Theleftcolumndepicts theoriginal point clouds.The right column showsthe vertical andedge cells of theelevationmapsused by theICP algorithm. Theindividualrowscorrespond to theinitialrelativepose(toprow), alignmentafter 5iterations (second row), after10iterations (third row) andthe final alignment after30iterations (fourth row). In addition to the position and orientation of the vehicle we also have to estimate the tilt and roll of the vehicle when integrating two elevation maps. In practical ex- periments we found that an iterative scheme, in which we repeatedly estimate the tilt and roll of the robot and then determine the relative position and orientation of the involved elevation maps, improves the registration accuracy. In most cases, two iterations are suffi cient to achieve precise matchings and to obtain highly accurate maps from multiple local maps generated from diff erent viewpoints. SICK laser range finder AMTEC wrist unit Fig. 7. Robot Herbertusedfor theexperiments. 5Experimental Results Theapproach describedabove hasbeen implementedand tested on areal robot sys- temand in simulationruns with real data.The robot used to acquire thedataisour outdoor robot Herbert,which is depicted in Figure7.The robot is aPioneer II AT system equippedwith aSICKLMS range scannerand an AMTEC wristunit, which is used as apan/ tilt device forthe laser. 5.1Learning AccurateElevation Maps from Multiple Scans To evaluate our approach we steered our robot Herbert through diff erentareas of our university campus andvisually inspected themapsobtainedwith our technique.In allcases,weobtainedhighlyaccurate maps.Figure8showsatypical example, in whichthe robot traveled underthe bridge depicted in Figure1andthencontinued drivinguparamp.Along its path therobot generatedlocal elevationmapsfrom36 scans. Theoverall numberofdatapointsrecorded was9,500,000. Thesizeofeach cellinthe elevationmap is 20 by 20cm. Thewholemap spansapproximately 70 by 30 meters.Ascan be seen fromthe figure, themap clearly reflectsthe details of the environment.Additionally,the matching of theelevationmapsisquite accurate. Figure9showsatypical exampleinwhich our algorithmyieldsmoreaccurate maps than thestandard approach.Inthissituationthe robot traveled along apaved wayand scannedatree located in front of thescene. Whereas theleftimage shows themap obtainedwith thestandard elevationmap approach,the right imageshows 202P. Pfa ff and W. Burgard An E ffi cient Extension of Elevation Maps for Outdoor Terrain Mapping203 the map obtained with our method. The individual positions of the robot where the scans were taken are also shown in the images. As can be seen from the figures, our method results in more free space around the stem of the tree. Fig. 8. Elevationmap generatedfrom36local elevationmaps. Thesizeofthe map is approx- imately70by30meters. Fig. 9. Maps generatedfromfour local elevationmapsacquiredwith Herbert. Theleftimage showsastandard elevationmap.The right imagedepicts themap obtainedwith our approach. Thepeak in front of thescenecorresponds to atree, whichismodeledmoreaccurately with our approach. 5.2Statistical Evaluation of theAccuracy Additionally,weperformedaseries of experimentstoget astatistical assessmentas to whether theclassificationofthe data pointsintonormal, vertical andgap points combinedwith thesub-samplingofthe normalpointsleadstobetterregistration results.Toperform theseexperimentsweconsidered twodi ff erentelevationmapsfor whichwecomputed theoptimal relative poseusing severalruns of theICP algorithm. We then randomlyaddednoise to theposeofthe second mapand appliedthe ICP algorithmtoregisterbothmaps. We performedtwo sets of experimentstocompare theregistrationresults forthe unclassifiedand theclassifiedpoint sets.Table 1shows theindividualclassesofnoise that we addedtothe true relative poseofthe twomaps before we startedthe ICPalgorithm. In this experiment describedhere, we only varied theposeerrorofthe maps andkeptthe errorinthe rotations constant.In particular, we randomly chose rotational displacements from ± 5 degrees around the real relative angle and also varying random displacements in the x and y direction. displacementclass max. rot. displ. max.displ.in x and y 1 ± 5degrees ± 0 . 5 m 2 ± 5degrees ± 1 . 0 m 3 ± 5degrees ± 1 . 5 m 4 ± 5degrees ± 2 . 0 m 5 ± 5degrees ± 2 . 5 m Table1. Displacementclassesusedtoevaluatethe performance of theICP algorithmonthe classifiedand unclassifiedpointsextracted fromthe elevationmaps. 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 1 2 3 4 5 average registration error displacement class classified points unclassified points Fig. 10. Averageregistrationerrorfor theindividualtypesofinitialdisplacement. Theresultingaverage displacementerrors afterconvergence of theICP algo- rithmare depicted in Figure10. As canbeseen fromthe figure, theICP algorithm performedsignificantly betteronthe classifiedpoint sets.Inthisfigure, theerrorbars indicatethe α = 0 . 05 confidencelevel. Additionally,weevaluated howoften theICP algorithmfailedtoaccurately reg- isterthe twomaps. Figure11depicts thenormalized divergence frequenciesinper- centfor theindividualdisplacementclasses. As this plot illustrates, theutilizationof theindividualclassesinthe ICPalgorithmleadstoaseriously betterconvergence rate.Inadditionalexperimentsnot reportedhereweobtainedsimilarresults forthe diff erentorientationalerrors. 204 P. Pfa ff and W. Burgard [...]... 8 P.S Maybeck The Kalman filter: An introduction to concepts In Autonomous Robot Vehicles Springer Verlag, 1990 9 H.P Moravec Robot spatial perception by stereoscopic vision and 3d evidence grids Technical Report CMU-RI-TR-9 6- 3 4, Carnegie Mellon University, Robotics Institute, 19 96 10 C.F Olson Probabilistic self-localization for mobile robots IEEE Transactions on Robotics and Automation, 16( 1):55 66 ,... inversely symmetrical manner, metric maps are easy Work supported by ProBayes and the French ANRT (National Association for Technical Research) P Corke and S Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp 219–230, 20 06 © Springer-Verlag Berlin Heidelberg 20 06 220 M Yguel, O Aycard, and C Laugier to build and update and they allow direct complex interactions with the environment such as obstacle... localization serves to build a grid map of the environment and has the capability to perform motion planning with safe navigation as described in [1] However, reflective cones as P Corke and S Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp 207–218, 20 06 © Springer-Verlag Berlin Heidelberg 20 06 208 C Tay Meng Keat, C Pradalier, and C Laugier artifical landmarks is not a very practical approach An improvement... Conference on Robotics and Automation, pages 138–145, 1985 9 C Tay, C Pradalier, and C Laugier Vehicle detection and car park mapping using laser scanner 2005 Wavelet Occupancy Grids: A Method for Compact Map Building* Manuel Yguel, Olivier Aycard, and Christian Laugier 1 2 -motion, GRAVIR-UJF-INRIA-INP Grenoble, France firstname.lastname@inrialpes.fr Inria Rhˆne-Alpes, 65 5 avenue de l’Europe - Montbonnot... Imaging and Modeling, pages 357– 364 , 2001 2 06 P Pfaff and W Burgard 2 J Bares, M Hebert, T Kanade, E Krotkov, T Mitchell, R Simmons, and W R L Whittaker Ambler: An autonomous rover for planetary exploration IEEE Computer Society Press, 22 (6) :18–22, 1989 3 P.J Besl and N.D McKay A method for registration of 3-d shapes IEEE Transactions on Pattern Analysis and Machine Intelligence, 14:239–2 56, 1992 4... current implementation is a naive and unoptimized version of FastSLAM that executes with a frequency of between 4 -6 Hz on a pentium 4 Figure 8 presents the multiple hypotheses of the position of CyCab and the other vehicles in the environment The various CyCab hypotheses are followed by a curve indicating its mean path taken and the various landmark hypotheses Figures 9 and 10 illustrates final constructed... against previously accepted hypotheses A current hypothesis that is very similar in position and angle with a previously accepted hypotheses can be taken to be a positive hypothesis by virtue of being previously accepted 4.1 Considering Previous Hypotheses The two main criteria for measuring similarity of a current hypothesis and a previously accepted hypothesis are their position and angle It will be convenient... hypothesis as represented by its inner and outer bounding boxes in fig 7 Due to the conservative approach in validating hypotheses in stage 2, some potential hypotheses are inevitably eliminated in the process (eliminated vehicle hypothesis to the right in fig 7) Fig 6 Accepted vehicle hypothesis after stage 1 of vehicle detection Fig 7 The same configuration as in figure 6 but with rejected vehicle hypothesis... position 6 7 with value: 2−s The square has is up left corner at coordinate: (2s i, 2s j) and has a side of 2s pixels 228 M Yguel, O Aycard, and C Laugier (a) (b) (c) (d) Fig 8 Scaled view of OG provided in WavOG representation (a) scale 1 : 64 pixels, s = 2 (b) scale 96 × 64 pixels, s = 3 (c) scale 1 scale 1024 : 24 × 16 pixels, s = 5 1 : 2 56 1 : 16 192 × 128 48 × 32 pixels, s = 4 (d) The laser range-finder... source code and robot References 1 J Borenstein and Y Koren Real-time obstacle avoidance for fast mobile robots IEEE Transactions on Systems, Man, and Cybernetics, 19(5):1179–1187, - 1989 2 C Cou´, T Fraichard, P Bessi`re, and E Mazer Using bayesian programming e e for multi-sensor multi-target tracking in automotive applications In Proceedings of the IEEE International Conference on Robotics and Automation, . for the hypotheses obtained to conflict with a previous corresponding hypothesis. Furthermore, multiple hypothesis SLAM methods such as FastSLAM produces a set of hypotheses, a map construction. canbeused. If multiple hy- potheses SLAM algorithms are used,the differenthypothesesmight come up withconflicting hypotheses caused by association of landmarks. Suchcon- flicting associationsoflandmarksmustberesolvedinorder. theposeofthe second mapand appliedthe ICP algorithmtoregisterbothmaps. We performedtwo sets of experimentstocompare theregistrationresults forthe unclassifiedand theclassifiedpoint sets.Table 1shows theindividualclassesofnoise