Risto Kaijaluoto

Precise indoor localization for mobile laserscanner

School of Electrical Engineering

Thesis submitted for examination for the degree of Master of

Science in Technology.

Espoo 15.5.2015

Thesis supervisor:

Prof. Ville Kyrki

Thesis advisor:

D.Sc. (Tech.) Antero Kukko

aalto university

school of electrical engineering

abstract of the

master's thesis

Author: Risto Kaijaluoto

Title: Precise indoor localization for mobile laser scanner

Date: 15.5.2015 Language: English Number of pages: 7+54

Department of Electrical Engineering and Automation

Professorship: Automation technology Code: AS-84

Supervisor: Prof. Ville Kyrki

Advisor: D.Sc. (Tech.) Antero Kukko

Accurate 3D data is of high importance for indoor modelling for various applica-tions in construction, engineering and cultural heritage documentation. TerrestrialLaser Scanning (TLS) is currently the most accurate method to collect such data.Due to its static single view point data collection, excessive time and data redun-dancy are needed to capture data of good integrity and coverage. With MobileLaser Scanner (MLS) data acquisition is considerably faster and changing viewpoint reduces occlusions. However, MLS requires an accurate estimate of its lo-cation and attitude to project the measurements. The location can be calculatedby using a horizontally mounted laser scanner combined with a Simultaneous Lo-calization and Mapping (SLAM) algorithms.The aim of this thesis is to investigate, what level of trajectory accuracies canbe achieved with high quality sensors and freely available state of the art SLAMalgorithms and whether the trajectories can be used to project measurementscollected with a secondary laser scanner to generate visually aesthetic and goodquality three dimensional point clouds.The performance of three SLAM algorithms were studied on �ve datasets cap-tured at our o�ce with Slammer platform. The datasets were processed with thealgorithms using a number of di�erent parameter combinations and their perfor-mance was evaluated by comparing the results against a TLS based reference. Acombination of Hector SLAM and Karto showed the best performance and 20 mmroot mean squared error (RMSE) levels could be achieved. Analysis of the 3Dpoint cloud produced with the trajectory showed good agreement with the TLSreference with 13 mm planar RMSE. The obtained point cloud is useful for indoormodelling with accuracies close to TLS with vastly faster data collection.

Keywords: SLAM, Indoor localization, Mobile laser scanning, MLS, Point cloud

aalto-yliopisto

sähkötekniikan korkeakoulu

diplomityön

tiivistelmä

Tekijä: Risto Kaijaluoto

Työn nimi: Liikkuvan laserkeilaimen tarkka sisätilapaikannus

Päivämäärä: 15.5.2015 Kieli: Englanti Sivumäärä: 7+54

Sähkötekniikan ja automaation laitos

Professuuri: Automaatiotekniikka Koodi: AS-84

Valvoja: Prof. Ville Kyrki

Ohjaaja: TkT Antero Kukko

Kolmiulotteisille malleille sisätiloista on monenlaisia käyttötarkoituksia ainarakennusten kunnon seuraamisesta virtuaalimaailmojen luomiseen. Mallin poh-jaksi tarvitaan tarkat 3D mittaukset ja tarkin tapa kerätä ne on maalaserkeilaus.Maalaserkeilauksen ongelma on staattisista mittauksista johtuvat katvealueet janäiden minimoiseksi tarvittava suuri määrä aikaavieviä mittauksia. Liikkuvallalaserkeilaimella datan keräys on huomattavasti nopeampaa ja jatkuvasti vaihtu-van mittauspaikan ansiosta katvealueita jää vähemmän. Keilaimen liikerata täy-tyy kuitenkin tietää tarkasti ja yksi tapa laskea se on toisen, vaakatasossa mittaa-van laserkeilaimen ja sen dataa prosessoivan samanaikaisen paikannus ja kartoi-tus (Simultaneous Localization and Mapping, SLAM) algoritmin avulla. Tämändiplomityön tarkoitus on tutkia millaisiin liikeradan tarkkuuksiin päästään käyt-tämällä hyvälaatuisia sensoreita ja laadukkaita, avoimen lähdekoodin SLAM algo-ritmejä ja että pystytäänkö tämän liikeradan avulla muodostamaan hyvännäköi-nen ja laadukas pistepilvi.Diplomityössä tutkittiin kolmea SLAM algoritmia viiden Slammerilla kerätyn tes-tiaineiston avulla. Jokaisen testiajon mittaukset prosessointiin useilla SLAM al-goritmien parametriyhdistelmillä ja laskettujen liikeratojen hyvyyttä arvioitiinvertaamalla niitä referenssi liikerataan joka oli muodostettu maalaserkeilaustenperusteella tuotetun kohdekartan avulla. Parhaaseen tulokseen päästiin Hec-tor SLAM ja Karto algoritmien yhdistelmällä, jolla keskineliövirheen neliöjuurioli 20 mm tai alle. Verratessa näiden liikeratojen avulla muodostettuja pis-tepilviä maalaserkeilauksen avulla muodostettuun, tasovirheen keskineliövirheenneliöjuuri oli 13 mm. Vaikka maalaserkeilauksen tarkkuuksiin ei päästy, liikkuvanlaserkeilaimen avulla muodostetut pistepilvet ovat riittävän hyviä moniin käyttö-tarkoituksiin ja ne saatiin mitattua huomattavasti nopeammin.

Avainsanat: Samanaikainen paikannus ja kartoitus, Sisätilapaikannus, Liikkuvalaserkeilaus, Pistepilvi

iv

Preface

This research was conducted at Finnish Geospatial Research Institute (FGI) in theCentre of Excellence in Laser Scanning Research and in the Department of RemoteSensing and Photogrammetry. First I would like to thank Prof. Juha Hyyppä forthe opportunity to work at FGI on my thesis in the excellent Mobimap and LaserScanning Research group. I would also like to thank my thesis supervisor Prof. VilleKyrki from the Intelligent Robotics group of Aalto University for always being readyto help and for the excellent improvement suggestions during the �nalizing stagesof the thesis. The biggest thanks go to my thesis advisor Dr. Antero Kukko, whoalways managed to �nd time to help me even when burdened by his own research.Among many other things, his instructions on how to use the hardware was essentialand the help with assessing the quality of point cloud was also invaluable.

Harri Kaartinen and Anttoni Jaakkola from the research group also deservethanks for always providing a helping hand when needed. I would also like to thankRoope Näsi, Saija Simola and Mika Kekäläinen, who worked on their Master's thesesat the same time for peer support.

Finally I would like to thank my family for their enduring support they gave andespecially my father Sakari Kaijaluoto for the innumerable suggestions on improvingthe grammar and language of the thesis.

Masala, 18.5.2015

Risto Kaijaluoto

v

Contents

Abstract ii

Abstract (in Finnish) iii

Preface iv

Contents v

Symbols and abbreviations vii

1 Introduction 1

2 Background 42.1 Laser Scanner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2 Inertial Measurement Unit . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Simultaneous Localization and Mapping Algorithms 83.1 Hector SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.2 GMapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.3 Karto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4 Hardware Implementation 14

5 Software Implementation 165.1 Robot Operating System . . . . . . . . . . . . . . . . . . . . . . . . . 165.2 Modi�cations to the SLAM algorithms . . . . . . . . . . . . . . . . . 175.3 Processing pipeline . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

6 Experiments 226.1 Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 226.2 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246.3 Evaluation of trajectories and e�ects of parameters . . . . . . . . . . 26

6.3.1 Hector SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . 276.3.2 GMapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 306.3.3 Karto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

6.4 Discussion on used algorithms . . . . . . . . . . . . . . . . . . . . . . 406.5 Evaluation of point cloud . . . . . . . . . . . . . . . . . . . . . . . . . 41

7 Conclusions and Future Work 45

References 47

Appendices 50

A Hector errors 50

vi

B GMapping errors 52

C Karto errors 54

vii

Symbols and abbreviations

Abbreviations

GNSS Global Navigation Satellite SystemIMU Inertial Measurement UnitLiDAR Light Detection and RangingMCL Monte Carlo LocalizationMLS Mobile Laser Scanner/ScanningRMSE Root mean squared errorSLAM Simultaneous Localization and MappingSPAN Synchronized Position Attitude NavigationTLS Terrestrial Laser Scanner/Scanning

1 Introduction

The Demand for digital 3D models of building interiors has been growing as thecost of producing them has been decreasing. They can be used for a variety ofpurposes from creating virtual worlds to monitoring building condition. In buildingmonitoring periodically taken 3D measurements can be used to assess the structuralintegrity of a building by for example measuring if supporting beams or walls havebulged or moved. The 3D model of a building can also help with renovations planningand can also be used for assessing the result. Virtual models of important culturaland historical sites can be used in marketing for the lesser known ones and for thepopular ones it can serve as a way to visit them alone as crowds of tourists cangreatly a�ect the atmosphere. While a virtual visit to a site is not the same as anactual one, having the option enables people from all over the world without theresources or time for actual visit to have the experience.

Currently the most common way to make indoor models is by using TerrestrialLaser Scanning (TLS). In TLS, a 2D scanner scanning vertically is mounted on atripod and as the scanner is rotated on the tripod, a 3D point cloud of the scanner'ssurroundings is created. Speci�c reference targets, which are not moved between thescans and can be seen from multiple scanning locations are used to combine pointclouds acquired from di�erent positions to create one representing the whole area ofinterest.

Scanning large indoor areas by a TLS system is time consuming and requiresa large amount of manual work. Taking a scan from one location can take severalminutes and as the scanner and, depending on the complexity of the building, thetargets too must be moved a number of times, the whole process of scanning abuilding can take hours. The scanning positions and the locations of the targets usedto register the di�erent scans have to be well planned to ensure that all scans can beregistered to one cloud and to minimize occlusions rising from the limited numberof viewpoints. The number of required reference targets and scanning locations canquickly grow to a large number especially in cluttered spaces with short visibility.

A mobile laser scanner (MLS) consists of a laser scanner taking measurements ofthe surroundings and sensors which provide orientation and position to the mobileplatform. With MLS continuously taking measurements, a large area can be coveredquickly and occlusions are much less of a problem. A MLS platform can also easilybe augmented with additional sensors such as infrared or hyperspectral cameras toadd additional information to the measured points. As the measurements must becontinuously projected to the world coordinate system, the trajectory of the platformmust be known with high precision at all times for the MLS system to be able toproduce point cloud of the environment.

Global Navigation Satellite Systems (GNSS) such as GPS maintained by theUnited States government or GLONASS maintained by the Russian governmentprovide signals, which enable receivers to pinpoint their location with accuracy inthe range of meters. This can be further improved to the range of tens of centimetreswith augmentation methods such as di�erential correction[1]. As the name implies,both systems provide global coverage and when combined with Inertial Measurement

2

Unit they can provide accurate trajectory. Unfortunately GNSS signals are weakand cannot reliably penetrate building walls and so cannot be relied upon indoors.Even if the signal could be intermittently received, the accuracy is further degradedby multi-path propagation caused by the signal re�ections from the multitude ofsurfaces in buildings [2].

One �eld receiving much interest is the use of nowadays ubiquitous RF sig-nals (such as WiFi, Bluetooth or FM radio) to help with localization. The devicelocation can be inferred by measuring the received signal strength (RSS) of dif-ferent signal sources and by comparing them to a database of measurements fromknown locations[3]. Unfortunately the accuracy possible with such techniques is inthe range of meters which is far too inaccurate for any mapping or data collectionpurposes[3, 4]. Changes in the environment also a�ect signal propagation and theaccuracy can be further lowered if for example a piece of furniture is moved around.

Horizontally mounted laser scanners are widely used for mobile robot localiza-tion and mapping purposes as they provide accurate spatial information of the worldwith little noise[5�7]. When combined with Simultaneous Localization and Mapping(SLAM) algorithms they can provide the trajectory of a platform in unknown envi-ronment [5, p.153, p.309]. As a laser scanners do not rely on external infrastructurethey work indoors just as well as outdoors.

High localization accuracy could also be achieved with cameras. Cameras pro-viding depth information in addition to colors such as Microsoft Kinect based onstructured light[8], Microsoft Kinect v2 based on time-of-�ight[8] or stereo camerasare especially suitable for localization as they, like laser scanners provide measure-ment of spatial relations. While applicable for indoor localization their use requiredi�erent algorithms and as such they are out of scope for this thesis.

The SLAM algorithms used in robotics are commonly geared towards onlineuse as the position and map provided by them are used for navigating around theenvironment. In our case, the data is collected by a platform moved by a humanso there is no need for real time position information and the SLAM problem canbe solved o�ine using all the collected data. This enables more processing to beconducted to the measurements which should translate to better accuracy. Themain research questions this thesis answers are:

• What kind of trajectory accuracies can common SLAM algorithms providewith high quality sensors through o�ine processing

• Can the produced trajectory be used for projecting data to create a visuallyaesthetic and good quality point cloud

To study these problems we mounted an IMU and two high quality laser scanners,one horizontal for SLAM and the other tilted for point cloud data collection to apushable wheeled cart.

The thesis starts with an introduction to the working of sensors, �rst of a laserscanner (Section 2.1) then of an inertial measurement unit (Section 2.2). Section3 describes basic principle of SLAM and the three studied SLAM algorithms intheir basic form. Detailed explanation of the hardware used is given in Section

3

4. Section 5 starts with explanation on the basics of Robot Operating System(ROS) framework which is used for data processing. The modi�cations made on theSLAM algorithms presented next and �nally the total pipeline from data collection to�nished 3D point cloud. Section 6 describes the experiments made to assess researchquestions. The Section starts with the explanation of a calibration procedure usedto �nd the transformation from the data collection scanner to the vehicle frameand then proceeds to describe how the reference point cloud and trajectories werecreated. Next Sections describe the results of the experiments, �rst for the producedtrajectories and then for the produced point cloud. Finally in Section 7 conclusionsare made and future directions for research are discussed.

4

2 Background

To localize itself, platform just like human requires some way to perceive the worldaround it. Wide variety of sensors with di�erent cons and pros can be utilized togive a robot a sense of its movements and of its surroundings.

2.1 Laser Scanner

Laser scanner (also called LiDAR, scanning laser range �nder or just laser range�nder) is a device measuring distances. Basic operating principle of the scanner isshown in Figure 1. Commonly the mirror is rapidly rotated to measure distances ina 2D plane. Recently manufactures such as Velodyne [9] have begun to o�er laserscanners which have multiple lasers (up to 64 for Velodyne HDL-64E) of which eachscans a di�erent plane to enable perceiving in 3D. Laser scanners are widely used inrobotics as a measurement device of choice for perceiving the world as they provideaccurate spatial information in easily comprehensible format which requires littleprocessing to be usable.

Figure 1: Principle of a 2D planar laser scanner, as the mirror rotates the beamscans a plane. [10, p. 38]

Two methods for measuring the distance travelled by the laser beam to the objectare commonly employed, one based on measuring of the time-of-�ight of a pulse andone on measuring the phase di�erence of a modulated continuous wave.

In time-of-�ight (pulsed laser) measurement a short laser pulse is emitted andthe time ∆t between sending and receiving re�ected echo is measured. As the speedof light c is constant and known to a high precision, the distance to the target canbe calculated with Equation 1, where n is the refractive index of air [10, p. 3].

r =∆t · cn · 2

. (1)

In phase di�erence measurement system a continuous amplitude modulated beamis transmitted to the target and the phase di�erence of the transmitted and receivedwaveform is measured. When the modulation frequency fm is known, the phase

5

di�erence ∆ϕ can be used to calculate the time di�erence ∆t as shown in Equation2. ∆t can then be translated to range r by Equation 1.[10, p. 6]

∆t =∆ϕ · fm

2π. (2)

If the actual range is more than half of the wavelength, the actual time di�erenceand therefore range cannot be determined uniquely as only the phase di�erence canbe measured. Multiple modulation frequencies can be used to overcome this rangeambiguity [10, p. 6]. Starting from the longest wavelength, the phase di�erence isused to calculate a coarse range which can then be re�ned by the phase di�erencesin the shorter wavelengths to obtain a unique and accurate range measurement.

Unlike ideal rays, real life laser beams have some radius which grows by rangeas the beam diverges [10, p. 12]. This can cause ambiguity to the measurementwhen the beam hits an edge of an object. Part of the beam is re�ected back fromthe object in the foreground and the rest from the background. If the arrival timesof the re�ections are averaged the result is a 'ghost point' between the objects withno physical meaning.

The problem with multiple re�ections is even more profound with phase di�er-ence measurement based laser scanners. When hitting a spatial discontinuity, themeasured phase and therefore range depends on the distance between foregroundand background objects, their relative re�ectance and the proportion of the beamhitting each one. These a�ect the measurement in an unpredictable way and theresulting range measurement can be anything between the maximum range of thescanner and the distance to the closest object and in some rare cases even smaller[11].

Probabilistic modelling of the measurement process can be used to include noiseand other uncertainty always inherent in real life measurements to localization meth-ods. Commonly the measurement can be modelled by a conditional probability den-sity p(zt|xt,m) where zt is the measurement (single sweep of a scanner), xt is thepose of the scanner and m is the map. The individual measurements of the sweepcan be assumed to be independent of each other so the combined probability canbe calculated as shown in equation 3 where k denotes the index of the individualmeasurement.

p(zt|xt,m) =K∏k=1

p(zkt |xt,m) (3)

Even though the independence assumption is often not true in real world scenarios,it simpli�es calculations and the resulting algorithm still works [5, p. 149].

Modelling each laser measurement as a beam is an obvious way and is stronglylinked to the physics of the device [5, p. 153]. Di�erent error sources are modelledby di�erent distributions which are combined together using prede�ned weights.Examples of distributions can be seen in Figure 2. The beam model is accurate,but ray casting operation is required to see where each beam would hit from thelocation xt to compute zk∗t , which is computationally expensive. Additionally, incomplex environments with clutter the model is not smooth. Small changes in the

6

pose, especially in the heading direction can cause individual measurements and theprobability p(zt|xt,m) to change radically which makes this model less useful withalgorithms trying to optimize the pose [5, p. 168].

Figure 2: Example of common combination of point density functions used withbeam model. zk∗t is the range calculated by ray casting and zmax is the maximumrange of the sensor. Blue represents the measurement uncertainty in the actualmeasurement (Gaussian distribution), green represents possible dynamic obstaclesin front of the target (Exponential distribution), red represents random noise fromunexpected sources and pink represents the possibility of a failed measurement [5,p. 154].

A simpler and computationally more e�ective option to model the uncertaintyis to use likelihood �elds [5, p. 169]. In this model only the endpoints of themeasurements are considered and the probability can be calculated as a combinationof zero mean Gaussian and an uniform probability as shown in Equation 4.

p(zt|xt,m) = zhit ·1√

2πσ2e−(dist)2/2σ2

+ zrandom · prandom (4)

The dist is the euclidean distance between the measured point and the nearestoccupied grid location and the Gaussian then models the uncertainty in the measure-ment accuracy while the prandom models the possibility of a random measurementalways possible with laser scanners, failed measurements with zmax value are ignoredaltogether.

While the inability to model dynamic obstacles reduces the accuracy when obsta-cles are present, the smoothness and computational simplicity make this algorithmmore useful [5, p. 173]. One clear advantage of this method is the possibility toprecompute the likelihood values of a map or a single scan by using a discrete grid,the operation in Equation 4 is then replaced by a simple table lookup [5, p. 172].

7

2.2 Inertial Measurement Unit

Inertial Measurement Unit (IMU) is a device which measures linear accelerationsand angular velocities which are used in inertial navigation to calculate the position,velocity and orientation of the device [12, 13]. An IMU consists of accelerometersmeasuring linear accelerations and gyroscopes measuring angular velocities. Whilenot directly providing position and orientation information, IMU measurements canbe integrated over time to calculate them.

In traditional approach the IMU sensors are mounted on a stabilized platform[13]. The sensor platform is connected to the host vehicle through a set of gimbalswhich let it rotate freely and the angular velocity measurements are used to driveservo motors on the gimbal axes to keep the platform stable. With stable platformthe integration of accelerometer information is straightforward and the attitude canbe determined from the gimbal positions. While they provided good performance,the mechanical construct of gimballed system is complicated, and it is expensive tomanufacture and di�cult to miniaturize [12].

A simpler approach is to mount the sensors rigidly to the host vehicle in astrapdown con�guration. In strapdown IMU the mechanical platform stabilization isreplaced with a mathematical one by integrating the angular velocity measurementsto gain the orientation of the platform and then by projecting the accelerometermeasurements accordingly before integrating them. This requires su�ciently highsensor sampling rate to keep the errors caused by integrating discrete samples small.Gyroscopes also need to have much higher dynamic range (4 orders of magnitude)than in stabilized platform IMU as they need to be able to measure angular velocitiesfrom the full manoeuvre envelope of the vehicle. In online use strapdown IMU alsorequires more computing power as the calculations are more complex although withmodern computers this is not a big cost driver any longer. [12]

IMU based inertial navigation requires only the measurements from its sensorsand is a totally self-contained system requiring no infrastructure. The largest disad-vantage of inertial navigation is caused by the need to integrate the measurements,twice for accelerometers and once for the gyroscope. The integrated errors keepaccumulating with no limit if no external corrections are available, as the IMUmeasurements are not error free. The errors in the orientation of the IMU are espe-cially harmful as the orientation is also used in the integration of the accelerometermeasurements. Even a tiny orientation error quickly manifests as a large error in po-sition. It is also possible to include magnetometers to an IMU; measurement of thestable Earth magnetic �eld direction can help to reduce the drift error in orientation[13]. Ferromagnetic materials interfere with the �eld which complicates the use ofthese measurements, the interference e�ect is especially pronounced in buildings asthey commonly contain high amount of ferromagnetic materials in the form of, forexample, reinforced concrete.

8

3 Simultaneous Localization andMapping Algorithms

In an unknown environment the robot faces two problems, it neither knows itslocation xt nor does it know the map m of the area. Inferring these two piecesof information from measurements of robot's internal sensors u1:t (IMU, odometry)and measurements of the world z1:t is commonly called Simultaneous Localizationand Mapping (SLAM) [5, p. 309]. A schematic picture of SLAM problem is shownin Figure 3. The problem of estimating the posterior p(x1:t,m|z1:t, u1:t) is di�cultbecause there is a circular dependency between the robot needing a map of itssurroundings to localize itself and the need to know its location for building themap. Fortunately, this problem can be solved by iteratively localizing the robotpose relative to its starting position and by using this information to construct themap.

Figure 3: Schematic picture of a full SLAM problem. The aim is to estimate allrobot poses and a map (grey background) from the controls and the measurements.When solving an online SLAM problem we are only concerned with the map andthe last pose.[5, p. 311]

SLAM algorithms can be divided into a frontend and a backend. The frontenddeals with sensory input doing scan matching and calculating spatial relations be-tween poses. The backend on the other hand deals with the collected informationand tries to optimize the estimated robot poses to keep the resulting map coherent[14]. In practise a SLAM frontend is enough to solve the SLAM problem but as thesmall errors in scan matching will inevitably accumulate and reduce the accuracy,in complex environments with longer trajectories and loop closures (returning to anarea visited before) a backend can greatly improve the result.

Scan Matching aims to �nd the rigid-body transformation (translation ∆x,∆yand rotation ∆θ) between two scans or a map and a scan. If available, the trans-formation estimate from motion measurements such as odometry can be used as aninitial guess which is then re�ned by a variety of methods. Scan matching is an inte-gral part of SLAM algorithms as (if successful) scan matching provides a much moreaccurate estimate of the movement between two time steps than internal sensors.

9

This thesis concentrates on SLAM algorithms working on a 2D plane using vol-umetric maps typically expressed as an occupancy grid. Occupancy grid is an �neuniform grid covering the whole area of interest. Typically the grid cells can havethree values, occupied (by wall for example), free or unknown. Probabilities canalso be incorporated by giving each cell a probability of it being occupied which isthen updated when new measurements are taken [5, p. 284]. Maps and localizationbased on landmarks could also be used but with occupancy grids the whole scandata can be used in the localization and mapping process and there is no addedcomplexity of extracting and detecting landmarks.

ROS framework is used for data processing in this thesis which constrained theSLAM algorithms tested to ones available as ROS packages. Machado Santos etal. [15] evaluated di�erent SLAM algorithms available in ROS and the three bestperforming SLAM algorithms based on their evaluations were selected for tests withhigh quality sensors in this thesis. An additional motivation for selecting the algo-rithms was their use of di�erent techniques for solving the SLAM problem.

3.1 Hector SLAM

Hector SLAM algorithm utilizes the full scan rate of modern high frequency laserscanners. By using every subsequent scan the search space for rigid-body transfor-mation between scans stays small and the transformation can be found in real time.The correct transformation is found by optimizing the alignment of scan endpointswith the map learned so far. Gradients of the grid cells are calculated at scan endpoints and the pose is optimized using Gauss-Newton method [7].

Figure 4: The position estimated by Hector SLAM has diverged from the correctone after running a couple of minutes in an area with short visibility and almost noline of sight to previously mapped area. The current laser scan (colored points) doesnot match with the walls mapped previously. Red arrow marks the current positionand green line marks the trajectory so far.

10

Hill climbing style optimization methods such as Gauss-Newton are prone togetting stuck at local optima. To ensure convergence to global minimum, the al-gorithm maintains a pyramid of multiple occupancy grids with each having halfthe resolution of the preceding one [7]. Scan matching is started with the coarsestresolution occupancy grid and then repeated on �ner grids using the result of scanmatching done at the previous resolution as the starting guess. Bilinear �lteringis employed to reach precision greater than available from discrete sized grid cells[7]. As Hector SLAM only uses the scan endpoints for matching and does not usethe structure of a laser scan, the endpoints can be processed in �exible way. Thisenables Hector SLAM to take into account attitude information from IMU whichis used for projecting the laser scans to horizontal plane. This improves matchingprecision if the laser scan attitude di�ers from horizontal.

Even though the high quality scan matcher gives the algorithm good accuracy,the position slowly drifts away (Figure ??) and during the closing of a loop the lack ofoptimizing backend can show up as a discrete jump in the trajectory when the poseis recovered. If the accumulated error is large enough, the pose might not recoverand the errors continue to accumulate while the mapper overwrites previously builtmap.

3.2 GMapping

GMapping is based on Rao-Blackwellized particle �lter which can also be used tosolve the SLAM problem. Particle �lter is a non-parametric implementation of aBayesian Filter where, instead of describing the posterior distribution with a para-metric function, it is represented by a set of discrete samples (particles) drawn fromthis distribution [16].

The localization step of GMapping is similar to the Monte Carlo Localization(MCL) method, where large number of particles xt, each representing a possible pose(state) of the robot at a speci�c time is created and maintained. The localizationis an iterative process which requires a set of particles Xt−1 (pose hypotheses),control ut, measurement zt and a map m as an input. As the �rst step of eachiteration of the �lter and for each particle, the probability p(xt|xt−1, ut) which isbased on the motion model is sampled to get a new pose hypothesis xt. A weightwt = p(zt|xt,m) is then calculated based on the measurement model and assignedto the pose hypothesis. These new particles and corresponding weights form atemporary set X̄t. To complete the iteration of the �lter, resampling of the particlesis done. A resampled set Xt is created by drawing with replacement from X̄t witheach particle drawn with probability based on its weight. Now even though thesample set was drawn from just the motion model, the resampling based on themeasurement model has changed the distribution to re�ect the measurements. [5,p. 250]

During SLAM the algorithm must also estimate the map in addition to theposition, each particle also holds a copy of the map generated by it so far. The SLAMproblem can be factorized to �rst estimating the movement and then estimating themap as shown in Equation 5. This type of factoring is called Rao-Blackwellization,

11

hence the name [5, p. 435][6, 17].

p(x1:t,m|z1:t, u1:t) = p(m|x1:t, z1:t) · p(x1:t|z1:t, u1:t) (5)

As can be seen, the movement estimation step is similar to Monte Carlo local-ization based on a known map. A straight forward way to solve the SLAM problemwould be by just adding a step to each iteration where for each particle the esti-mated pose is used to integrate the current laser scan to the map learned so far.[5,p. 478] Unfortunately it is ine�cient to use as many particles as with MCL becauseeach particle must maintain a map which requires processing power and memory.The ability to use less particles makes the algorithm less robust.

Grisetti et al. [6] proposed an improved method where they use a more accuratext proposal sampling process and only resample the particles when needed. Theimproved proposal sampling method is based on the insight that the informationcarried by the laser scanner measurement is much more accurate than one availablefrom odometry. It employs scan matching, which uses the position from odometry asthe initial guess. The area around the pose acquired by scan matcher is then sampledand evaluated to calculate a Gaussian approximation of the proposal distribution.New pose for the particle is then drawn from this distribution instead of the motionmodel.

While particle resampling is necessary to replace samples unlikely to representthe correct robot path and map with better ones, it also carries the risk of replacingthe correct hypothesis with a worse one as there is random element in the resamplingstep. To reduce this risk Grisetti et al. only commit the resampling step when theparticle set stops approximating the target posterior well enough i.e. when thevariance of particle importance weights rises above a prede�ned level [6] . Withthese improvements the number of particles needed for consistent map building andlocalization by a factor of ten as compared to the method employed by Hähnel,Thurn et al. [5, p. 474].

While the algorithm does not have an optimizing backend, the multiple hypothe-ses of the map that are carried by the particles help maintain a coherent map evenwith loop closures.

3.3 Karto

Karto [18] uses correlative scan matching algorithm by [19] in its frontend andSparse Pose Adjustment (SPA) [14] as its backend. The scan matcher requires anestimate of movement as its �rst guess (from for example odometer, in our casefrom Hector SLAM) and then performs a search around it at discrete steps over asearch window. The likelihoods of di�erent transformations are calculated with thehelp of a lookup table similar to one used in the likelihood �eld model explainedearlier in Section 2.1. To narrow down the search over the large 3D space (x, y, θ)of possible transformations, the area is �rst evaluated at a coarser scale against alookup table with larger cell size to �nd areas of interest for �ner search. The useof multiple resolutions enables the algorithm to run in real time. An additionaladvantage of the method is that, as a large variety of transformation candidates

12

are evaluated, their likelihoods can be used to estimate covariance values for theproposed transformation. This value is important for the backend as a weight forthe constraint created from the scan match. Covariance values calculated from largerset of transformations are less overcon�dent than the ones calculated from a smallerset of transformations [19]. The downside of discrete steps is the maximum limiton accuracy they give. This is especially noticeable if every scan from a scanneris input to the algorithm as the displacement between scans can be less than thesearch resolution and this causes the localization to become unreliable.

The current scan is matched with a �xed size rolling bu�er of previous scans.Old scans are searched after each scan match to see if a su�ciently long connectedchain falls under predetermined radius from the current location (chains closelyconnected to the current scan are ignored). If a chain of scans is found a loopclosure is attempted by performing scan matching between the current scan and thefound chain of previously processed scans, if the scan matching succeeds with matchresponse value larger than predetermined threshold, loop is closed and a constraintbetween those poses is added to the underlying pose graph.

The SPA based backend of the Karto uses a pose graph structure where scannerposes xt form the nodes and the edges between them are the scan matching resultszt from the frontend. The measurements always carry some errors from sensornoise etc. and as such, there are no poses which would satisfy all the constraints.Therefore the graph based SLAM problem must be considered as an optimizationproblem where one seeks to minimize these errors [20]. As the number of nodesgrows the accompanying non-linear constraints form a large system of equationswhich would be di�cult to solve if it were not for the fact that as nodes are for themost part only connected to nodes nearby, the system is sparse [5, p. 343][14]. Byemploying solvers speci�cally tailored for sparse systems such as sparse Choleskyfactorization the optimization can be performed quickly enough to be run in realtime [14, 20]. The Karto with its SPA backend has been shown to give excellentresults when compared to other algorithms [21].

Figure 5: Example graph, poses are represented by circles and world features bydiamonds. In the left side, red arrows denote measurements zt and black onesdenote odometry ut. In the right graph they both are combined to spatial constraintsbetween poses.

13

An example of a small graph is shown in Figure 5. With maps based on oc-cupancy grids the features and corresponding measurements are replaced with ascan matching operation between the poses where the feature was observed. Thepose optimization is sensitive to false constraints caused by a failed data associationduring loop closure or wrong result from scan matching [22].

14

4 Hardware Implementation

The Slammer indoor MLS uses the same sensor module that has previously beenused outdoors on various scenarios either mounted on a car, all-terrain vehicle orbackpack [23, 24]. The sensor module consists of a NovAtel SPAN Flexpak6 GNSSreceiver with tactical grade IMU (UIMU-LCI) and two Faro Focus 3D (S120 andX330) high precision laser scanners. For the indoor use the sensors are mounted ona wheeled cart as shown in Figure 6. The S120 laser scanner is mounted horizontallyto measure the platform movement, while the X330 in front of the system is tilted10 degrees from the vertical for producing 3D point cloud of the scene (the angle isadjustable at 10 degree steps for optimizing the con�guration). The positions of thelaser scanners are interchangeable.

All devices (GNSS antenna, IMU and laser scanners) are connected to the SPANcentral unit which is then connected to a tablet computer recording the receiveddata. From the IMU its attitude at 20 Hz and accelerations and angular velocities(corrected for gravity and earth rotation) at 200 Hz are recorded. The laser scannersonly send a timing signal to the SPAN system for logging (pulse when a new scanstarts). The collected laser measurements are saved on a SD card on each of thescanners. On start up the SPAN system updates its clock and needs a GNSS signalfor it. The SPAN system itself could provide a trajectory estimate but this wouldrequire frequent GNSS signals which are not available and therefore only the IMUmeasurements are used.

Table 1: The scanner speci�cations. Ranging accuracy and noise are at 25 m range,noise minimum with 90% re�ectance and maximum with 10 % re�ectance.

Scanner Range Wavelength Field of View Measurement speedm nm Deg kpoints/s

S120 120 905 305 122-976X330 330 1550 300 122-976Scanner Rotation Rate Min. Ang. Res. Ranging error Ranging noise

Hz mrad mm mmS120 30-97 0.157 ±2 0.95-2.2X330 30-97 0.157 ±2 0.3-0.5

The laser scanners are designed for TLS and as such the whole scanner is ablerotate around its mount. For our purposes this is not required and the scanners areset to helical scan mode where the scanner itself stays stationary and only the mirrorrotates. Their TLS background can also be seen in their settings, rotation speedand the number of points on a single scan can be set by adjusting scan resolutionand the degree of measurement averaging. The scanner speci�cations are listed inTable 1. The scanners are largely similar but X330 as an evolved version of S120has longer range and lower measurement noise.

15

Figure 6: Slammer platform consisting of IMU (NovAtel UIMU LCI, #3 in the�gure), horizontally mounted scanner for SLAM (FARO Focus 3DS120, #1), andsecondary scanner for 3D point cloud generation (FARO Focus 3DX330 #2). Thescanners are interchangeable. The tablet computer (#4) is used for IMU and tim-ing data recording. Lines illustrate the approximate measurement planes of thescanners.

16

5 Software Implementation

5.1 Robot Operating System

Robot Operating System (ROS) is not an actual operating system like Windows orLinux but a �exible general framework for robotic software. At its core it providesa peer to peer communication layer for a network of software modules which run ona single computer or a cluster of heterogeneous computers [25]. In the frameworksoftware modules doing computation are called nodes. Nodes communicate witheach other by exchanging messages. The �elds of a message are de�ned using asimple interface de�nition language and they can contain basic datatypes such asstrings, integers or arrays of them or combinations of other message types. It is alsoeasy to create own message types.

Messages can be exchanged in asynchronous manner in publisher subscriber fash-ion or in synchronous mode as services in client-server mode [25] . In publisher sub-scriber communication a node (or multiple nodes) can publish messages (e.g. sensormeasurements or results of data processing) to a topic speci�ed by a string. Nodesinterested in this information can subscribe to the topic to receive the messages. Inclient-server communication one node acts as a server by advertising a service, whichis speci�ed by a name and nodes interested in this service can use it by connectingto it as a client and sending a service call which has two sets of �elds similar to mes-sages, one for request and one for response. In both communication modes a ROSmaster node keeps track of topics/services and corresponding subscribers/clientsand publishers/servers to inform nodes where they need to send their information.An example of how nodes are linked together through communication can be seenin Figure 7.

In addition to the communication framework ROS and the accompanying com-munity provide numerous tools and libraries which enable developers to concentrateon their main problem instead of developing supporting software. Commonly usedtools include rviz visualization tool and rosbag recording/playback tool. Rviz en-ables easy visualization of many message types from point clouds to trajectorieswhile rosbag enables one to record messages from topics of interest for later use.

The transformation library tf [26] is an important part of ROS framework asmobile robots commonly have various coordinate frames (for sensors, tools and therobot itself) requiring data to be transformed between them. The tf library listensto messages containing transformations between di�erent coordinate frames from allnodes and forms a tree structure of the frames for easy traversal. The tf node alsokeeps a history of each transformation between two frames for a predetermined timeperiod. Nodes can then query transformations between any two frames from the tfnode at any (current or past) time instant, in case there is no transformation at thequeried time instant, one is interpolated from the closest ones.

17

Figure 7: Example node graph of Hector SLAM running on data played from bag�le. Boxes denote nodes and arrows between them denote messaging between themwith the names above arrows denoting topic names. The communication consistsmostly of nodes passing transformations between them.

5.2 Modi�cations to the SLAM algorithms

Two modi�cations to the SLAM algorithms were made, one for Hector SLAM andone for Karto. Both are optional features which can be disabled.

The movement of the scanner during single scan sweep causes distortion to thescan adding error to scan matching, an example of this e�ect is shown in Figure 8. Ifthe movement is known, it can be compensated by transforming each measurement ofthe scan according to the scanner pose at the time of measurement. To improve theperformance of the algorithm we added a processing step before matching to mitigatethis e�ect. The IMU deployed is of su�cient quality and the IMU measurementscould be used to estimate this movement but to keep the estimates from driftingaway, fusion of the IMU and SLAM estimates with help of for example extendedKalman �lter (EKF) would be required. An EKF implementation provided by themakers of Hector SLAM was tested but no set of parameters resulting in convergentbehaviour were able to be found.

As an intermediate measure only the rotational component of the movementis considered. This is more straightforward as the IMU provides angular velocitymeasurements and orientation estimates based on them. Even though the absoluteheading (rotation around z-axis) value based on the IMU measurements drifts awayfrom the truth, it can be used to estimate the relative rotation over short periodsof time accurately. In this work Hector SLAM algorithm was augmented with thisfeature. Currently only the scanner start and end poses are queried from the tf nodewhile the intermediate poses corresponding to each single scan point measurementare linearly interpolated . Karto and GMapping algorithms make assumptions ofundistorted scan when doing scan matching, which makes implementing movement

18

Figure 8: E�ect of movement on a scan. Yellow coloured points denote the scantransformed to compensate for movement during the scan. Other coloured pointsrepresent the scan without any processing. As the platform moves forward themeasurements captured on latter part of the scan appear closer than where theyshould be. A previously calculated trajectory was used to give an estimate of themovement. The point size is 5 mm.

compensation scheme implemented for Hector SLAM more di�cult. It is a task offuture experimentation to be fully exploited.

The Karto library is mainly aimed for mobile robots and online use, but as wedo the processing o�ine with all of the data available some changes can be made. Inaddition to using parameters which provide higher accuracy and are computationallytoo heavy for online computation, a modi�cation to the loop closing function wasmade. In its original form a loop is closed immediately when a match betweencurrent scan and a chain of nearby processed scans gives a response value greaterthan a predetermined threshold. As processing is done o�ine, there is no immediateneed to close the loops and an approach called delayed loop closure is used instead.With delayed loop closure when an acceptable loop closure is found, instead of closingit the scan and corresponding scan chain are saved and the processing of subsequentscans is continued. Loop closure attempts with saved scan chain are continued duringthe processing of the subsequent scans and if a higher response value is attained,the saved scan is updated with the scan with better match. This is continued aslong as loop closure is attempted with a member of the saved scan chain. After nomore updates to the saved scan and corresponding chain are attempted the loop isclosed.

This is advantageous as a better match should lead to smaller error and the matchgenerally gets better as the scanning locations get closer together. In the originalform, if the threshold is small a suboptimal loop closure is made when the currentlocation is still far away from the chain of scans but if the threshold is too high,some not as good but still useful loop closures might be missed altogether. Withthe delayed loop closure approach more loop closures can be made while ensuringthat they are as good as possible.

19

5.3 Processing pipeline

Robot Operating System (ROS) [27] framework is used for data processing . Itprovides a straightforward way for testing, comparing and combining di�erent al-gorithms as all of the algorithms can be used within the common framework. Thepipeline from collected data to 3D point cloud of an area can be seen in Figure 9.

The data preprocessing starts by transforming the IMU measurements and times-tamps logged in a propriety binary format to ASCII text. The transformation isdone using a NovAtel convert4 tool. Next the ASCII �le is parsed with a Matlabscript to separate the di�erent message types and messages from di�erent devicesto separate �les. The text �les and recorded laser scan data are then combined toform IMU and LaserScan ROS messages.

The IMU messages contain attitude, accelerations and angular velocities of thesystem. As attitude is recorded at lower frequency (20 Hz vs. 200 Hz) than acceler-ations and angular velocities, the attitude information for messages without one isintegrated from the angular velocities. For laser scan messages the propriety Faroformat is read with the help of Faro SDK and then combined with a time stampto form a ROS message. The IMU and laser scan messages are then combined toa single ROS bag �le. The laser scans from the scanners are saved separately forthree separate topics in total. ROS natively provides a method to play the messages

Figure 9: Pipeline from data collection to �nalized point cloud. Boxes are processingsteps and arrows denote data �ows

in a bag �le for the algorithms but it is mainly aimed to simulate real time use.Only the speed of the playback of the saved messages can be controlled but thereis no way to force every message to be processed. If internal bu�ers get �lled withmessages faster than they can be processed the oldest ones are simply ignored. Toensure every message gets processed and that there is no delay between the process-

20

ing of subsequent messages, the SLAM algorithms were extended with a speci�c baghandler class which processes the messages in right temporal order with no waitingtimes. To enable the laser scan messages to be correctly transformed to world co-ordinates, pose and attitude related messages with timestamps later than currentlaser scan timestamp must have been processed �rst.

With the data in ROS formats the actual processing is started by running thelaser scan and IMU data through Hector SLAM algorithm to compute an initialtrajectory estimate. The Hector SLAM is the only algorithm studied which workswithout an initial guess of movement from odometry. The GMapping and Kartoalgorithms require an initial guess and as there are no wheel encoders to provideodometry in Slammer platform, the Hector trajectory is used instead.

The Hector SLAM naturally provides dense trajectory and in environments withgood overall visibility of the area, i.e. no loop closures after long periods exploringnew areas, the algorithm could provide a su�ciently accurate trajectory. While thelack of a backend causes the algorithm to slowly drift away from the correct solutionin more complex environments as shown in Figure 4, the accurate matching provideslocally very good estimates of the path taken and as such can be used to generate asort of accurate fake odometry which can then be employed by the other algorithms.

The Hector SLAM projects the measured points into a XY plane based on themeasurements of the IMU. Even though the sensors are mounted on a wheeledplatform and only driven on a �at �oor, taking into account the small constanto�set in attitude and the small �uctuations (less than one degree) caused by lowquality wheels does slightly improve SLAM performance.

The large discontinuities caused by loop closures in the Hector trajectory aresmoothed away before its use in other algorithms. This is accomplished by checkingeach transformation between two poses and by comparing it to the preceding ones.If there is a large change in the transformation length or direction from the precedingones, the transformation is replaced by an average of the preceding transformations.

The smoothed trajectory is then used as input to Karto and GMapping andprocessed. To speed up the processing only a subset of the scans are processed byKarto and GMapping, in our case with frequency of 3 Hz. Both algorithms also sup-port the ability to process a new scan only if the scanner has moved predeterminedamount since last scan used. Time based subset approach was selected as it enablesthe algorithms to process scans also when not moving. When stationary the scansare undistorted and should provide the best matches.

After processing with Karto or GMapping the produced sparse trajectory is com-bined with the dense smoothed Hector trajectory. This is accomplished by perform-ing an a�ne transformation to each subtrajectory which aligns the subtrajectoryendpoints with the sparse trajectory. Headings of the poses of subtrajectories areinterpolated from the sparse trajectory.

Finally the combined (or in the case of Hector the non smoothed one) trajectoryis used to transform the scans made by the tilted data collection laser scanner to form3D point cloud of the whole area. The scans are transformed point by point basedon the position from SLAM and attitude from IMU, the transformation applied toeach individual measurement is linearly interpolated from the transformations at

21

the beginning and end of the whole scan.

22

6 Experiments

To test our approach we made 5 test runs in the FGI o�ce, which has both longcorridors and cluttered spaces with limited visibility. A map of the area with labelsis shown in Figure 10. The test runs were made in two sets, three with fastermovement speed and Faro S120 scanner as the horizontal SLAM scanner and twowith slower movement speed and the newer Faro X330 as the horizontal scanner.Details of the datasets are shown in Table 2. While each test run had di�erent routethey all started and ended in the top part of the upper corridor and consisted of onevisit to the lower corridor and some exploration of the library.

Figure 10: Map of the �oor used for experiments with labels of di�erent areas

All tests were made with the same IMU logging settings, acceleration and angularvelocity measurements were logged at 200 Hz and attitude calculated by SPAN at 20Hz. Horizontal laser scanner maximum range was set to 41 meters, which is close tothe the length of the longest corridor and enables the scanner to detect everythingin its �eld of view.

6.1 Reference

Faro X330 laser scanner was used in TLS mode to generate a point cloud of thestudy area for geometric reference. Target spheres with 199 mm and 145 mm diam-eters were placed around the area and used for registering. Spheres were selected

23

Table 2: Test runs starting with X were made with X330 scanner as the horizontalscanner and ones starting with S with S120. Length is the length of the trajectoryand speed is the average movement speed.

Data acquisition settings and trajectory lengthTest Rot. Rate Ang. Res. p/scan Length Speed

Hz mrad m m/sX1 59.7 0.767 6828 256.9 0.27X2 59.7 0.767 6828 265.6 0.25S1 47.7 0.613 8536 268.5 0.52S2 47.7 0.613 8536 312.1 0.52S3 47.7 0.613 8536 232.6 0.49

(a) (b)

Figure 11: Details from the horizontal slice of the TLS scan used to generate thereference trajectory. The TLS scan points (red, point size 5 mm) do not alignperfectly with each other which is an evidence of registration errors between di�erentTLS scans. Grid cell size is 10 mm.

over paper or paddle targets as they provide superior registration accuracy [28]. Al-together 31 TLS scans were taken, which were then registered together into a singlepoint cloud with the help of Faro Scene software. The mean standard deviationof the detected sphere locations from di�erent scan locations was 4.7 mm on theregistered point cloud. Visual inspection of a horizontal slice of the point cloud wasperformed and misalignments of similar, around 5 mm magnitude were detected asseen in Figure 11.

To obtain reference trajectories for the test runs, a 50 mm thick slice aroundthe elevation of the horizontal laser scanner from the �oor level (about 50 cm)was extracted from the reference point cloud and the scans collected during eachof the test runs were matched to it with the Hector SLAM algorithm. The useof Hector SLAM instead of using Monte Carlo Localization or just matching thescans to the reference map is advantageous as there are places (under desks etc.),

24

which were occluded and not seen in the TLS based reference map. The SLAMalgorithm can generate a map for those locations too and use it for localization.The parameters of the Hector SLAM were set to only update the underlying mapwith very low probability and so the original geometry of the TLS based referencemap was preserved and used for matching when available.

After being generated by Hector SLAM the reference trajectories were inter-actively inspected by checking that each scan correctly matches the slice of thereference point cloud. A trajectory modi�er tool, which enables easy inspection andmodi�cation if necessary, was created and used for this task. This inspection wasnecessary as the matching by Hector SLAM occasionally fails and the trajectorycan deviate more than 50 mm from the correct one. Precise manual correction wasextremely di�cult with the scans distorted by movement and some small (under 10mm) errors remain in the reference trajectories.

6.2 Calibration

The IMU and laser scanners all produce data in their own coordinate frames, to beable to combine measurements from them, the transformations between the coordi-nate frames must be known. An accurate estimate of the transformation betweenthe horizontal scanner and IMU had been acquired during earlier use of the sensormodule and there was no need for any further work with that transformation. Thephysical con�guration for the second data collection laser scanner had not been usedearlier and the transformation had to be found.

Figure 12: Coordinate frames of the Slammer platform, x axis is red, y is green andz is blue

As a �rst step, the o�sets between the IMU and the data laser scanner weremeasured with a tape measure. A simple calibration routine was then devised tore�ne the tape measurements to an estimate closer to the real one. The routineutilizes the platforms ability to localize itself accurately and is based on the insightthat if same the object is observed multiple times by the data collection scanner, themeasurements will only align with each other when the transformation is correct.

25

A short section of a corridor was repeatedly measured with the MLS platformby moving in both directions and the trajectory was calculated in same manner asused to create reference trajectories in Section 6.1. A point cloud was then createdand three perpendicular planes were used as the objects (two walls and a �oor) tomeasure the quality of a transformation. A subset of points near the real locationof a plane was selected for processing. Random Sample Consensus (RANSAC) [29]algorithm was used for detecting the plane and then the average distance fromeach point of the selected subset to the plane was calculated. This was repeatedfor all planes and the average distances from each plane were then averaged fortransformation quality metric. Point Cloud Library (PCL) [30] was used to processthe point cloud.

(a) (b)

(c)

.(d)

Figure 13: Result of the IMU → data collection scanner calibration. The left sideis produced based on the initial measured estimates and the right side shows thesituation after running the calibration routine. The object in top is a door frameseen from above and that in bottom is a side view of �oor, trim and wall.

A supervised brute force approach was used to �nd the correct transformation.Limits and step sizes for all of the 6 parameters were set and all possible combinationswere evaluated. The correct transformation was found by starting around the initialparameters with a large step size and progressively moving towards smaller step sizearound good transformations. While some automated optimization scheme couldhave been implemented and would have required no supervision and manual settingof the limits between changing step size to �nd the correct transformation, thissimple approach was su�cient for this one time use. The in�uence of calibrationcan be seen in Figure 13. The transformation found is shown in Table 3. The

26

smallest step size used was 0.5 mm for translations and 5 mrad for rotations whichgive the upper limit for calibration accuracy. The actual accuracy is less than thisbecause the accuracy of localization also limits the transformation accuracy availablewith this calibration approach as errors in localization translate to errors in the pointcloud.

Table 3: IMU → data collection laser scanner transformation. Measured values arebased on tape measurements and estimated rotations while calibrated values areacquired with the help of a calibration routine.

Linear o�sets Rotationsx y z roll pitch yaw

mm mm mm rad rad radMeasured 472 3 -247 3.142 -1.396 0.0Calibrated 462 6 -250.5 3.237 -1.409 -0.1

6.3 Evaluation of trajectories and e�ects of parameters

The Rawseeds benchmarking toolkit [31] was used with some modi�cations to com-pare the results of di�erent algorithms. It provides tools to calculate AbsoluteTrajectory Error (ATE) and has functionality to draw �gures of trajectories anderrors.

Absolute Trajectory Error (ATE) is calculated as shown in Equation 6 whereEi

trans is the translational error of the ith pose in XY plane. The poses calculatedwith SLAM algorithms are compared with poses from the reference trajectory.

RMSEATE =

√√√√ 1

n

n∑i=1

(Eitrans)2 (6)

Equation 7 shows the calculation of the the rotational component of the errorwhere Erot is the di�erence in the heading angle between poses of the computedtrajectory and corresponding reference one.

RMSErot =

√√√√ 1

n

n∑i=1

(Eirot)2 (7)

Absolute Trajectory Error is very sensitive to small errors in the beginning (es-pecially in rotation) as they cause the rest of the trajectory to diverge from thecorrect one. The trajectories were tied to the corresponding reference ones andthrough them to the TLS point cloud by setting their starting pose to the referencetrajectory starting pose. The results of �rst scan matches of SLAM processing havelarge e�ect on the precise orientation of the map produced and the random noisealways present in the scan matching cause the orientation of the map and trajectory

27

to also �uctuate slightly. To reduce this e�ect, all trajectories were rotated in a waywhich gives best alignment with the reference based on the �rst 10 meters of thetrajectory.

In Sections 6.3.1 to 6.3.3 the in�uence of several parameters of the algorithms isstudied and the quality of trajectories produced with good parameter combinationsis assessed. Parameters to be inspected were selected based on tests with a widervariety of parameters on a short section of test run S1 or because of their likelyimportance (for example map resolution).

6.3.1 Hector SLAM

Hector SLAM was tested with three occupancy grid maximum resolutions (1 cm, 2cm and 4 cm), two occupancy grid pyramid level heights (smaller value giving 8 cmmaximum grid cell size and larger giving 16 cm grid cell size) and with and withoutrotation compensation for a total of 12 parameter combinations. Average, minimumand maximum RMSE errors for translation and rotation for the di�erent parametercombinations are listed in Table 4. The errors of the slower and faster movementtest runs with X330 scanner are listed separately as they di�ered greatly from eachother. Full results for each separate test run are listed in Appendix A.

(a) (b)

Figure 14: Errors as time series of two runs with Hector SLAM. Diagrams on theleft side are from test run S3H241 and those on the right side are from X1H241.The large increase in the error is caused by visiting the lower corridor.

The bulk of the trajectory error for each test run was caused by a slight errorin the estimated orientation when visiting the over 40 m long lower corridor ofthe building. This can be seen as the large rise in errors in Figure 14, while themagnitude of error di�ers, all test runs showed similar tendency. The corridor isquite feature poor and as such di�cult for scan matching and as extra challengethe lobby which connects the two corridors contains glass and metal surfaces onmultiple sides which further deteriorates scan matching performance in the crucial

28

Table 4: ATE metrics and heading standard deviation with 100 pose moving windowfor the test runs with Hector SLAM. The �rst column denotes the test run andparameter combination. The �rst character marks the test run type, S for FaroS120 and X for X330, H stands for Hector SLAM and the last three digits indicatethe parameters used. The �rst one tells the occupancy grid resolution in centimetres,the second tells the number of levels in the occupancy grid pyramid and the last onetells whether rotation compensation was used (1) or not (0).

Position RMSE Heading RMSE HeadingTest Mean Min Max Mean Min Max SD100

mm mm mm deg deg deg mdegSH150 137.8 106.3 171.6 0.46 0.43 0.49 85.7SH151 151.1 109.3 176.1 0.56 0.45 0.62 66.0SH140 135.3 104.6 155.7 0.46 0.42 0.48 84.8SH141 133.0 98.3 155.5 0.47 0.41 0.52 57.5SH240 118.0 89.9 146.8 0.40 0.33 0.48 86.2SH241 115.6 71.0 140.8 0.37 0.33 0.43 64.3SH230 118.8 90.5 151.3 0.45 0.33 0.64 86.1SH231 194.3 68.8 376.7 0.59 0.32 0.96 61.7SH430 92.1 47.6 146.6 0.27 0.21 0.34 83.7SH431 122.8 50.6 233.1 0.30 0.16 0.54 78.8SH420 818.7 46.5 2341.4 0.47 0.22 0.92 88.4SH421 485.8 55.0 1330.9 0.37 0.16 0.69 70.4XH150 35.3 23.5 47.1 0.08 0.06 0.10 21.4XH151 40.0 25.5 54.5 0.11 0.09 0.13 17.8XH140 28.8 26.7 31.0 0.09 0.08 0.11 21.5XH141 30.0 25.8 34.2 0.11 0.09 0.13 18.0XH240 51.0 36.8 65.2 0.14 0.14 0.15 26.3XH241 45.9 24.7 67.2 0.13 0.10 0.15 22.0XH230 43.1 29.8 56.4 0.15 0.13 0.16 27.2XH231 38.2 36.2 40.1 0.13 0.13 0.13 21.5XH430 69.0 59.4 78.7 0.23 0.23 0.24 39.0XH431 82.1 67.9 96.3 0.32 0.22 0.42 39.3XH420 65.4 53.6 77.3 0.24 0.23 0.25 40.5XH421 67.4 44.0 90.8 0.34 0.19 0.49 35.5

time when mapping of the lower corridor starts. Spikes in the error during visit tothe lower corridor are caused by the position sliding along the length of the featurepoor corridor. This is the cause for The high range of the scanner would enable itto see the ends of the corridor all the time which would add objects perpendicularto the length of the corridor to reduce the sliding tendency but unfortunately thecorridor ends are mostly glass which cannot be reliably detected. With 40 m longcorridor a 0.15 degree error in the orientation when entering and starting to map

29

the corridor would result in 10 cm position error at the opposite end of the corridor.The maximum RMSE errors for SH231, SH420 and SH421 are all caused by

the SLAM algorithm being unable to recover correctly from erroneous scan match,instead it starts to build new map slowly overwriting the old correct one. It is nocoincidence that all these failures happened with the lower height setting of theoccupancy grid pyramid. Additional large cell sized occupancy grids add a layerof robustness to the scan matching but this comes with a price. In the remaining27 test runs with no major matching failure having the additional occupancy gridwith 16 cm cell size increases the RMSE value in 14 cases while the opposite is truein only 5 cases with the di�erence in the remaining 8 runs being below 5%. Thecoarsest grid is used for scan matching �rst and the coarsenesses of it can cause the�rst level match to be further from the correct one than the scan matching startingpose.

Most noticeable thing about the results is the large di�erence in position accuracybetween the test runs with di�erent laser scanners and movement speed. While theX330 is upgraded version of S120 and provides better performance, the di�erence inaccuracies is just too great to be explained by the similar but moderately updatedscanner. The more likely culprit for majority of the increased accuracy is the almosthalved movement speed of the X1 and X2 trajectories. Slower movement speed andhigher scanner rotation rate help the scan matching in two ways, less movement leadsto less of distortion to the scans while the transformation between two subsequentscans also stays smaller and easier to �nd as the search starts from the previous scanlocation.

The e�ect of the highest occupancy grid resolution setting on errors can also�rst seem counter intuitive, for XH test runs, higher resolution leads to lower errorswhile the opposite is true for SH tests. Again this is likely caused by the di�erencein movement and scanner rotation speed as the slight di�erences in the precision andnoise handling of the laser scanners is not large enough to warrant such a drasticdi�erence in SLAM behaviour. The faster movement a�ects both the map buildingand scan matching. Larger cell size can help mask the e�ect of movement relatedscan distortion during both scan matching and map building. With smaller cell sizethe inaccuracies in the scan matching cause the points to fall easier to di�erent cellsmaking the actual location of the walls less certain, with larger cell sizes the pointscan fall to the correct cells even when they are distorted or misplaced by localizationerrors.

The e�ect of slower movement speed on trajectory accuracy could imply thatrotation compensation could well improve performance but the results evidentlyshow that this is not the case. Out of the 30 test run results, the compensatedtrajectories were better in 11 cases, worse in 15 cases and in 4 runs there was nomajor di�erence (less than 5% di�erence from the non compensated one). In totalthe e�ect on trajectory accuracy in position and heading tilts to the negative and itclearly does not provide the improvement which was hoped from it.

Closer inspection of trajectory error time series with and without compensationrevealed that while the compensation scheme has little e�ect on the absolute ac-curacy, it greatly reduces noise in the heading estimates as can be seen in Figure

30

(a) (b)

Figure 15: E�ect of rotation compensation on Hector SLAM trajectory headingerror, left side is without compensation and right with. Lower ones show the totaltrajectory while the upper ones show zoomed in detail. Rotation compensationincreases smoothness while also adding single large errors. Left is from test runS1H150 and right from S1H151.

15. To better quantify this e�ect, moving standard deviation with 100 pose widewindow was calculated for all trajectories and the result can be seen in Table 4. Nosuch smoothing e�ect was observed for the position. As a down side the rotationcompensation also increases both amount and magnitude of relatively large singleerrors in the heading estimates which is evident in the Figure 15 and in the largemaximum heading errors which can be seen in Appendix A. The reason for theseoutliers could be in the SPAN central unit which occasionally drops measurementsfrom the IMU. These gaps in the orientation record could result in bad estimatesfor the rotation as the tf system of ROS interpolates the orientation values from themessages with closest timestamps.

6.3.2 GMapping

Twelve parameter combinations were tested on GMapping. Three map resolutions(1 cm, 3 cm and 5 cm), two sigma values (0.01 and 0.05) which are used by thegreedy endpoint matcher to smooth the map and two step sizes (0.04 and 0.02,angular and linear, meters for linear and radians for angular) used by the gradientdescent based scan matcher pose optimizer. During match optimization the stepsize is halved when better matches cannot be found using the current step size. Thehalving is continued as long as the results improve or at least for a predeterminednumber of times.

Smoothed Hector trajectories with parameter combination HS241 were used asthe initial trajectory providing laser odometry to the GMapping algorithm. Therandom nature of the sampling done by GMapping makes each run di�erent. Eachtrajectory and parameter combination was processed three times to reduce the ef-

31

Table 5: ATE metrics for the test runs with GMapping. The �rst column denotesthe test run and parameter combination. The �rst character marks the test run type,S for Faro S120 and X for X330, H stands for Hector trajectory and G stands forGMapping. The last three digits code for parameters used. The �rst one states themap resolution, the second represents the linear and angular optimization step sizeand the last one marks the sigma of scan end point matcher. All actual parametersare one hundredth of the given numbers. The parameter combination with lowesterror for XG and SG trajectories is made bold.

Position RMSE Heading RMSETest Mean SD Min Max Mean SD Min Max

mm mm mm mm deg deg deg degSG121 48.5 22.2 34.6 105.2 0.24 0.05 0.19 0.36SG141 31.9 5.5 25.5 39.8 0.21 0.02 0.19 0.24SG125 38.9 9.9 24.5 53.5 0.22 0.03 0.19 0.26SG145 43.0 13.4 23.3 65.7 0.23 0.03 0.19 0.27SG321 115.2 26.0 78.6 151.5 0.36 0.05 0.27 0.41SG341 124.8 44.3 76.4 200.0 0.38 0.09 0.26 0.50SG325 191.8 55.1 127.4 289.6 0.42 0.07 0.32 0.53SG345 197.2 28.7 161.2 262.5 0.44 0.09 0.32 0.58SG521 116.7 31.8 72.7 149.9 0.41 0.06 0.35 0.51SG541 113.3 29.0 69.4 146.7 0.40 0.03 0.34 0.45SG525 261.6 78.3 184.9 393.6 0.60 0.15 0.46 0.86SG545 270.7 65.0 188.1 392.4 0.65 0.17 0.42 0.96Mean 129.5 34.1 88.9 187.5 0.38 0.07 0.29 0.49XG121 52.8 8.3 42.6 65.9 0.17 0.03 0.13 0.20XG141 48.4 8.4 36.6 59.2 0.17 0.01 0.14 0.18XG125 54.3 16.7 29.6 77.8 0.17 0.02 0.15 0.20XG145 54.2 11.8 38.7 68.4 0.16 0.02 0.13 0.18XG321 35.1 12.1 22.5 52.3 0.16 0.02 0.14 0.19XG341 39.8 16.8 21.0 60.4 0.17 0.01 0.15 0.19XG325 154.3 38.4 103.9 205.0 0.46 0.26 0.27 0.91XG345 167.8 53.1 118.4 233.8 0.37 0.06 0.33 0.48XG521 27.8 10.4 16.1 38.7 0.16 0.02 0.14 0.18XG541 25.2 9.6 14.4 35.7 0.15 0.01 0.14 0.16XG525 119.4 27.8 92.3 156.9 0.28 0.03 0.25 0.34XG545 126.9 41.5 96.8 208.7 0.32 0.05 0.26 0.39Mean 75.5 21.2 52.7 105.2 0.23 0.04 0.19 0.30

fect of randomness. In total 15 test runs for each parameter combination were doneand the results were averaged for the analysing of the performance of the di�er-ent parameter combinations. GMapping has a hard coded maximum size of 2048measurements per scan and as scans in these test runs had considerably more mea-

32

surements, scans were subsampled to �t in to the limits of GMapping. The numberof particles used was 100.

Table 6: ATE metrics for the test runs with GMapping for selected parametercombinations. The �rst column denotes the test run and parameter combination.The �rst character marks the test run type, S for Faro S120 and X for X330, H standsfor Hector SLAM trajectory which after smoothing is used as initial trajectory andG stands for GMapping. The last three digits code for parameters used. The �rstone states the map resolution, the second gives the linear and angular optimizationstep size and the last one marks the sigma of scan end point matcher. The parametercombination with lowest error for each test run is made bold.

Position RMSE Heading RMSETest Mean SD Min Max Mean SD Min Max

mm mm mm mm deg deg deg degS1H241 135.1 0.0 135.1 135.1 0.43 0.0 0.43 0.43S1G121 45.5 4.5 41.1 50.2 0.19 0.002 0.19 0.19S1G141 34.6 0.9 34.0 35.7 0.19 0.005 0.19 0.20S1G521 132.7 21.7 107.7 146.7 0.46 0.066 0.39 0.51S1G541 123.1 14.3 108.3 136.9 0.42 0.026 0.40 0.45S2H241 71.0 0.0 71.0 71.0 0.33 0.0 0.33 0.33S2G121 36.1 1.1 35.0 37.2 0.24 0.006 0.24 0.25S2G141 35.4 5.6 29.1 39.8 0.23 0.002 0.23 0.24S2G521 77.7 4.6 72.7 81.6 0.36 0.010 0.35 0.37S2G541 78.7 14.4 69.4 95.2 0.37 0.026 0.34 0.39S3H241 140.8 0.0 140.8 140.8 0.36 0.0 0.36 0.36S3G121 64.0 36.7 34.6 105.2 0.28 0.067 0.23 0.36S3G141 25.8 0.4 25.5 26.2 0.21 0.006 0.21 0.22S3G521 139.8 9.2 132.0 149.9 0.41 0.024 0.39 0.43S3G541 138.0 9.0 128.7 146.7 0.40 0.022 0.38 0.42X1H241 67.2 0.0 67.2 67.2 0.15 0.00 0.15 0.15X1G121 54.2 10.2 46.7 65.9 0.15 0.022 0.13 0.17X1G141 54.4 6.4 47.1 59.2 0.16 0.017 0.14 0.18X1G521 37.2 1.3 36.3 38.7 0.17 0.008 0.16 0.18X1G541 33.8 1.7 32.5 35.7 0.16 0.003 0.16 0.16X2H241 24.7 0.0 24.7 24.7 0.10 0.0 0.10 0.10X2G121 51.4 7.9 42.6 58.0 0.19 0.007 0.18 0.20X2G141 42.4 5.2 36.6 46.7 0.17 0.004 0.17 0.17X2G521 18.5 2.1 16.1 20.2 0.14 0.002 0.14 0.14X2G541 16.6 2.0 14.4 18.2 0.14 0.001 0.14 0.14

Average, standard deviation and minimum and maximum RMSE errors for trans-lation and rotation for the di�erent parameter combinations are listed in Table 5.Test run wise results can be found in Appendix B. Again as with Hector, the slower

33

movement X330 trajectories show vastly smaller errors over the faster S120 tra-jectories. Out of the parameters, the e�ect of sigma value on trajectories is theclearest; the larger value markedly increasing the RMSE values with larger map cellsizes. Smoothing makes �nding the correct transformation for scan matching with-out getting stuck in local optima easier but, as a drawback, it decreases achievableaccuracy. When GMapping matches scans to the map, larger cell size already makesthe response smoother and doing too much further smoothing is clearly detrimentalto the SLAM result. Di�erent map resolutions require di�erent sigma values.

1.43 1.44 1.45 1.46 1.47 1.48 1.49 1.5 1.51

-32.32

-32.31

-32.3

-32.29

-32.28

-32.27

x [m]

y [

m]

Figure 16: The e�ect of staying stationary for GMapping. The position starts tooscillate with large magnitude when compared to the other errors in the trajectory.The crosses connected by lines represent poses from GMapping while the othercrosses are from the dense initial trajectory.

Visual inspection of the trajectories reveals that GMapping does not handleupdates well while staying stationary with the higher resolution settings. With thehigher resolution settings the relatively large scan match optimization step size couldcause the trajectory to become noisy and when the position is not changing the lo-cation starts to oscillate with growing amplitude. The largest errors in trajectoriesare caused by this behaviour. This can be seen in Figure 16. The e�ect of thisbehaviour on trajectory error is further ampli�ed during post processing when thedense Hector trajectory is combined with the sparse one from GMapping. Trans-forming the subtrajectories to �t the gaps between random and large movementswhen there is almost no movement causes the points to spread widely around thepose where scanner was actually stopped. This is a problem mostly for the X330trajectories as there were fewer stops and they were shorter on the S120 trajectories.The longer the scanner stays stationary the larger the oscillation becomes.

In S test runs higher map resolution leads to lower errors while in the X test runsthe opposite is true. A major reason for this is the smoother trajectory with 5 cm

34

cell size as seen in Figure 17. While the XG5 trajectories still achieve smaller errorsthan XG1 trajectories, the di�erence is less pronounced than could be inferred fromthe RMSE values as they are polluted by position oscillating. More sigma valuesshould be tried to �nd the one �tting with each map resolution level to get a betteridea on map resolution e�ect on trajectory quality. In principle higher resolutionshould provide better performance as there is more information available for scanmatching.

(a) (b)

Figure 17: Left is from test run X1G121 with 1 cm resolution and right fromX1G521 with 5 cm resolution. The 5 cm cell size reduces the random errors whilenot moving which causes the X1G521 to have smaller RMSE values while the actualerrors in trajectory are not so di�erent.

The lack of smoothness in the trajectory is evident when looking at error timeseries in Figure 17. With 1 cm resolution the error oscillates constantly in thecentimetre range and while the trajectory with 5 cm resolution is smoother thereare many large jumps. When compared to Hector SLAM results the GMappingheading estimate is also more noisy (Figure 17 vs. Figure 14). This is causedby the combined e�ect of interpolating the heading values for the subtrajectoriesbetween GMapping poses and from the lack of smoothness in the overall GMappingtrajectory.

The results in Table 6 reveal that larger scan matcher optimization step sizesincreases the accuracy of trajectories. While both values are large enough to accountfor the errors in the initial guess from Hector, the scan matching results are also usedto calculate a model from which the next set of particles are drawn and sampling alarger area improves the performance. Smaller optimization step sizes should alsobe tested with the higher map resolution as they could eliminate the oscillation inthe trajectories.

In general GMapping with its multiple map hypothesis seems to be able toimprove Hector results but unfortunately at least with current parameters the tra-jectory is far from smooth which is a serious problem whe it is used to produce a

35

3D point cloud of a building.

6.3.3 Karto

Karto with its explicit loop closure functionality seemed like the most promisingapproach and to get a better idea of its performance it was tested with a total of48 setting combinations. Two scan matcher resolutions in the higher end of usableresolutions, 5 mm and 2.5 mm and four (0.1, 0.3, 0.6 and 0.9) values for a multiplierwhich penalizes scan matches di�ering from the initial guess given by odometry.These parameters were selected to gain some insight of the quality of Karto's scanmatcher. Giving a larger value to the parameter which penalizes transformationsdi�ering from the initial guess should force it to follow the initial trajectory moreclosely. Three loop closure minimum response values, 0.8, 0.7 and 0.6 for coarsematching and correspondingly 0.7, 0.6 and 0.5 for �ne matching were tested. Boththe default loop closure method and the delayed one were tested.

For test runs X1, X2 and S2 the parameters were additionally evaluated withinitial Hector trajectory calculated without rotation compensation (only with de-layed loop closing). As the only advantage of the rotation compensation schemeswas found to be reduced jitter in the heading value (Section 3.1) which is currentlyignored as the heading values between Karto matches are interpolated, no attemptwas made to analyse the e�ect of initial trajectory rotation compensation on Karto.These trajectories were used as additional samples when testing di�erent parametercombinations.

Hector trajectories with parameter combinations H241 and H240 were smoothedand used as a laser based odometry for the algorithm. Results of the test runscan be seen through Tables 7 to 9. Full results for every parameter combinationand test run can be found in Appendix C. Again the XK datasets give vastly lowererrors when compared to the SK datasets for same reasons as explained in Section3.1. The e�ect of scan matcher resolution can be seen in Tables 7 and 8 wheremean RMSE errors have been calculated for both scan resolutions. In SK sets the0.0025 resolution is clearly better but the situation is more even in the XK sets.Closer inspection of the test runs X1 and X2 reveal that also in the X2 test runthe 0.0025 resolution gives better results and that it is only the X1 test run wherecoarser resolution gives better scores (and with a wide margin). In X1 all the 6parameter combination which led to worse RMSE value than the initial trajectorywere with 0.0025 resolution while in the other test runs the worst RMSE values weremostly with the 0.005 resolution. The better performance with higher resolution wasexpected as higher resolution enables the scan matcher to �nd matches closer to thetruth and this should improve the trajectory. The reason for this not applying forX1 trajectory is mystery.

The in�uence of scan matching distance penalty multiplier and loop closure re-sponse threshold on the RMSE value was analysed by calculating correlation coef-�cients between the parameters and the RMSE value. No correlation was found forthe penalty multiplier and only a weak one for the response threshold. In delayedloop closing datasets there was a weak positive correlation of 0.099 and with normal

36

Table 7: ATE metrics and the number of loop closures for the test runs with Karto.The RMSE values are calculated from 7 datasets for S and 6 datasets for the Xtests. The �rst column denotes the test run and parameter combination. The �rstcharacter marks the test run type, S for Faro S120 and X for X330, K stands forKarto and the last three digits indicate the parameter values used. The �rst numberdenotes the coarse loop closure minimum response value. The second tells the scanmatch resolution, 5 for 5 mm and 2 for 2.5 mm and the last one marks the scanmatching penalty multiplier.

Position RMSE Heading RMSE LoopsTest Mean SD Min Max Mean SD Min Max

mm mm mm mm deg deg deg deg NSK621 49.1 19.9 15.6 78.2 0.26 0.05 0.20 0.32 14.9SK721 64.1 30.5 18.4 117.5 0.29 0.07 0.21 0.41 10.9SK821 56.7 49.9 17.6 164.3 0.30 0.11 0.20 0.53 6.6SK623 47.7 28.3 17.5 99.9 0.28 0.04 0.21 0.35 15.3SK723 67.9 51.3 22.3 166.2 0.32 0.12 0.21 0.57 11.6SK823 40.3 20.2 16.9 79.4 0.28 0.04 0.19 0.31 7.1SK626 49.0 32.0 16.0 111.9 0.28 0.05 0.22 0.37 15.3SK726 51.6 38.9 28.6 138.1 0.28 0.08 0.22 0.46 11.3SK826 66.4 19.8 37.8 88.5 0.31 0.04 0.25 0.36 6.4SK629 44.0 20.1 25.8 71.6 0.27 0.05 0.22 0.33 15.6SK729 44.7 17.5 18.8 60.7 0.27 0.03 0.23 0.30 10.9SK829 57.5 22.1 40.9 98.7 0.30 0.05 0.23 0.39 6.4Mean 53.2 29.2 23.0 106.3 0.29 0.06 0.22 0.39 11.0SK651 58.3 41.2 29.8 123.6 0.26 0.06 0.21 0.38 14.6SK751 66.5 23.8 36.0 103.3 0.28 0.05 0.22 0.35 10.4SK851 68.3 29.4 34.8 112.4 0.27 0.05 0.20 0.35 7.1SK653 76.3 42.3 31.3 143.5 0.31 0.07 0.22 0.43 15.6SK753 101.1 45.9 49.2 142.8 0.34 0.07 0.25 0.41 12.7SK853 611.3 1340.7 68.1 3650.6 1.59 3.37 0.27 9.23 5.6SK656 121.5 96.2 34.0 255.1 0.57 0.49 0.25 1.28 16.1SK756 133.8 93.2 21.9 269.3 0.61 0.50 0.25 1.40 13.1SK856 144.3 85.6 51.0 254.5 0.62 0.46 0.25 1.28 8.4SK659 70.2 28.0 36.7 116.7 0.28 0.05 0.19 0.36 14.7SK759 71.8 40.6 21.3 119.7 0.29 0.06 0.18 0.35 11.6SK859 83.5 46.8 20.0 158.4 0.31 0.07 0.21 0.42 6.4Mean 133.9 159.5 36.2 454.2 0.48 0.44 0.23 1.35 11.4

loop closing behaviour the correlation was weakly negative ,-0.1. While weak theydo make sense as the delayed loop closure would bene�t from lower threshold asmore (still good) loop closures could be made and for the normal loop closing ifloops can be found, higher threshold ensures good quality loop closures. The e�ect

37

Table 8: ATE metrics and the number of loop closures for the test runs with Karto.The RMSE values are calculated from 7 datasets for S and 6 datasets for the Xtests. The �rst column denotes the test run and parameter combination. The �rstcharacter marks the test run type, S for Faro S120 and X for X330, K stands forKarto and the last three digits indicate the parameter values used. The �rst numberdenotes the coarse loop closure minimum response value. The second tells the scanmatch resolution, 5 for 5 mm and 2 for 2.5 mm and the last one marks the scanmatching penalty multiplier.

Position RMSE Heading RMSE LoopsTest Mean SD Min Max Mean SD Min Max

mm mm mm mm deg deg deg deg NXK621 28.1 21.5 12.3 69.5 0.14 0.04 0.11 0.23 16.3XK721 21.3 7.2 15.8 35.3 0.13 0.03 0.10 0.18 15.3XK821 27.5 8.5 12.8 35.7 0.15 0.03 0.12 0.20 9.7XK623 21.0 7.3 15.2 35.1 0.13 0.02 0.11 0.16 16.8XK723 24.1 11.4 15.0 44.8 0.13 0.02 0.12 0.18 14.8XK823 35.9 17.6 15.4 64.5 0.15 0.04 0.12 0.22 9.7XK626 34.2 25.9 13.6 71.6 0.16 0.07 0.12 0.28 16.7XK726 31.1 17.7 19.3 66.0 0.15 0.05 0.11 0.24 14.3XK826 29.6 9.1 20.5 43.2 0.15 0.02 0.12 0.19 9.2XK629 28.8 10.1 20.6 47.9 0.14 0.02 0.11 0.17 16.8XK729 34.9 14.5 19.0 60.5 0.15 0.03 0.13 0.22 14.3XK829 26.8 10.0 12.1 41.7 0.14 0.02 0.12 0.18 9.5Mean 28.6 13.4 16.0 51.3 0.14 0.03 0.12 0.20 13.6XK651 28.5 8.9 16.8 42.3 0.13 0.02 0.11 0.16 16.2XK751 24.5 7.9 14.8 35.2 0.13 0.02 0.11 0.15 14.8XK851 20.1 4.8 13.6 26.0 0.13 0.02 0.11 0.16 8.2XK653 32.6 6.3 25.5 43.0 0.13 0.02 0.11 0.15 16.7XK753 26.2 10.7 15.1 42.9 0.13 0.03 0.11 0.19 14.7XK853 22.1 4.6 16.6 28.1 0.13 0.02 0.11 0.16 7.7XK656 26.4 15.4 10.7 54.9 0.14 0.04 0.11 0.22 17.2XK756 27.5 7.6 16.5 40.4 0.13 0.03 0.11 0.19 14.2XK856 27.1 4.2 23.5 34.0 0.14 0.03 0.11 0.19 7.8XK659 27.2 7.9 19.5 40.2 0.14 0.03 0.11 0.18 16.8XK759 31.3 13.3 15.9 49.3 0.14 0.04 0.10 0.22 13.3XK859 20.2 2.6 17.3 23.6 0.13 0.03 0.11 0.17 7.8Mean 26.1 7.8 17.1 38.3 0.13 0.03 0.11 0.18 12.9

of matching penalty multiplier were found to be negligible, the use of intermediatevalues from 0.3 to 0.6 seemed to lead to some increase in error but as a whole theydo not seem like very important parameters.

The performance of the two loop closing strategies were compared on each test

38

run and the results are listed in Table 9. Two cases for the both closing strategies arelisted, one with the preferred response threshold (0.6 for delayed, 0.8 for normal),and one averaged over all response threshold values. The resolution used was 0.0025as it gives better results on average and eliminates many parameter combinationswhich resulted in worse solution than initial trajectory.

Selecting the response threshold accordingly gives improvement in a majorityof cases. The e�ect is especially clear for the delayed loop closure where using thelower threshold gives smaller average and smallest minimum error on all test runs.With suitable response thresholds the delayed loop closing gives better results thanthe normal loop closing on 4 out of 5 test runs and while the improvement gain isnot very large or clear in the S sets the result still imply that delayed loop closingis the preferred method to be used.

(a) (b)

(c)

.(d)

Figure 18: Errors from test runs S2K6211 and X2K6211 and corresponding initialHector trajectories S2H241 and X2H241 as time series.

With the higher In all of the test runs Karto provides clear improvement to the

39

Table 9: ATE metrics of the di�erent loop closing strategies. The �rst columndenotes the test run and parameter combination. The �rst character marks thetest run type, S for Faro S120 and X for X330, K stands for Karto and the lastthree digits indicate the parameter values used. Accuracy of Delayed loop closingis compared to the normal one. RMSE values are calculated from 8 datasets forKD62, 4 for KO82, 24 for KD2 and 12 for KO2. Last number tells the scan matchresolution, 5 for 5mm and 2 for 2.5mm. The parameter combination with lowesterror for each test run is made bold.

Position RMSE Heading RMSETest Mean SD Min Max Mean SD Min Max

mm mm mm mm deg deg deg degS1H241 135.1 0.0 135.1 135.1 0.43 0.0 0.43 0.43S1KD62 46.5 13.6 27.6 59.01 0.26 0.03 0.22 0.29S1KO82 31.8 10.6 16.9 40.90 0.22 0.03 0.19 0.25S1KD2 69.8 40.4 27.6 166.21 0.31 0.10 0.22 0.57S1KO2 59.3 36.7 16.9 148.37 0.28 0.07 0.19 0.43S2H241 71.0 0.0 71.0 71.0 0.33 0.0 0.33 0.33S2KD62 47.1 17.1 31.8 71.59 0.29 0.03 0.27 0.33S2KO82 50.5 26.5 26.9 88.49 0.31 0.04 0.28 0.36S2KD2 69.2 38.5 31.8 164.35 0.33 0.07 0.27 0.53S2KO2 59.5 23.4 26.9 109.95 0.31 0.03 0.27 0.36S3H241 140.8 0.0 140.8 140.8 0.36 0.0 0.36 0.36S3KD62 46.4 36.9 15.6 99.94 0.27 0.06 0.20 0.35S3KO82 50.0 22.1 26.1 77.39 0.28 0.03 0.25 0.32S3KD2 61.3 47.3 15.6 205.45 0.28 0.05 0.20 0.36S3KO2 49.0 48.9 16.0 205.45 0.26 0.05 0.21 0.36X1H241 67.2 0.0 67.2 67.2 0.15 0.00 0.15 0.15X1KD62 20.5 4.1 16.8 26.29 0.12 0.01 0.11 0.12X1KO82 42.6 15.9 27.7 64.45 0.17 0.03 0.14 0.22X1KD2 29.5 9.1 16.8 44.35 0.14 0.03 0.11 0.20X1KO2 43.8 18.4 16.7 69.50 0.17 0.04 0.10 0.24X2H241 24.7 0.0 24.7 24.7 0.10 0.0 0.10 0.10X2KD62 16.1 3.7 12.3 21.19 0.12 0.00 0.12 0.12X2KO82 21.7 7.0 12.1 28.07 0.13 0.01 0.12 0.14X2KD2 20.6 6.6 12.3 32.11 0.12 0.01 0.10 0.14X2KO2 22.4 8.6 12.1 47.92 0.13 0.01 0.10 0.17

trajectories over Hector with the delayed loop closing with low loop closure thresholdproviding the best results in 4 out of 5 test runs as seen in Table 9. While there isvariation in the results and sometimes Karto fails giving out a worse trajectory, inalmost every case there is signi�cant improvement in the trajectory.

40

6.4 Discussion on used algorithms

No single SLAM algorithm was able to provide a trajectory �lling the requirementsfor point cloud creation of being dense, smooth and globally consistent. HectorSLAM �lls the �rst two requirements but it has no functionality to reduce theerrors in trajectory caused by accumulating drift except for discrete jumps. On theother hand GMapping and Karto both have problems if the displacement betweenscans is too small and can not provide reliable trajectory if they need to processevery subsequent scan.

Combining the dense Hector SLAM trajectory with GMapping or Karto providesa solution which can produce a trajectory meeting the requirements. By using thetrajectory from Hector as the initial guess, the Karto library is working as a kind ofad-hoc backend for the Hector SLAM. This overcomes the problems caused by thelack of backend in Hector while enabling Karto to bene�t from Hector's high qualityscan matcher. While not containing an optimizing backend, the multiple hypothesisof GMapping provides similar capability.

The large errors caused by the localization diverging with GMapping when notmoving made the RMSE values less valuable for estimating the actual trajectory ac-curacy. The inspection of error time series and trajectories made it clear that Kartoprovided trajectories which were more coherent and closer to the reference. Thisinspection also revealed that with Karto the smoothness of the Hector trajectory isbetter preserved than with GMapping. Of course it must be taken in to accountthat more time was spent �nding good parameters for Karto and Hector than forGMapping. With better parameter values the jitter in the GMapping trajectorymight disappear altogether.

GMapping processing also contains a random element in both the resample andthe motion model sampling steps, which adds an additional source of uncertainty tothe process. Getting a di�erent trajectory every time the same dataset is processedis not desirable but the di�erences are small and total failure is unlikely. It shouldbe noted that Karto too fails occasionally, either through the error metric of opti-mization becoming Not A Number (NaN) or with loop closures associating wrongscans together. This can result in a worse trajectory than the initial trajectory givenby Hector SLAM without the trajectory being noticeable wrong.

An attempt was also made to process the data again with Hector SLAM. Thesmoothed trajectory from the �rst run was used to transform the scans to compen-sate for the distortions caused by movement. Unfortunately this just resulted inlarger errors to the trajectory. One likely cause for this behaviour is the small errorsin the initial trajectory, which get magni�ed. If the movement estimate used forlaser scan transformation is incorrect, the scan can get even more distorted whichresults in even larger error in the scan matching. The smoothing used was quite con-servative in changing the trajectory and only removed large jumps. More aggressivesmoothing might provide better performance.

For Karto one obvious source of improvement would be the increasing of scanmatching resolution, but unfortunately it makes the algorithm unstable. With thehighest setting used so far (2.5 mm) the scan matching occasionally fails with rota-

41

tional component becoming NaN, which causes the algorithm to crash. Setting theresolution to 2 mm made this happen noticeably more often and values less than 2mm caused Karto to crash always. The algorithm should be studied in detail to seeif the source of the crashes could be identi�ed and �xed. This

The developed delayed loop closure behaviour should also be studied further.The results in Section 3.3 show that, while in general delaying the closing of a loopgives improved performance, with some settings it actually worsens the situation.In theory the same loops are closed with the both methods the only di�erence beingthat with delayed closing the matches are better. The reason why this still givesworse trajectories should be investigated.

Signi�cant accuracy gains could also be achieved by improving the combining ofdense and sparse trajectories. An obvious �rst step would be the use of real headingvalues for the subtrajectories instead of the interpolated ones. Another error sourceis that when a subtrajectory between two optimized poses is transformed to �tbetween the endpoints, it is scaled by the same amount in both X and Y directions.This scaling can magnify small random jitter in the localization to centimetre rangewhen the scanner is almost stationary. This e�ect could be reduced by scaling onlyalong the line between the endpoints.

6.5 Evaluation of point cloud

X1KD621 and X2KD621 trajectories processed by Karto and X2H241 trajectoryprocessed by Hector SLAM were selected for creating point clouds from data scannerscans and for the evaluation of the full pipeline (an smoothed version of the X2H241trajectory was used as initial trajectory for X2KD621). The selected trajectorieswere among the most accurate ones with least error. The trajectory error metrics canbe seen in Table 10. GMapping trajectories were not used due to lack of smoothness.The computed trajectories were combined with the data scanner to generate 3Dpoint clouds of the FGI study area. The point cloud data was scanned with 30 Hzscan frequency and 244,000 points per second with maximum range of 6.0 m. Thepoint clouds were validated for geometric accuracy against the TLS reference pointcloud also used for the SLAM trajectory evaluation. An example of a point clouddata collected in X2KD621 is shown in Figure 19. The point cloud in X1 includes31 million points, while the X2 cloud has about 40 million points in total.

The geometric error of the point clouds was analysed on seven pylons in thestudy area. Some of them are visible in the �gure 20. The centre of the pylonwas measured from the TLS and Slammer point cloud interactively. The resultingerrors are seen in Table 11. The maximum errors for the both cases were found inthe lower corridor. The error grows steadily starting from the lobby as could beexpected based on the trajectory errors.

To get the scale of the error over the whole building the lengths of the maincorridors were measured. From the TLS cloud the upper corridor length was deter-mined to be 43.498 m, X1KD621 shows 43.492 m, and X2KD621 43.501 m lengthcorrespondingly. The lower corridor length from the TLS cloud was 44.039 m, whileit was found to be 44.056 m for the X1KD621 data, and 44.068 m for the X2KD621.

42

Table 10: ATE errors for the selected trajectories and initial trajectories used forKarto.

Position error Heading errorTest RMSE SD Max RMSE SD Max

mm mm mm deg deg degX1HS241 44.3 24.3 113.7 0.15 0.04 1.21X1KD6211 16.8 12.3 63.1 0.11 0.06 2.09X2H241 24.7 17.5 81.5 0.10 0.07 1.62X2HS241 25.9 13.3 64.0 0.10 0.04 1.62X2KD6211 12.3 7.7 35.4 0.12 0.07 1.41

Figure 19: 3D point cloud of the whole �oor collected with the Slammer system fromtest X2KD621. Point coloring is based on the laser intensity and data �le numberto illustrate the progress of the data acquisition.

Table 11: Planar errors in the point cloud measured on 7 pylons. N is the numberof locations compared.

X1KD621 X2KD621 X2H241mm mm mm

RMSE 13.1 12.8 16.2SD 7.3 12.3 9.8Minimum 3.6 0.1 1.7Maximum 31.4 47.0 28.7N 18 19 19

The errors are of an order of magnitude of 0.01-0.1% of the total length which isextremely small. However, the horizontal location error for the data point cloudvaries from place to place. This variation is summarized in Table 11.

43

Figure 20: 3D point cloud of the library area from test run X2H241. The precisionand accuracy of the data is well represented, regardless of the complexity of the scene.Point colouring is based on the laser intensity and data �le number to illustrate theprogress of the data acquisition.

Figure 21: Details of the building wall from X1KD621, doors and interior featurescaptured by Slammer. Points are colored by laser intensity.

The X2H241 trajectory had an accuracy comparable to that of Karto trajectoriesand the point cloud produced showed no great errors and actually had the smallestmaximum error as seen in Table 11. This does not mean that Hector trajectoriesin general are suitable for generating point clouds as X2H241 is exceptional among

44

the Hector trajectories with its small deviation from the reference and lack of largejumps.

0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1 1.2

-1.6

-1.5

-1.4

-1.3

-1.2

-1.1

-1

-0.9

-0.8

x [m]

y [

m]

(a) (b)

Figure 22: The e�ect of returning to previously mapped area after error in the po-sition has accumulated for extensive time period. The left side shows the trajectory(red is the reference) and the right side the resulting point cloud of test run X1H241.

In the X2 test run the library was explored by progressively moving to the closestunexplored corridor starting from the upper corridor. This suits well with HectorSLAM as it minimizes the lengths of stints exploring new areas. In the X1 test runthe library was explored in an opposite way by starting with long loop around thefurthest corridors. The e�ect can be seen in trajectory X1H241, part of which isshown in Figure 22. Just before entering the area shown in the Figure, the positionhad jumped to one matching the area mapped in the beginning but the map used bythe algorithm still contains parts mapped before the jump and the position startsoscillating between the two competing map representations. This manifests itself asghost walls in the produced point cloud.

45

7 Conclusions and Future Work

In this study we investigated the use and e�ciency of high-end laser scanners ando�ine processing with SLAM algorithms to compute trajectories of a MLS platformand used them to generate 3D point clouds indoors in the absence of GNSS signals.The trajectory estimation was initially carried out using Hector SLAM with itshigh quality scan matcher to provide an initial trajectory for Karto and GMappingalgorithms. While Hector alone can provide good quality trajectories and GMappingcan somewhat improve them, Karto with its explicit loop closure behaviour and abackend for optimizing poses to reduce cumulating scan matching errors turned outto be a superior choice.

For investigating the use of Slammer and combination of di�erent SLAM algo-rithms, �ve di�erent trajectories were collected and processed using di�erent pa-rameter sets. The resulting trajectories were evaluated against a TLS reference.The results indicate that the scanner version used for SLAM might have a role inthe performance of the algorithms, but most probably the di�erences are mostlydue to increased scan frequency and slower data acquisition. The analysis of the3D point cloud generated from the secondary scanner data based on the obtainedSLAM trajectories shows good agreement with the TLS reference. The best pointcloud generated showed an accuracy of 13 mm mean and 30 mm maximum error forhorizontal position. The dimensions of the building show under 0.1% error.

The wide variability on the trajectory errors revealed by di�erent parameterand test run combinations highlighted the need to be diligent when benchmarkingSLAM algorithms. A large variety of latent variables and processes work togetherand a�ect the results from SLAM algorithms in seemingly random ways to makingthe process in e�ect to a stochastic one. Without a sample size large enough theresults obtained are more or less useless for creating any general conclusions. Forexample, if only one test run with many parameter sets or one set of parameterswith many test runs were used, the conclusions on performance of di�erent SLAMalgorithms, parameter choices or data collection practises might have been totallydi�erent. One example from the current work is the usefulness of using IMU tocompensate for scanner rotation during movement. It was �rst evaluated using onlysome of the test runs with H24 parameters. In those runs the compensation schemeseemed to provide a noticeable improvement to Hector trajectories requiring onlyrelatively simple modi�cations to the algorithm. Actually this improvement didn'tmaterialize with the majority of other parameter combinations.

While the point cloud quality with 4.7 mm RMSE achievable by TLS scanningmight not be reached, the centimetre range localization accuracy provided by thecombination of Karto and Hector SLAM is enough to produce coherent and visuallypleasing point clouds with vastly faster data collection. The layout of the buildingmapped with long dead-end corridors and many glass surfaces in crucial places isalso especially di�cult for SLAM. Tests in other buildings might compare morefavourably with TLS.

Strict global accuracy of the model in relation to reality is important for casessuch as building quality monitoring, but for many other uses small errors in the

46

position and orientations of walls and other structures can be tolerated as long asthey are locally consistent and there are no ghosting objects. If used, for example,to generate a virtual world, the direction of a dead end corridor being o� by evena few degrees is almost impossible to notice without extensive and close inspection.It is su�cient if the resulting 3D point cloud "looks right" for a human eye.

These results are also only preliminary and can be expected to be improved byfurther modi�cations to the SLAM algorithms and by integration of the IMU mea-surements more robustly to compensate for movement during scan and to constraintscan matching. Processing the scans in inverted order could also be attempted, bymerging the result with results of normal processing some improvements could bemade.

Other SLAM algorithms could also be tested, for example, Stoyanov et al. [32]have created SLAM algorithm based on Normal Distributions Transform which isavailable also as a ROS package. Unfortunately their approach does not containexplicit loop closure handling and they recognized it as a problem. Without loopclosure handling mechanism it likely can not match the excellent performance ofKarto in larger environments. Other SLAM algorithms with loop closure handlingmechanism and optimizing backend should be searched for.

The huge impact of movement speed on trajectory accuracy also points to theimportance of data collection practises; improvements in that area could furtherincrease the accuracy. For example would slower movement provide even higheraccuracy, could moving back and forth between the starting areas of a corridorsproduce a more robust estimate of their relative orientation, or during movementin long corridors, could orienting the platform in a way which would enable thelaser scanner to view the both ends and one full wall continuously lead to improvedresults. Mounting a second, cheaper laser scanner horizontally should also be triedto assess, how much of the good performance is due to the expensive laser scannerand what is the contribution of the SLAM algorithms.

The SPAN system can occasionally (when passing a window) acquire coordinatesfrom satellites and these could be used in the future to tie the measurements toa global coordinate system. This could also be realized by pushing the platformoutdoors when possible.

It is also possible to add synchronous RGB, infrared or hyperspectral camerasto the platform using the modular sensor suite of Slammer. One future option tobe explored is to set the data collection laser scanner to TLS mode where it rotatescontinually around its mount. This would reduce occlusions even further but asa down side the vehicle would block part of the measurements and the e�ectivemeasurement rate would be reduced. The use of a wheeled cart also limits the useto areas with �at �oors. A backpack based measurement platform would provideimproved mobility and is one direction for future research. In backpack the platformwould not be any longer tied to the �oor and the third dimension would had to betaken into account, which would greatly complicate the SLAM problem.

47

References

[1] Moiz Chasmai, Arun Barde, Gagandeep Purohit, and Anjaneya Sharma. Ac-curacy enhancement techniques for global navigation satellite systems and itsmilitary ground based navigation applications. International Journal, 3(1),2014.

[2] Gérard Lachapelle. Gnss indoor location technologies. Positioning, 1(08), 2004.

[3] Rainer Mautz. Overview of current indoor positioning systems. Geodezija irkartogra�ja, 35(1):18�22, 2009.

[4] Hongbo Liu, Yu Gan, Jie Yang, Simon Sidhom, Yan Wang, Yingying Chen,and Fan Ye. Push the limit of wi� based localization for smartphones. InProceedings of the 18th annual international conference on Mobile computingand networking, pages 305�316. ACM, 2012.

[5] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics(Intelligent Robotics and Autonomous Agents). The MIT Press, 2005. ISBN0262201623.

[6] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard. Improved techniquesfor grid mapping with rao-blackwellized particle �lters. Robotics, IEEE Trans-actions on, 23(1):34�46, 2007.

[7] Stefan Kohlbrecher, Johannes Meyer, Oskar von Stryk, and Uwe Klingauf. A�exible and scalable slam system with full 3d motion estimation. In Proc. IEEEInternational Symposium on Safety, Security and Rescue Robotics (SSRR).IEEE, November 2011.

[8] Daniel Lau. The Science Behind Kinects or Kinect 1.0 versus 2.0,2013. http://www.gamasutra.com/blogs/DanielLau/20131127/205820/

The_Science_Behind_Kinects_or_Kinect_10_versus_20.php. Accessed16.5.2015.

[9] Velodyne. Velodyne HDL-64E, 2014. http://www.velodynelidar.com/

lidar/hdlproducts/hdl64e.aspx. Accessed 16.5.2015.

[10] George. Vosselman and Hans-Gerd Maas. Airborne and Terrestrial Laser Scan-ning. Whittles Publishing, 2010. ISBN 9781904445876.

[11] Martial Hebert and Eric Krotkov. 3-d measurements from imaging laser radars:How good are they? In Intelligent Robots and Systems' 91.'Intelligence forMechanical Systems, Proceedings IROS'91. IEEE/RSJ International Workshopon, pages 359�364. IEEE, 1991.

[12] AD King. Inertial navigation-forty years of evolution. GEC review, 13(3):140�149, 1998.

48

[13] Oliver J Woodman. An introduction to inertial navigation. University of Cam-bridge, Computer Laboratory, Tech. Rep. UCAMCL-TR-696, 14:15, 2007.

[14] Kurt Konolige, Giorgio Grisetti, Rainer Kummerle, Wolfram Burgard, BensonLimketkai, and Regis Vincent. E�cient sparse pose adjustment for 2d map-ping. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ InternationalConference on, pages 22�29. IEEE, 2010.

[15] Joao Machado Santos, David Portugal, and Rui P Rocha. An evaluation of 2dslam techniques available in robot operating system. In Safety, Security, andRescue Robotics (SSRR), 2013 IEEE International Symposium on, pages 1�6.IEEE, 2013.

[16] Sanjeev Arulampalam, Simon Maskell, Neil Gordon, and Tim Clapp. A tutorialon particle �lters for online nonlinear/non-gaussian bayesian tracking. SignalProcessing, IEEE Transactions on, 50(2):174�188, 2002.

[17] Kevin Murphy. Bayesian map learning in dynamic environments.

[18] SRI International. Karto, 2015. http://www.kartorobotics.com/. Accessed16.5.2015.

[19] Edwin Olson. Real-time correlative scan matching. In Robotics and Automation,2009. ICRA'09. IEEE International Conference on, pages 4387�4393. IEEE,2009.

[20] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, and Wolfram Burgard.A tutorial on graph-based slam. Intelligent Transportation Systems Magazine,IEEE, 2(4):31�43, 2010.

[21] Regis Vincent, Benson Limketkai, and Michael Eriksen. Comparison of indoorrobot localization techniques in the absence of gps. In SPIE Defense, Secu-rity, and Sensing, pages 76641Z�76641Z. International Society for Optics andPhotonics, 2010.

[22] Niko Sunderhauf and Peter Protzel. Switchable constraints for robust posegraph slam. In Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ Inter-national Conference on, pages 1879�1884. IEEE, 2012.

[23] Xinlian Liang, Juha Hyyppa, Antero Kukko, Harri Kaartinen, AnttoniJaakkola, and Xiaowei Yu. The use of a mobile laser scanning system formapping large forest plots. Geoscience and Remote Sensing Letters, IEEE, 11(9):1504�1508, 2014.

[24] Xinlian Liang, Juha Hyyppa, Antero Kukko, Harri Kaartinen, AnttoniJaakkola, and Xiaowei Yu. The use of a mobile laser scanning system formapping large forest plots. Geoscience and Remote Sensing Letters, IEEE, 11(9):1504�1508, 2014.

49

[25] Morgan Quigley, Ken Conley, Brian Gerkey, Josh Faust, Tully Foote, JeremyLeibs, Rob Wheeler, and Andrew Ng. Ros: an open-source robot operatingsystem. In ICRA Workshop on Open Source Software, 2009.

[26] Tully Foote. tf: The transform library. In Technologies for Practical RobotApplications (TePRA), 2013 IEEE International Conference on, Open-SourceSoftware workshop, pages 1�6, April 2013. doi: 10.1109/TePRA.2013.6556373.

[27] ROS. Robotic Operating System, 2015. http://www.ros.org. Accessed16.5.2015.

[28] Burcin Becerik-Gerber, Farrokh Jazizadeh, Geo�rey Kavulya, and GulbenCalis. Assessment of target types and layouts in 3d laser scanning for reg-istration accuracy. Automation in Construction, 20(5):649�658, 2011.

[29] Martin Fischler and Robert Bolles. Random sample consensus: a paradigm formodel �tting with applications to image analysis and automated cartography.Communications of the ACM, 24(6):381�395, 1981.

[30] Radu Bogdan Rusu and Steve Cousins. 3D is here: Point Cloud Library (PCL).In IEEE International Conference on Robotics and Automation (ICRA), Shang-hai, China, May 9-13 2011.

[31] Andrea Bonarini, Wolfram Burgard, Giulio Fontana, Matteo Matteucci,Domenico Giorgio Sorrenti, and Juan Domingo Tardos. Rawseeds: Roboticsadvancement through web-publishing of sensorial and elaborated extensive datasets. In In proceedings of IROS'06 Workshop on Benchmarks in Robotics Re-search, 2006.

[32] Todor Stoyanov, Jari Saarinen, Henrik Andreasson, and Achim Lilienthal. Nor-mal distributions transform occupancy map fusion: Simultaneous mapping andtracking in large scale dynamic environments. In Intelligent Robots and Sys-tems (IROS), 2013 IEEE/RSJ International Conference on, pages 4702�4708.IEEE, 2013.

50

A Hector errors

Table A1: ATE errors for the test runs with Hector SLAM. The �rst two charactersmark the test run, H stands for Hector SLAM and the last three digits code forparameter values used. The �rst one states the occupancy grid resolution in cen-timetres, the second one tells the number of levels in the occupancy grid pyramidand the last one tells whether rotation compensation was used (1) or not (0).

Position error Heading errorTest RMSE SD Max RMSE SD Max

mm mm mm deg deg degS1H150 135.6 97.0 398.8 0.43 0.22 2.71S1H151 167.9 118.4 481.4 0.62 0.38 8.29S1H140 145.5 107.3 436.4 0.47 0.24 2.68S1H141 155.5 114.1 479.0 0.52 0.29 5.99S1H240 146.8 102.1 410.1 0.48 0.24 2.72S1H241 135.1 96.4 387.8 0.43 0.25 7.78S1H230 151.3 99.1 393.5 0.64 0.38 2.72S1H231 137.3 94.7 393.8 0.50 0.29 5.28S1H430 82.1 40.0 308.4 0.27 0.17 2.56S1H431 233.1 124.6 743.5 0.54 0.36 7.74S1H420 68.1 30.0 148.6 0.27 0.17 2.51S1H421 71.6 31.8 153.8 0.26 0.17 5.22S2H150 106.3 62.5 285.6 0.49 0.27 1.60S2H151 109.3 64.2 247.2 0.60 0.38 6.00S2H140 104.6 61.0 279.5 0.48 0.27 1.60S2H141 98.3 53.9 238.9 0.47 0.28 4.85S2H240 89.9 46.7 227.1 0.38 0.22 1.58S2H241 71.0 34.6 154.8 0.33 0.21 6.16S2H230 90.5 47.0 224.1 0.38 0.21 1.58S2H231 68.8 33.3 147.7 0.32 0.20 4.63S2H430 47.6 18.1 108.4 0.21 0.14 1.36S2H431 50.6 19.1 143.7 0.16 0.12 6.85S2H420 46.5 17.0 97.3 0.22 0.14 1.44S2H421 55.0 23.5 129.4 0.16 0.11 5.54S3H150 171.6 129.9 467.7 0.46 0.23 1.69S3H151 176.1 134.2 491.8 0.45 0.25 7.45S3H140 155.7 119.9 442.0 0.42 0.23 1.69S3H141 145.3 105.1 400.0 0.41 0.21 6.49S3H240 117.2 85.7 332.2 0.33 0.18 1.61S3H241 140.8 105.1 408.2 0.36 0.21 7.83

51

Table A2: ATE errors for the test runs with Hector SLAM. The �rst two charactersmark the test run, H stands for Hector SLAM and the last three digits code forparameter values used. The �rst one states the occupancy grid resolution in cen-timetres, the second one tells the number of levels in the occupancy grid pyramidand the last one tells whether rotation compensation was used (1) or not (0).

Position error Heading errorTest RMSE SD Max RMSE SD Max

mm mm mm deg deg degS3H230 114.6 83.3 318.2 0.33 0.19 1.63S3H231 376.7 187.7 781.7 0.96 0.41 5.55S3H430 146.6 103.6 500.3 0.34 0.19 1.75S3H431 84.6 56.8 324.9 0.21 0.14 9.00S3H420 2341.4 1315.3 3203.3 0.92 0.39 1.83S3H421 1330.9 765.6 1904.9 0.69 0.37 2.91X1H150 47.1 37.3 198.7 0.06 0.05 2.04X1H151 54.5 42.9 197.8 0.09 0.05 1.07X1H140 31.0 22.2 87.3 0.08 0.05 2.05X1H141 34.2 22.6 90.8 0.09 0.05 1.06X1H240 65.2 46.7 244.7 0.15 0.09 2.12X1H241 67.2 46.7 211.3 0.15 0.09 1.22X1H230 56.4 36.2 147.3 0.16 0.10 2.11X1H231 40.1 24.8 108.8 0.13 0.09 1.12X1H430 78.7 52.9 221.1 0.24 0.13 1.81X1H431 96.3 69.6 248.3 0.42 0.32 2.29X1H420 53.6 30.8 134.7 0.23 0.14 1.86X1H421 90.8 63.7 248.7 0.49 0.37 1.46X2H150 23.5 16.2 88.8 0.10 0.07 1.08X2H151 25.5 17.7 132.6 0.13 0.09 1.91X2H140 26.7 19.5 89.5 0.11 0.08 1.12X2H141 25.8 17.8 85.0 0.13 0.09 1.88X2H240 36.8 28.8 122.4 0.14 0.09 1.18X2H241 24.7 17.5 81.5 0.10 0.07 1.62X2H230 29.8 22.4 99.7 0.13 0.08 1.14X2H231 36.2 28.6 121.5 0.13 0.08 1.62X2H430 59.4 42.3 172.4 0.23 0.13 1.21X2H431 67.9 49.1 188.6 0.22 0.12 1.58X2H420 77.3 58.8 229.2 0.25 0.13 1.33X2H421 44.0 26.8 107.0 0.19 0.12 1.57

52

B GMapping errors

Table A3: ATE errors for the test runs averaged from three distinct GMappingruns. The �rst two characters mark the test run, HS stands for smoothed Hectortrajectory which is used as an initial guess while G stands for GMapping. The lastthree digits code for parameter values used. The �rst one states the map resolution,the second one represents the linear and angular optimization step size and the lastone marks the sigma of scan end point matcher. The actual parameters are onehundredth of these numbers.

Position error Heading errorTest RMSE SD Max RMSE SD Max

mm mm mm deg deg degS1HS241 148.4 79.5 356.8 0.43 0.11 7.78S1G121 45.5 21.7 96.7 0.19 0.13 2.34S1G141 34.6 15.4 73.2 0.19 0.13 2.30S1G125 32.4 15.2 72.2 0.19 0.13 2.33S1G145 45.2 18.1 90.2 0.20 0.13 2.40S1G321 115.0 63.3 299.1 0.34 0.19 2.54S1G341 126.1 54.5 263.0 0.35 0.18 2.57S1G325 253.9 85.6 425.3 0.43 0.25 2.48S1G345 223.0 85.4 415.6 0.46 0.25 2.64S1G521 132.7 77.8 339.6 0.46 0.22 2.67S1G541 123.1 73.8 317.7 0.42 0.22 2.60S1G525 316.9 113.6 625.9 0.62 0.32 2.71S1G545 329.4 135.4 691.6 0.74 0.37 2.82S2HS241 109.9 57.0 241.1 0.34 0.11 6.15S2G121 36.1 17.1 90.8 0.24 0.16 1.66S2G141 35.4 18.5 94.3 0.23 0.15 1.69S2G125 44.3 19.7 108.1 0.24 0.16 1.75S2G145 41.0 18.3 93.8 0.24 0.16 1.62S2G321 102.2 47.9 242.0 0.37 0.22 1.90S2G341 97.2 40.2 216.8 0.36 0.21 1.82S2G325 175.6 54.3 309.2 0.47 0.33 3.41S2G345 194.0 77.8 427.5 0.53 0.30 2.47S2G521 77.7 35.6 187.4 0.36 0.21 1.79S2G541 78.7 39.2 197.5 0.37 0.21 1.80S2G525 203.6 79.9 474.8 0.57 0.35 3.60S2G545 225.9 95.7 522.6 0.56 0.30 2.50

53

Table A4: ATE errors for the test runs averaged from three distinct GMappingruns for S3 and two for X1 and X2. The �rst two characters mark the test run,HS stands for smoothed Hector trajectory which is used as an initial guess while Gstands for GMapping. The last three digits code for parameter values used. The�rst one states the map resolution, the second one represents the linear and angularoptimization step size and the last one marks the sigma of scan end point matcher.The actual parameters are one hundredth of these numbers.

Position error Heading errorTest RMSE SD Max RMSE SD Max

mm mm mm deg deg degS3HS241 148.4 79.5 356.8 0.43 0.11 7.78S3G121 64.0 47.0 194.2 0.28 0.18 1.47S3G141 25.8 13.3 80.1 0.21 0.14 1.23S3G125 40.0 26.1 124.9 0.24 0.16 1.33S3G145 43.0 28.5 127.9 0.24 0.16 1.32S3G321 128.4 96.2 337.9 0.36 0.21 1.59S3G341 151.3 109.3 397.8 0.43 0.24 1.69S3G325 146.1 81.2 345.2 0.35 0.21 1.58S3G345 174.5 91.5 358.4 0.34 0.21 1.77S3G521 139.8 107.5 393.0 0.41 0.25 1.75S3G541 138.0 105.7 387.0 0.40 0.24 1.74S3G525 264.3 172.7 655.6 0.62 0.35 2.10S3G545 256.7 178.9 689.0 0.64 0.38 2.12X1HS241 44.3 24.3 113.7 0.15 0.04 1.21X1G121 54.2 29.2 527.1 0.15 0.11 2.02X1G141 54.4 28.8 452.2 0.16 0.11 2.06X1G125 62.4 30.7 561.1 0.16 0.11 2.03X1G145 62.8 33.6 573.6 0.14 0.10 2.12X1G321 45.6 25.5 284.7 0.15 0.10 2.14X1G341 54.0 32.4 243.9 0.17 0.11 2.17X1G325 183.6 92.2 867.7 0.61 0.47 2.78X1G345 215.2 117.7 1827.9 0.40 0.29 2.57X1G521 37.2 18.6 90.3 0.17 0.12 2.11X1G541 33.8 19.0 88.8 0.16 0.12 2.13X1G525 143.5 99.5 489.2 0.26 0.17 2.22X1G545 149.5 92.8 442.4 0.31 0.20 2.25X2HS241 25.9 13.3 64.0 0.10 0.04 1.62X2G121 51.4 36.5 199.8 0.19 0.13 1.69X2G141 42.4 28.5 190.1 0.17 0.12 1.77X2G125 46.3 31.8 231.7 0.18 0.13 1.82X2G145 45.7 30.7 257.1 0.18 0.13 1.75X2G321 24.7 16.4 126.3 0.18 0.14 1.68X2G341 25.7 15.3 348.0 0.18 0.14 1.69X2G325 125.0 75.5 544.8 0.32 0.26 3.02X2G345 120.4 67.8 623.3 0.34 0.27 2.85X2G521 18.5 12.4 69.9 0.14 0.11 1.71X2G541 16.6 10.3 68.8 0.14 0.11 1.73X2G525 95.4 53.3 319.3 0.30 0.24 2.93X2G545 104.2 53.6 492.3 0.33 0.26 2.58

54

C Karto errors

Table A5: ATE errors and the amount of loop closures for the test runs with Karto.The �rst two characters mark the test run, HS stands for smoothed Hector SLAM(the initial trajectory used for the Karto trajectories below it) and K stands forKarto. The next four characters code for parameter values used. The fourth char-acter tells whether original (O) or delayed (D)loop closing method was used, the�fth character denotes the coarse loop closure minimum response values (the actualvalue is 1/10th and �ne response minimum is 0.1 smaller than the correspondingcoarse threshold). The sixth charactes tells the scan match resolution, 5 for 5mmand 2 for 2.5mm and the next one marks the scan matching penalty multiplier formatches di�ering from the odometry estimate. The last number tells whether theinitial trajectory had rotation compensation (1) or not (0).

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NS1HS241 148.4 79.5 356.8 0.43 0.11 7.78 0S1KO6211 49.5 36.3 135.8 0.25 0.12 2.47 10S1KO6511 30.2 18.3 89.5 0.21 0.12 2.45 15S1KO7211 79.3 66.2 225.7 0.23 0.11 2.34 7S1KO7511 80.3 52.4 181.0 0.31 0.14 2.69 8S1KO8211 31.5 17.7 83.9 0.20 0.11 2.43 5S1KO8511 55.1 31.9 136.1 0.20 0.12 2.54 4S1KO6231 63.3 44.5 179.6 0.30 0.14 2.46 8S1KO6531 48.3 35.4 156.4 0.23 0.12 2.51 16S1KO7231 71.0 52.6 204.5 0.30 0.14 2.51 7S1KO7531 49.2 33.6 155.9 0.26 0.13 2.48 11S1KO8231 16.9 8.7 50.9 0.19 0.11 2.31 5S1KO8531 154.8 115.5 427.4 0.37 0.14 2.22 3S1KO6261 111.9 86.8 345.2 0.37 0.14 2.46 11S1KO6561 255.1 135.0 584.7 1.28 0.17 4.79 19S1KO7261 33.9 23.2 119.2 0.22 0.12 2.39 6S1KO7561 269.3 147.1 634.2 1.40 0.17 5.09 14S1KO8261 37.8 19.3 89.1 0.25 0.13 2.35 5S1KO8561 254.5 135.1 582.8 1.28 0.17 4.79 10S1KO6291 47.9 31.7 142.0 0.27 0.14 2.44 10S1KO6591 36.7 22.5 90.9 0.19 0.11 2.36 14S1KO7291 18.8 9.7 59.8 0.23 0.12 2.38 6S1KO7591 25.8 15.6 70.0 0.25 0.12 2.44 7S1KO8291 40.9 26.4 119.6 0.23 0.12 2.52 3S1KO8591 31.5 21.4 87.8 0.23 0.13 2.52 3

55

Table A6: ATE errors and the number of loop closures for the test runs with Karto.Full parameter explanation in Table A5 caption.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NS1KD6211 52.6 40.4 144.6 0.24 0.12 2.36 11S1KD6511 31.0 15.8 86.5 0.22 0.12 2.45 14S1KD7211 60.5 40.5 157.1 0.26 0.13 2.50 6S1KD7511 56.9 37.7 144.4 0.24 0.13 2.58 8S1KD8211 63.0 37.2 139.4 0.27 0.13 2.61 4S1KD8511 42.7 24.0 107.1 0.22 0.12 2.43 4S1KD6231 59.0 41.7 180.3 0.28 0.13 2.41 10S1KD6531 31.3 19.5 102.6 0.22 0.12 2.39 14S1KD7231 166.2 108.2 435.1 0.57 0.15 2.67 7S1KD7531 59.1 44.9 187.1 0.25 0.13 2.45 10S1KD8231 79.4 53.3 212.3 0.31 0.14 2.65 5S1KD8531 154.8 115.5 427.4 0.37 0.14 2.22 3S1KD6261 46.9 31.7 147.5 0.29 0.14 2.48 11S1KD6561 254.3 134.6 582.0 1.28 0.17 4.79 20S1KD7261 28.6 21.1 103.0 0.24 0.12 2.38 6S1KD7561 254.3 134.6 580.1 1.28 0.17 4.79 13S1KD8261 59.8 38.5 131.5 0.30 0.13 2.58 4S1KD8561 254.5 135.1 582.8 1.28 0.17 4.79 10S1KD6291 27.6 15.1 75.5 0.22 0.12 2.40 10S1KD6591 81.7 56.0 194.0 0.29 0.14 2.56 14S1KD7291 56.7 36.0 152.1 0.26 0.13 2.51 7S1KD7591 21.3 14.0 68.5 0.18 0.11 2.39 7S1KD8291 45.1 30.3 139.1 0.24 0.12 2.46 6S1KD8591 20.0 12.0 59.5 0.21 0.12 2.44 4S2HS241 109.9 57.0 241.1 0.34 0.11 6.15 0S2KO6211 78.2 46.1 161.7 0.32 0.16 1.83 19S2KO6511 123.6 77.5 256.7 0.38 0.18 1.88 13S2KO7211 59.4 45.0 198.9 0.28 0.14 1.73 15S2KO7511 103.3 65.4 225.5 0.35 0.17 1.84 11S2KO8211 26.9 16.2 70.3 0.28 0.15 1.62 7S2KO8511 103.4 62.3 213.0 0.35 0.18 1.83 11S2KO6231 31.8 18.5 86.2 0.27 0.15 1.72 22S2KO6531 62.8 35.5 144.3 0.29 0.16 1.79 19S2KO7231 36.7 21.8 95.0 0.28 0.16 1.68 19S2KO7531 142.8 86.7 283.8 0.40 0.18 1.98 14S2KO8231 43.5 25.6 102.5 0.30 0.16 1.76 11S2KO8531 88.7 53.1 183.9 0.32 0.17 1.82 10S2KO6261 65.4 52.0 224.5 0.31 0.15 1.72 20S2KO6561 46.4 26.2 111.2 0.25 0.15 1.77 17S2KO7261 47.4 33.0 129.5 0.28 0.15 1.70 16S2KO7561 90.7 57.2 201.3 0.32 0.17 1.92 14S2KO8261 88.5 62.4 227.8 0.36 0.17 2.00 9S2KO8561 81.7 46.0 165.1 0.33 0.17 1.87 10

56

Table A7: ATE errors and the number of loop closures for test runs with Karto. SeeTable A5 caption for full parameter explanation.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NS2KO6291 71.6 54.8 252.0 0.33 0.16 1.73 22S2KO6591 58.9 36.4 126.3 0.28 0.16 1.76 17S2KO7291 57.3 35.8 136.0 0.30 0.16 1.73 15S2KO7591 89.9 53.7 185.6 0.32 0.17 1.94 12S2KO8291 43.1 28.8 99.4 0.30 0.16 1.74 10S2KO8591 100.8 59.6 211.3 0.35 0.18 1.91 9S2KD6211 43.6 24.5 104.3 0.29 0.16 1.84 19S2KD6511 29.8 18.3 79.1 0.24 0.14 1.71 18S2KD7211 117.5 103.5 428.0 0.41 0.15 1.68 15S2KD7511 83.4 50.1 163.9 0.34 0.17 1.87 13S2KD8211 164.3 143.6 605.9 0.53 0.17 1.61 8S2KD8511 60.4 39.9 140.7 0.27 0.15 1.71 10S2KD6231 31.8 18.5 86.2 0.27 0.15 1.72 22S2KD6531 62.8 35.5 144.3 0.29 0.16 1.79 19S2KD7231 36.7 21.8 95.0 0.28 0.16 1.68 19S2KD7531 142.8 86.7 283.8 0.40 0.18 1.98 14S2KD8231 43.5 25.6 102.5 0.30 0.16 1.76 11S2KD8531 88.7 53.1 183.9 0.32 0.17 1.82 10S2KD6261 41.6 29.8 144.1 0.29 0.16 1.66 18S2KD6561 46.4 26.2 111.2 0.25 0.15 1.77 17S2KD7261 47.4 33.0 129.5 0.28 0.15 1.70 16S2KD7561 90.7 57.2 201.3 0.32 0.17 1.92 14S2KD8261 88.5 62.4 227.8 0.36 0.17 2.00 9S2KD8561 81.7 46.0 165.1 0.33 0.17 1.87 10S2KD6291 71.6 54.8 252.0 0.33 0.16 1.73 22S2KD6591 58.9 36.4 126.3 0.28 0.16 1.76 17S2KD7291 57.3 35.8 136.0 0.30 0.16 1.73 15S2KD7591 89.9 53.7 185.6 0.32 0.17 1.94 12S2KD8291 43.1 28.8 99.4 0.30 0.16 1.74 10S2KD8591 100.8 59.6 211.3 0.35 0.18 1.91 9S2HS240 77.5 37.9 190.5 0.38 0.15 1.58 0S2KD6210 65.5 48.2 204.4 0.31 0.16 1.66 20S2KD6510 46.2 36.9 160.5 0.27 0.15 1.59 22S2KD7210 68.1 53.9 284.4 0.36 0.17 1.64 17S2KD7510 43.3 32.8 172.7 0.29 0.15 1.68 14S2KD8210 17.6 9.9 53.4 0.28 0.16 1.66 10S2KD8510 34.8 19.8 91.0 0.27 0.15 1.64 12S2KD6230 30.9 20.7 72.9 0.26 0.15 1.51 21S2KD6530 56.8 38.0 175.6 0.30 0.16 1.65 20S2KD7230 103.3 90.2 373.1 0.38 0.15 1.55 14S2KD7530 49.8 37.3 134.8 0.28 0.15 1.60 19S2KD8230 28.5 18.6 62.6 0.28 0.15 1.69 8S2KD8530 3650.6 2834.4 13814.5 9.23 0.98 17.80 3

57

Table A8: ATE errors and the number of loop closures for test runs with Karto. SeeTable A5 caption for full parameter explanation.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NS2KD6260 23.5 13.5 63.3 0.25 0.15 1.53 22S2KD6560 34.0 22.3 88.9 0.25 0.14 1.55 20S2KD7260 138.1 118.1 498.9 0.46 0.16 1.58 17S2KD7560 21.9 12.4 71.3 0.25 0.15 1.58 20S2KD8260 52.6 40.5 211.1 0.34 0.17 1.58 9S2KD8560 183.6 120.6 437.4 0.53 0.20 1.83 9S2KD6290 31.3 22.9 98.8 0.25 0.14 1.47 22S2KD6590 46.3 29.1 122.0 0.26 0.15 1.60 20S2KD7290 37.3 24.4 86.4 0.26 0.15 1.57 18S2KD7590 44.8 28.3 107.5 0.27 0.15 1.65 21S2KD8290 98.7 78.6 287.1 0.39 0.17 1.68 9S2KD8590 95.9 58.9 185.5 0.35 0.17 1.71 11S3HS241 148.4 79.5 356.8 0.43 0.11 7.78 0S3KO6211 49.5 36.3 135.8 0.25 0.12 2.47 10S3KO6511 30.2 18.3 89.5 0.21 0.12 2.45 15S3KO7211 79.3 66.2 225.7 0.23 0.11 2.34 7S3KO7511 80.3 52.4 181.0 0.31 0.14 2.69 8S3KO8211 31.5 17.7 83.9 0.20 0.11 2.43 5S3KO8511 55.1 31.9 136.1 0.20 0.12 2.54 4S3KO6231 63.3 44.5 179.6 0.30 0.14 2.46 8S3KO6531 48.3 35.4 156.4 0.23 0.12 2.51 16S3KO7231 71.0 52.6 204.5 0.30 0.14 2.51 7S3KO7531 49.2 33.6 155.9 0.26 0.13 2.48 11S3KO8231 16.9 8.7 50.9 0.19 0.11 2.31 5S3KO8531 154.8 115.5 427.4 0.37 0.14 2.22 3S3KO6261 111.9 86.8 345.2 0.37 0.14 2.46 11S3KO6561 255.1 135.0 584.7 1.28 0.17 4.79 19S3KO7261 33.9 23.2 119.2 0.22 0.12 2.39 6S3KO7561 269.3 147.1 634.2 1.40 0.17 5.09 14S3KO8261 37.8 19.3 89.1 0.25 0.13 2.35 5S3KO8561 254.5 135.1 582.8 1.28 0.17 4.79 10S3KO6291 47.9 31.7 142.0 0.27 0.14 2.44 10S3KO6591 36.7 22.5 90.9 0.19 0.11 2.36 14S3KO7291 18.8 9.7 59.8 0.23 0.12 2.38 6S3KO7591 25.8 15.6 70.0 0.25 0.12 2.44 7S3KO8291 40.9 26.4 119.6 0.23 0.12 2.52 3S3KO8591 31.5 21.4 87.8 0.23 0.13 2.52 3S3KD6211 52.6 40.4 144.6 0.24 0.12 2.36 11S3KD6511 31.0 15.8 86.5 0.22 0.12 2.45 14S3KD7211 60.5 40.5 157.1 0.26 0.13 2.50 6S3KD7511 56.9 37.7 144.4 0.24 0.13 2.58 8S3KD8211 63.0 37.2 139.4 0.27 0.13 2.61 4S3KD8511 42.7 24.0 107.1 0.22 0.12 2.43 4

58

Table A9: ATE errors and the number of loop closures for test runs with Karto. SeeTable A5 caption for full parameter explanation.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NS3KD6231 59.0 41.7 180.3 0.28 0.13 2.41 10S3KD6531 31.3 19.5 102.6 0.22 0.12 2.39 14S3KD7231 166.2 108.2 435.1 0.57 0.15 2.67 7S3KD7531 59.1 44.9 187.1 0.25 0.13 2.45 10S3KD8231 79.4 53.3 212.3 0.31 0.14 2.65 5S3KD8531 154.8 115.5 427.4 0.37 0.14 2.22 3S3KD6261 46.9 31.7 147.5 0.29 0.14 2.48 11S3KD6561 254.3 134.6 582.0 1.28 0.17 4.79 20S3KD7261 28.6 21.1 103.0 0.24 0.12 2.38 6S3KD7561 254.3 134.6 580.1 1.28 0.17 4.79 13S3KD8261 59.8 38.5 131.5 0.30 0.13 2.58 4S3KD8561 254.5 135.1 582.8 1.28 0.17 4.79 10S3KD6291 27.6 15.1 75.5 0.22 0.12 2.40 10S3KD6591 81.7 56.0 194.0 0.29 0.14 2.56 14S3KD7291 56.7 36.0 152.1 0.26 0.13 2.51 7S3KD7591 21.3 14.0 68.5 0.18 0.11 2.39 7S3KD8291 45.1 30.3 139.1 0.24 0.12 2.46 6S3KD8591 20.0 12.0 59.5 0.21 0.12 2.44 4X1HS241 44.3 24.3 113.7 0.15 0.04 1.21 0X1KO6211 69.5 60.2 241.8 0.23 0.07 2.03 18X1KO6511 25.1 12.7 73.3 0.12 0.06 2.06 15X1KO7211 18.3 11.6 65.2 0.10 0.06 2.06 13X1KO7511 14.8 8.3 49.1 0.11 0.06 2.11 14X1KO8211 35.0 28.0 127.7 0.15 0.06 2.13 10X1KO8511 16.1 7.4 58.4 0.11 0.06 2.00 9X1KO6231 16.7 10.2 56.0 0.11 0.06 2.18 20X1KO6531 27.9 17.2 85.7 0.12 0.06 2.14 16X1KO7231 44.8 36.3 163.7 0.18 0.06 2.08 13X1KO7531 15.1 8.2 60.6 0.11 0.06 2.09 15X1KO8231 64.5 56.1 226.0 0.22 0.06 2.06 9X1KO8531 25.2 13.0 74.1 0.12 0.06 2.21 9X1KO6261 62.9 53.9 211.7 0.21 0.06 2.10 19X1KO6561 19.0 8.4 59.7 0.12 0.06 2.06 16X1KO7261 66.0 55.4 236.5 0.24 0.07 2.13 13X1KO7561 26.1 18.7 79.0 0.12 0.06 2.07 14X1KO8261 43.2 35.1 151.5 0.19 0.07 2.13 10X1KO8561 34.0 21.4 125.6 0.13 0.06 2.06 9X1KO6291 26.0 18.0 93.0 0.15 0.07 2.13 18X1KO6591 19.5 8.8 59.7 0.12 0.06 2.09 15X1KO7291 60.5 52.1 217.8 0.22 0.06 2.09 13X1KO7591 18.2 12.0 56.0 0.11 0.06 2.11 15X1KO8291 27.7 19.7 100.0 0.14 0.06 2.13 9X1KO8591 19.7 8.7 62.3 0.11 0.06 2.09 10

59

Table A10: ATE errors and the number of loop closures for test runs with Karto.See Table A5 caption for full parameter explanation.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NX1KD6211 16.8 12.3 63.1 0.11 0.06 2.09 14X1KD6511 16.8 9.8 54.9 0.11 0.06 2.11 17X1KD7211 35.3 27.4 130.9 0.18 0.07 2.12 16X1KD7511 21.2 11.2 77.9 0.11 0.06 2.04 14X1KD8211 35.7 20.9 100.2 0.20 0.07 2.17 10X1KD8511 20.6 7.8 53.6 0.11 0.06 2.12 8X1KD6231 19.5 12.6 78.0 0.12 0.06 2.12 15X1KD6531 33.1 19.7 85.5 0.14 0.07 2.21 18X1KD7231 27.9 22.2 89.9 0.13 0.06 2.09 16X1KD7531 25.1 17.3 74.7 0.11 0.06 2.13 17X1KD8231 40.3 30.7 139.9 0.17 0.07 2.20 9X1KD8531 18.3 9.4 62.0 0.11 0.06 2.10 10X1KD6261 19.3 12.0 64.1 0.12 0.06 2.09 16X1KD6561 30.0 15.2 85.5 0.12 0.06 2.21 19X1KD7261 19.3 13.7 69.3 0.11 0.06 2.09 16X1KD7561 16.5 9.0 65.4 0.11 0.06 2.06 14X1KD8261 33.8 25.5 113.4 0.16 0.07 2.19 10X1KD8561 25.2 14.2 73.6 0.12 0.07 2.15 8X1KD6291 26.3 16.6 81.9 0.11 0.06 2.09 14X1KD6591 19.5 8.3 59.6 0.12 0.06 2.09 18X1KD7291 27.8 21.5 100.4 0.15 0.06 2.13 15X1KD7591 15.9 9.2 57.1 0.10 0.06 2.03 15X1KD8291 41.7 31.2 137.1 0.18 0.07 2.13 10X1KD8591 18.7 10.3 67.7 0.11 0.06 2.12 8X1HS240 73.9 41.6 161.8 0.15 0.04 2.12 0X1KD6210 33.6 28.6 113.7 0.15 0.06 2.06 16X1KD6510 24.6 17.6 109.9 0.11 0.06 1.97 16X1KD7210 15.8 10.5 66.7 0.12 0.06 2.11 15X1KD7510 35.2 25.8 136.6 0.12 0.07 2.12 18X1KD8210 25.5 17.4 76.0 0.15 0.07 2.17 10X1KD8510 24.9 17.7 108.5 0.11 0.06 2.12 10X1KD6230 35.1 28.4 122.1 0.16 0.07 2.16 15X1KD6530 35.8 25.3 157.8 0.11 0.06 2.13 17X1KD7230 18.1 13.6 63.1 0.12 0.06 2.11 15X1KD7530 20.8 12.4 58.1 0.12 0.06 2.08 16X1KD8230 23.2 15.8 69.9 0.12 0.07 2.16 10X1KD8530 16.6 8.7 57.8 0.11 0.06 2.10 6X1KD6260 13.6 8.9 52.6 0.12 0.06 2.10 15X1KD6560 10.7 4.8 44.8 0.11 0.06 2.12 19X1KD7260 23.9 17.8 83.0 0.13 0.06 2.13 15X1KD7560 27.5 18.1 73.1 0.12 0.06 2.16 16X1KD8260 20.5 15.2 72.5 0.14 0.07 2.12 9X1KD8560 24.5 14.4 65.2 0.11 0.06 2.15 11

60

Table A11: ATE errors and the number of loop closures for test runs with Karto.See Table A5 caption for full parameter explanation.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NX1KD6290 30.8 24.7 105.1 0.14 0.06 2.15 15X1KD6590 25.9 17.0 72.7 0.11 0.06 2.12 19X1KD7290 31.7 26.0 109.2 0.14 0.06 2.08 16X1KD7590 43.0 32.6 123.3 0.14 0.07 2.17 15X1KD8290 20.7 13.1 62.1 0.14 0.07 2.12 11X1KD8590 17.3 8.3 74.7 0.11 0.06 2.12 9X2HS241 25.9 13.3 64.0 0.10 0.04 1.62 0X2KO6211 16.7 10.0 55.2 0.12 0.07 1.29 15X2KO6511 27.5 16.5 66.1 0.14 0.07 1.29 15X2KO7211 19.3 14.3 72.6 0.13 0.07 1.36 13X2KO7511 21.5 10.3 50.2 0.13 0.07 1.29 11X2KO8211 25.2 17.6 60.2 0.14 0.07 1.27 7X2KO8511 13.6 8.3 50.5 0.13 0.07 1.33 6X2KO6231 17.2 11.2 65.6 0.12 0.07 1.28 15X2KO6531 25.5 13.8 67.2 0.14 0.07 1.33 15X2KO7231 15.0 10.1 44.7 0.12 0.07 1.32 13X2KO7531 18.2 8.6 40.2 0.14 0.07 1.35 12X2KO8231 28.1 20.6 88.2 0.13 0.07 1.46 8X2KO8531 19.6 11.4 64.0 0.16 0.08 1.29 6X2KO6261 22.0 14.7 71.9 0.14 0.07 1.45 14X2KO6561 23.8 11.7 48.2 0.13 0.07 1.29 15X2KO7261 20.0 14.1 64.4 0.13 0.07 1.40 12X2KO7561 40.4 25.6 115.4 0.19 0.08 1.29 11X2KO8261 21.3 15.5 54.3 0.12 0.07 1.37 8X2KO8561 23.5 14.5 71.8 0.15 0.08 1.29 6X2KO6291 47.9 36.1 143.4 0.17 0.08 1.28 15X2KO6591 40.2 26.2 114.7 0.18 0.08 1.28 15X2KO7291 19.0 12.7 76.5 0.13 0.07 1.32 13X2KO7591 49.3 34.9 159.4 0.22 0.08 1.30 11X2KO8291 12.1 7.8 37.8 0.12 0.07 1.37 8X2KO8591 19.0 11.4 59.6 0.15 0.08 1.30 6X2KD6211 12.3 7.7 35.4 0.12 0.07 1.41 17X2KD6511 34.5 23.7 87.0 0.15 0.08 1.28 16X2KD7211 17.0 12.0 60.8 0.13 0.07 1.30 17X2KD7511 21.3 14.6 58.7 0.14 0.08 1.34 16X2KD8211 12.8 7.9 51.6 0.12 0.07 1.36 8X2KD8511 19.3 11.5 58.7 0.14 0.07 1.33 6X2KD6231 15.2 10.8 48.0 0.12 0.07 1.38 18X2KD6531 43.0 32.7 113.5 0.15 0.08 1.34 16X2KD7231 15.1 10.9 50.9 0.12 0.07 1.29 16X2KD7531 42.9 31.6 138.8 0.19 0.08 1.34 15X2KD8231 15.4 10.0 54.9 0.13 0.07 1.41 11X2KD8531 28.1 17.3 65.0 0.14 0.08 1.31 8

61

Table A12: ATE errors and the number of loop closures for test runs with Karto.See Table A5 caption for full parameter explanation.

Position error Heading error LoopsTest RMSE SD Max RMSE SD Max

mm mm mm deg deg deg NX2KD6261 15.6 11.6 46.1 0.12 0.07 1.35 17X2KD6561 54.9 44.5 190.5 0.22 0.08 1.34 16X2KD7261 32.1 25.4 95.0 0.13 0.07 1.36 13X2KD7561 28.2 18.0 65.1 0.14 0.07 1.33 14X2KD8261 23.8 18.1 79.5 0.13 0.07 1.34 10X2KD8561 30.6 21.5 107.4 0.19 0.08 1.27 6X2KD6291 21.2 16.7 80.7 0.12 0.07 1.36 19X2KD6591 26.0 16.4 67.2 0.15 0.08 1.24 16X2KD7291 29.0 20.8 97.5 0.14 0.07 1.31 14X2KD7591 32.5 22.2 77.3 0.14 0.08 1.31 11X2KD8291 26.8 19.9 80.8 0.13 0.07 1.33 9X2KD8591 23.3 15.8 83.2 0.17 0.08 1.27 6X2HS240 32.8 25.1 113.5 0.14 0.04 1.18 0X2KD6210 20.0 12.6 58.1 0.13 0.07 1.31 18X2KD6510 42.3 30.8 98.5 0.16 0.08 1.32 18X2KD7210 22.2 13.4 61.8 0.12 0.07 1.32 18X2KD7510 33.0 21.5 73.8 0.15 0.08 1.28 16X2KD8210 30.6 21.9 71.8 0.13 0.07 1.53 13X2KD8510 26.0 17.8 73.9 0.16 0.08 1.28 10X2KD6230 22.3 13.5 57.7 0.14 0.08 1.26 18X2KD6530 30.4 19.7 68.6 0.14 0.07 1.39 18X2KD7230 23.9 16.4 79.8 0.13 0.07 1.32 16X2KD7530 35.2 24.2 82.3 0.14 0.08 1.29 13X2KD8230 43.8 30.5 118.7 0.14 0.08 1.35 11X2KD8530 24.8 14.4 55.2 0.14 0.08 1.34 7X2KD6260 71.6 60.7 261.1 0.28 0.08 1.72 19X2KD6560 19.8 10.7 54.7 0.14 0.07 1.39 18X2KD7260 25.4 17.1 74.7 0.13 0.07 1.28 17X2KD7560 26.3 16.6 60.6 0.14 0.08 1.29 16X2KD8260 34.8 25.4 94.7 0.14 0.08 1.49 8X2KD8560 25.1 15.9 59.5 0.14 0.08 1.34 7X2KD6290 20.6 12.3 62.9 0.13 0.07 1.32 20X2KD6590 31.9 25.2 109.6 0.16 0.07 1.37 18X2KD7290 41.3 28.9 113.7 0.14 0.08 1.30 15X2KD7590 28.7 18.1 66.2 0.14 0.08 1.30 13X2KD8290 31.8 22.4 88.2 0.13 0.07 1.42 10X2KD8590 23.6 13.0 55.2 0.14 0.08 1.34 8