Elastic LiDAR Fusion: Dense Map-Centric Continuous-Time SLAM

Chanoh Park1,2, Peyman Moghadam1,2, Soohwan Kim1, Alberto Elfes1, Clinton Fookes2, Sridha Sridharan2

Abstract— The concept of continuous-time trajectory rep-resentation has brought increased accuracy and efficiency tomulti-modal sensor fusion in modern SLAM. However, regard-less of these advantages, its offline property caused by therequirement of global batch optimization is critically hinderingits relevance for real-time and life-long applications. In thispaper, we present a dense map-centric SLAM method basedon a continuous-time trajectory to cope with this problem.The proposed system locally functions in a similar fashion toconventional Continuous-Time SLAM (CT-SLAM). However,it removes the need for global trajectory optimization byintroducing map deformation. The computational complexityof the proposed approach for loop closure does not dependon the operation time, but only on the size of the space itexplored before the loop closure. It is therefore more suitablefor long term operation compared to the conventional CT-SLAM. Furthermore, the proposed method reduces uncertaintyin the reconstructed dense map by using probabilistic surfaceelement (surfel) fusion. We demonstrate that the proposedmethod produces globally consistent maps without global batchtrajectory optimization, and effectively reduces LiDAR noise bysurfel fusion.

I. INTRODUCTION

In recent years, Continuous-Time Simultaneous Localiza-tion and Mapping (CT-SLAM) [1], [2] has been gainingpopularity over traditional discrete-time SLAM. The mainadvantages of deploying CT-SLAM are twofold. Firstly, com-pared with conventional discrete-time SLAM approaches,CT-SLAM offers efficient approaches in combining high-rate asynchronous sensors. It integrates multi-modal sen-sor measurements into temporally close trajectory controlpoints, which are weighted differently to each control pointaccording to their time of generation. In this manner, theasynchronous estimation problem is inherently addressedtogether. While all of the measurements are utilized in theoptimization, it does not increase the dimension of the statevector. Thus, the dimensionality is decoupled from the num-ber of measurements and is affected only by the trajectorysampling rate and the operation time without sacrificing themap accuracy.

The second main advantage of CT-SLAM is whereelement-wise pose recovery is required. As CT-SLAM isutilizing a continuous-time trajectory, it is relatively easy toextract a system pose at any query time. Thus, in early studiesit was proven that the motion distortion problem of Light

1 The authors are with the Autonomous Systems, Cyber-PhysicalSystems, DATA61, CSIRO, Brisbane, QLD 4069, Australia. E-mails:firstname.lastname@data61.csiro.au2 The authors are with the School of Electrical Engineering andComputer Science, Queensland University of Technology (QUT), Bris-bane, Australia. E-mails: chanoh.park, peyman.moghadam,c.fookes, s.sridharan@qut.edu.au

Fig. 1: Reconstructed surfel map of an office with a hand-held spinning LiDAR and proposed Elastic LiDAR Fusionmethod. Surfels with a diameter of 20mm cover the mapsurface with a 10mm resolution.

Detection and Ranging sensors (LiDAR) [3] or the rollingshutter problem of low cost image sensors [4]–[6] can beeffectively solved by this property.

While high-rate asynchronous sensor fusion and element-wise pose interpolation is successfully addressed by theapproach in CT-SLAM, the fact that its computational com-plexity for global consistency increases according to theoperation time is hindering its expansion to real-time robotapplications or long-term SLAM [3], [7]. The need for globalbatch optimization builds up the system load as time goeson and in the end it will reach an intractable level. Thisproblem becomes critical when it comes to mobile robotapplications where not only global mapping and localizationbut also long-term operation should be guaranteed. Further-more, most of CT-SLAM approaches are focusing more ontrajectory optimization and therefore, they are not able totake advantage of multiple observations of a space for betterdense mapping.

This paper proposes a new approach for dense LiDARmapping that combines conventional CT-SLAM with a map-centric approach inspired by [8] to overcome limitationsmentioned above. The proposed approach operates in asimilar fashion to local CT-SLAM. However, to remove theglobal trajectory dependency of the loop closure we build upthe global map without a global trajectory and deform thespace itself for a loop closure. As the proposed method doesnot maintain a trajectory, we propose a map prior constraintfor the local trajectory optimization so the map itself canplay the role of a trajectory. Furthermore, a dense globalmap is built based on map element-wise probabilistic fusionin a way that all of the spatially redundant LiDAR mea-surements are fully utilized and merged into the canonical

arX

iv:1

711.

0169

1v3

[cs

.RO

] 5

Mar

201

8

(a) Handheld 3D spinning LiDAR

Local Mapping Global Mapping

Loop Closure

Detection

Continuous-Time

Trajectory OptimizationDense Surfel

Fusion

Deformation

Module

Sparse Surfel

Integration

Sparse Surfel

Map

Dense Surfel

Map

Function

Map Data

Legend

Dense Surfels

Loop Closure Constraint

Radius Search

Map Update

Active Area

Map Update

Deformation

Graph

Deformation

Graph Application

Multi-Resolution

Sparse Surfels

LiDAR

Point Cloud

Key DB

Map Prior

(b) System block diagram

Fig. 2: (a) The experimental handheld 3D spinning LiDAR for mobile mapping. It contains a 2D laser, an IMU, an encoder,a color camera and a thermal camera. Note that the thermal camera is not used in the paper. (b) System block diagram ofour method. The device local trajectory is tracked in the Local Mapping stage, while the global consistent map is maintainedin the second Global Mapping stage.

form of the space. Therefore, the proposed method has astrong advantage in long-term operation over conventionaltrajectory-centric CT-SLAM.

This paper is organized as follows. Section 2 reviewsrelated works. In Section 3, we describe the overall architec-ture of our system, and detail our approach in Section 4, 5and 6. Results demonstrating advantages of our method overconventional CT-SLAM are given in Section 7. Finally wesummarize and present future directions in Section 8.

II. RELATED WORK

In 2009, Bosse and Zlot [9] proposed a linear interpolationbased continuous-time scan matching with a spinning 2DLiDAR. Their method subsampled vehicle odometer readingsand applied geometric feature matching constraints on therelated two trajectory points. The suggested linear continu-ous-time trajectory addressed the motion distortion problem,however, as it was an open loop approach, global mapconsistency was not guaranteed. In their later research [3],they further extend their work to a complete SLAM systemby introducing a global trajectory optimization. However,the introduction of the global batch trajectory optimizationrequired an offline processing step once scanning is done.

More recent researches has extended the approach in [3]by loosely coupled Visual-LiDAR constraint [10], tightlycoupled Visual-inertial constraint [7], [5], and tightly cou-pled Visual-Inertial-LiDAR constraint [2], yet all of theseapproaches suffer from the same problem.

Although the continuous-time trajectory approachsuccessfully addressed multi-modal sensor fusion [11] andmotion distortion problems, their applications are limitedto offline-based processing, because of the necessity of theentire trajectory and increasing computational complexity.Thus, it is not applicable to real-time robotic applicationswhere long-term operation should be guaranteed as well asglobal consistency.

Recently, ElasticFusion [8] proposed a map-centric ap-proach that removes pose graph optimization yet performsglobally consistent mapping by deforming the map itself.The concept of map-centric SLAM eliminated the need of

a pose graph for globally consistent mapping and convertedthe time dependency of the global optimization to a space-dependent problem. Also, by confining the tracking andfusion within recent map elements, they drastically reducedthe processing time per input frame. However, some featuresof their approach, such as projective data association andconfidence based surfel fusion, are limited to RGB-D sensorsand are not applicable to LiDAR sensor model [12].

Thus, in this paper we adopt the map-centric approach in[8] to improve applicability of conventional CT-SLAM in thelifelong applications.

III. OVERVIEW

Our proposed CT-SLAM system is composed of two maincomponents: local mapping and global mapping as shownin Figure 2. The first stage of the system takes InertialMeasurement Unit (IMU) and LiDAR measurements tobuild motion-distortion-corrected maps by continuous-timetrajectory optimization. This stage is similar to the slidingwindow operation in CT-SLAM [1], [3], [9], but the maindifference is that it takes the sparse global map as a map priorfor localization. The loop closure detection module keepsgenerating key points of the geometry in the moving windowand compares them to the previously generated key frames.

The second stage of the proposed system builds andfuses surfel maps. Note that the two different maps, multi-resolutional ellipsoid surfel map and disk surfel map, whichare denoted as sparse and dense maps, are utilized for dif-ferent purposes. The multi-resolutional 3D ellipsoidal surfelsin the sparse map proposed in [9] are ideal for fast androbust continuous-time trajectory optimization, whereas it istoo sparse for dense mapping [13]. Thus, we utilize 2D disksurfels [8], [14] for dense surfel fusion.

Global map consistency is achieved by non-rigid deforma-tion using the loop closure constraint from the loop closuremodule. This part of our method is inspired by the globaldeformation part of ElasticFusion [8]. We also extend itby considering surfel uncertainty propagation. Upon loopclosure detection, non-rigid deformation is performed on agraph to reduce the error of loop closure constraints. After

the optimization of the deformation graph, the deformationis applied to the entire map.

IV. LOCAL MAPPING

When a LiDAR sensor is capturing continuously on amoving platform, LiDAR scans become locally motion dis-torted. In this section, we describe how the geometrical andinertial information from measurements can be utilized tocompensate for such motion distortion, combined with thecontinuous-time trajectory optimization. At the end of thisstage, a locally consistent point cloud will be generatedaccording to the corrected trajectory. As the proposed methoddirectly utilizes the global map prior as a constraint, thereprojected point cloud is always represented in the globalmap coordinates.

A. Continuous-Time Trajectory Representation

Before presenting the detailed description of the localmapping, we will review the concept and advantages of thecontinuous-time trajectory representation.

The first important property of the continuous-timetrajectory representation is that the trajectory is modeledas a function of time. An exact system pose Tτ ∈ SE(3)at an arbitrary query time τ ∈ R can be interpolated froma set of trajectory elements Q. A single discrete trajectoryelement Qk ∈ Q is created at an interval of τstep with aninitial prior. Let Tτ be composed of translational componenttτ ∈ R3 and rotational component Rτ ∈ SO(3),

Tτ =

[Rτ tτ0 1

](1)

Then, Tτ is defined as an interpolation of neighboringtrajectory elements Qk∈Φ(τ) where Φ(τ) representsa set of neighbor indices. There are two types ofwell-known interpolation techniques for the continuoustrajectory. The trajectory element Qk will be a direct poseTk ∈ SE(3), Qk∈Φ(τ) = {Rk, tk,Rk+1, tk+1} for thelinear case [9] or indirect control points crk, ctk ∈ R3,Qk∈Φ(τ) = {ctk−1, crk−1, · · · ctk+2, crk+2} for theB-spline case [5], [7].

Although, the B-spline trajectory representation is C2

continuous and offers easy formulation for a derivative ofthe trajectory, its motion resolution and computational costis inversely proportional. Hence, it has been applied for theoffline applications [7] or used in real-time application witha low frequency motion assumption [15].

In contrast, the linear case leaves the raw high-frequencymotion and only adjusts the low-frequency part of the trajec-tory by optimizing the subsampled trajectory and applyingthe low-frequency compensation to the original trajectory. Insuch way, the trajectory can represent a higher-frequency mo-tion whereas the computational cost is much lower than theB-spline case. For this reason, we opted for the linear case.

In the linear interpolation case Tτ is defined from twoposes where their timestamps satisfy τk < τ < τk+1. Thus,given poses, their interpolation at τ is given by,

Rτ = Rkeα[ω]× , tτ = (1− α)tk + αtk+1 (2)

where the relative motion ω ∈ R3 of the exponen-tial mapping e[ω]× , that linearly interpolates the rotationalcomponent of two positions on the manifold, is definedas ω = ln(R−1

k Rk+1) and the interpolation ratio α =(τ − τk)/(τk+1 − τk).

In addition to the first property; ability of evaluatinga pose at arbitrary time τ , the second advantage is theefficiency in applying constraints to the trajectory elementsunder the circumstances where we need to carefully dealwith asynchronous and high-rate sensors. Let fn ∈ R bean objective function defined for each constraint n whichtakes sensor measurements zn and a system pose Tτ asinputs. Then, our objective is to find the corrected trajectoryelements Q that minimize the error functions.

Q = argminQ

∑n

∑l

fn(Tτ (Qk∈Φ(τz)), zn,l) (3)

where l represents the l-th measurement from the n-th con-straint and τz represents the timestamp of each measurement.Equation (3) can be solved by an iterative nonlinear leastsquare method.

As Tτ are defined as an interpolation of trajectory ele-ments, high rate sensor measurements increase the number ofrows in its Jacobian instead of the number of the state dimen-sions to be optimized. This makes an obvious difference fromthe conventional discrete-time trajectory model, where posesare created whenever there is a new sensor observation. Anexample of different objective functions fn will be describedin the following section.

B. Deformation Handling by Local Trajectory Optimization

When the motion of a device is relatively moderate com-pared to the scanning speed and the initial trajectory guess isfairly good, shapes of local features are often well preservedin the measurements even with the motion distortion. Thefirst two geometrical constraints utilize this property ofsweeping type LiDAR measurements [9], [10]. This processstarts by transforming the LiDAR measurements with respectto the world frame and extracting sparse ellipsoidal surfelsfrom multi-resolutional voxels [9]. From the sparse surfels,we calculate the distance errors between two correspondingnew surfels a and b and between a new surfel c and itscorresponding global map surfel m in their averaged normaldirections as illustrated in Figure 3.

eI =∑‖nTab(Rτauτa + tτa − (Rτbuτb + tτb))‖2 (4)

eM =∑‖nTmc(um − (Rτcuτc + tτc))‖2 (5)

where uτ is the centroid of a surfel, tτ ,Rτ are the inter-polated sensor pose at time τ . Note that the map prior umdoes not need a transformation, as they are points in theworld coordinate system. The motion distortion is correctedbased on the map prior. The initial map prior is given froma short period of stationary scanning in the beginning.

uτa

TτaTτb

Tτc

uτbuτc

um

Fig. 3: Illustration of geometrical constraints on the localtrajectory. Two surfels uτa and uτb generated at τa, τbwithin a local window forms constraints on the interpolatedtrajectory poses Tτa and Tτb . The constraint between thelocal sweep uτc and the map prior um forces the trajectoryto be fitted onto the map prior.

On the other hand, the inertial information from IMUoffers a prior regarding rotational velocity and translationalacceleration of the trajectory. As the observed rotationalvelocity and translational acceleration from IMU should co-incide with that of the trajectory, the constraints are given as,

eα =∑‖ατ − Rτ T (

d2

dτ2tτ − g) + bα‖2 (6)

eω =∑‖$τ − ωτ + bω‖2 (7)

where α, $ are respectively acceleration and rotationalvelocity from IMU measured at time τ , g is the gravita-tional acceleration, Rτ , tτ ,ωτ are respectively interpolatedrotation, translation, and rotational velocity of the trajectoryat τ , and bα, bω are the IMU biases. These two constraintsform a strong restriction on local smoothness but they areprone to error in a relatively long period, e.g., a few seconds,as they are first and second derivative constraint. Geometricalconstraints of Equation (4), (5) address this problem and helpoptimization to be locally and globally consistent.

Finally, the corrected trajectory is achieved by finding aset of parameters that minimize the constraints above as,

x = argminx

eI + eM + eω + eα (8)

where x = [Q,bω,bα, d].After the optimization, local dense and sparse surfel maps

Ml, Sl are built and updated by the optimized trajectory. Each2D disk surfel in the local dense map is composed of positionp ∈ R3, surfel normal n ∈ R3, and timestamp t. Also, theuncertainties of position and normal Σp ∈ R3×3, Σn ∈ R3×3,which are utilized in dense surfel matching and fusion arecalculated from their neighboring points. On the other hand,local sparse surfel maps Sl which are utilized in Equation (4),(5) are updated by the optimized trajectory. Each 3D ellipsoidsurfel is defined with a centroid c ∈ R3 and a covariancematrix Σc ∈ R3×3 which represents the distribution of pointswithin the voxel. The local maps Ml, Sl are fused into theglobal maps Mg , Sg in the following section.

V. SURFEL FUSION

In this section, temporal and probabilistic fusion to fullyutilize multiple observations of a scene will be discussed.

A. Surfel Matching and Fusion

We modified the fusion method for the dual surfel mapfrom our previous work [12]. As the property of eachsparse and dense surfel map Mg , Sg is different, differentapproaches for data association and fusion should be takento each map. For sparse surfel map Sg , considering that pla-narity of a surfel is important in continuous-time trajectoryoptimization, the surfel map is updated upon finding a widerand thinner surfel within a sparse map resolution. The densesurfel Mg , on the other hand, where the utilized number ofpoints is much smaller than the sparse surfel and as suchis prone to sensor noise, requires a dedicated approach fordata association and fusion. For data association, it utilizesa sensor noise model to search its match deeper along thebeam direction, while also densifying map resolution byintroducing the resolutional threshold. To effectively handlesensor noise, surfels are fused based on Bayesian fusion thatconsiders the sensor noise model.

For the colorization of the dense mapping, we utilized con-fidence based color fusion proposed in [16] with additionalparameters to consider uncertainties caused by fisheye lensdistortion and depth.

B. Active and Inactive Maps

One of the most important assumptions in the fusion is thatthe global map consistency around the area where a fusionoccurs should be guaranteed. However, as we are deployingan on-the-fly loop closure method, this is not the case. In theworst case, surfels will be fused right before the loop closureand will entirely ruin the local area as in Figure 4 (b). Thus,similar to [8], we introduce active and inactive maps A, Iaccording to surfel timestamps so that new surfels are alwaysoptimized and fused within the active area. To detect themisalignment between the active area and the inactive area,it finds the amount of overlap and misalignment by IterativeClosest Point (ICP). For a robust and fast ICP, only sparsesurfels are utilized with a geometrical weight as in [12].When the overlap and misalignment is large enough after theICP, it triggers deformation that will be described in the nextsection. To maintain the map coherency between active andinactive components, matched inactive components to activecomponents are found and updated in every step. Figure 4and Algorithm 1 show our temporal fusion method.

VI. GLOBAL MAP BUILDING

As the proposed system does not maintain any trajectory,an alternative method for a loop closure is necessary. Inthis section, we describe our deformation-based approach formaintaining the map to be globally consistent.

(a)

A

I

Ml

(b)

(d)(c)

OverlapT

Fig. 4: (a) A map including a loop closure. Red solid linesrepresent the latest map elements. (b) Fusion at the loop.Before the fusion (upper) and after the fusion (lower) (c)Fusion only within an active window (solid green lines). (d)Misalignment detection between inactive (solid black lines)and active area.

Algorithm 1: Temporal Fusion. To prevent the fusionright before loop closure, the proposed method dividesthe map into active and inactive regions according tothe timestamps of each surfel and fuses within activeareas only. A misalignment between active and inactiveis detected by ICP.

Input: New Frame Sl, Ml, Global Maps Sg , Mg

Output: Updated Global Maps Sg , Mg

1 A← GetCurrentActive(Sg,Mg)2 I← GetCurrentInactive(Sg,Mg)3 Sg,Mg ← Fusion(Sl,Ml,A)4 [ R, t, inlier, dist ]← ICP (Sl, I)5 if inlier > θin & dist > θd then6 [Rj , tj ]← GraphOptimization(R, t,Sl,Sg)7 Sg,Mg ←MapDeform(Rj , tj ,Sg,Mg)8 else9 n← NearistNeighborSearch(A, I)

10 if n < θn then11 Sg ← Fusion(A, I)12 end13 end

A. Map Deformation

Upon loop closure detection, map deformation is carriedout to maintain global map consistency. Deformation nodesand loop closure constraints are only selected from the sparsesurfel map Sg . Then, once optimal deformation is found byan optimization, the deformation is applied to both entiremap elements in Mg , Sg . We adopted the graph deformationtechnique and temporally connected deformation graph of[8]. However, our approach is different from [8] in thatthe number of deformation nodes is decided by the m2

(square metre) area size of the reconstructed space and theuncertainty is deformed along with the normal and centroid.

1) Graph Construction: Graph nodes are constructedfrom randomly selected surfel centroids of Sg . The centroidsare selected to uniformly represent the space. The numberof nodes is decided by m2 surface area of the space. As theproposed dense surfel map fusion maintains a canonical formof surface without surficial redundancy, the m2 size of themap surface can be easily found by counting the number oftotal surfels. Thus, considering that each surfel of the maphas a fixed radius circle, the total number of nodes n is,

n =⌈γπr2(E/m2)

⌉(9)

where γ is the total number of map elements, r is theradius of the circle, and E is the number of nodes perm2. Denser nodes help to reduce local spatial distortion butexponentially increase the computational cost. To reject anytemporally uncorrelated connection, we order the nodes bytemporal sequence and define their neighbors according totheir temporal closeness.

The deformation graph is composed of node positionsgj ∈ R3 , node rotations and translations, Rj , tj ∈ SE(3),and a set of neighbors V(gj). Most of the time, the mapis deformed by the difference between node translations.Regarding the number of neighbors, previous research [8]indicates that four neighbor nodes are sufficient.

2) Graph Application: Assuming that a set of graph nodelocations gj are established and their deformation attributesRj , tj are found, the graph deformation can be applied tothe entire map. Given a set of node parameters, an influencefunction φ(pi) that deforms any given point pi by the j-thnode gj is defined as

φ(pi) = Rj(pi − gj) + gj + tj (10)

For a smooth blending of deformation, the completedeformation of a point is defined as a weighted sum ofthe influence function φ(pi) with its neighbor nodes N(pi)around the position. When selecting nodes, their temporaland spatial closeness are considered. Thus, the deformedpose p

′

i is defined as,

p′

i =∑

j∈N(pi)

wj(pi)φ(pi) (11)

where the weight wj(pi) is decided by the distance betweenthe node gj and the point pi,

wj(pi) = (1− ‖pi − gj‖ /dmax) (12)

where dmax represents the max distance between the nodeand point within N(pi). The nodes that are relatively farfrom the given point pi have smaller weights and less effect.Note that Equation (11) will be applied to both sparse anddense maps Sg , Mg .

Not only surfel centroids, but other surfel attributes alsoneed to be deformed. The new normal direction of the surfelis defined as

n′

i =k∑

j∈N(pi)

wj(pi)R−Tj ni (13)

The uncertainty and geometry matrices Σpi, Σni, Σci aredeformed by a first order linear propagation. The generalform of the propagated uncertainty and geometry in the newdeformed space is given as

Σ′i = R′

iΣiR′

iT (14)

where R′

i represents the blended rotation of the surfel loca-tion and defined as

R′i =

k∑j∈N(pi)

wj(pi)Rj (15)

3) Graph Optimization: In this section, we will describeconstraints for the graph optimization, given a set of matchedsurfel centroid pairs psrc, pdest ∈ Sg . During the optimiza-tion, the transformation Rj , tj of each node which minimizethe source and destination will be found. The first constraintthat reduces the difference between the deformed source setsp′src = φ(psrc) and target surfel pdest is

eloop =∑‖p′src − pdest‖ 2 (16)

The distortion is applied over the all global map surfelsincluding the destination surfels pdest itself. To prevent theinfinite loop of deformation between source and destination,the following pinning constraint will fix the destinationsurfels not to be deformed.

epin =∑‖p′dest − pdest‖ 2 (17)

To guarantee the smooth deformation over the wholeregion, the following term spreads the deformation to theneighboring nodes. When the node rotation Rj is near theidentity matrix, the smoothness term indicates the distancebetween tj and its neighboring node translation tk.

ereg =m∑j

∑k∈V(gj)

‖Rj(gk − gj) + gj + tj − gk − tk‖2

(18)Then, the optimal node transformation Rj , tj that mini-

mizes radical deformation and loop closing error is definedas

[Rj , tj ] = argminRj ,tj∈SE(3)

ωregereg + ωpinepin + ωloopeloop

(19)where ωreg, ωpin, ωloop represent the weights for eachconstraint which follow the proposed values in [8].

Since the cost function is a non-linear pose graph opti-mization problem in the form of f(T) = Tp + c on a mani-fold, we use the non-linear iterative Gauss-Newton method.

4) Global Loop Closure: There are two sources of loopclosure detection. For a loop where the source and destina-tion distance is moderately close, the ICP in Algorithm 1 willdetect the misalignment first. However, for a large-scale map-ping where a possible large extent of misalignment exists, analternative 3D point cloud-based place recognition such as[17], [18] is required. We utilize the 3D point cloud based

Methods Optimization* No. State Elapsed Time (sec)

Proposed Fig. 5 (i) 192 0.12CT-SLAM [3] Fig. 5 (ii) 3396 195.40

TABLE I: Global optimization cost comparison of the mapin Figure 5. Note that the state dimension for each state is 6DoF for both [3] and the proposed work. In the proposedwork, the state vector represents the node transformationwhereas in [3] it is trajectory elements. CT-SLAM optimizesthe subsampled global trajectory instead of the full trajectory.*Location of loop closure occurrence.

place recognition presented in [17] where descriptors of theinput scene are calculated in every frame and compared tothe keys of the scenes in the database. In both cases, loopclosure constraints are given as follows.

pdest = Rpsrc + t (20)

where psrc are randomly selected points from Sl and the mis-alignment R, t, which is achieved from ICP between Sl and I.Note that we utilize a transformed destination pdest from thepsrc to prevent any unwanted deformation caused by sourceand destination point difference. This way, the deformationimplicitly reduces the misalignment [R, t] to be [I, 0].

VII. EXPERIMENT

We present the quantitative and qualitative performanceanalysis of the proposed method in terms of global loopclosure cost, trajectory estimation and surface estimationperformance. The evaluation is made based on four differentdatasets which include various ranges, from a small sizeroom to a large size indoor and outdoor mixed environment.

A hand-held 3D spinning LiDAR is utilized for the realdata experiments. The device consists of a spinning HokuyoUTM-30LX laser, an encoder, a Microstrain 3DM-GX3IMU, Grasshopper3 2.8 MP color camera and Optris PI 450thermal-infrared 382×288 pixel camera (Figure 2 (a)).

A. Global Loop Closure Cost Estimation

To quantitatively compare the global optimization costof the proposed method over the batch optimization basedCT-SLAM, we utilized a scanning scenario that includesa loop closure and a redundant scanning path as shownin Figure 5 (a). The loop is closed at Figure 5 (i) in theproposed method which is right after the detection of a loopclosure. After the first loop closure it goes without furtherloop closure until the end. However, CT-SLAM [3] does theloop closure at the end (Figure 5 (ii)) on its entire trajectoryfor the global batch optimization. Table I shows the costcomparison between CT-SLAM and the proposed method.The state dimension to be optimized is much smaller in theproposed method. The qualitative comparison between thetrajectory optimized by global batch trajectory optimizationand deformation is shown in Figure 6.

(a) (b)

Fig. 5: Scanning trajectory and deformation graph of Figure.1. (a) Scanning path. Our proposed method closes a loop at(i) whereas CT-SLAM does at (ii). The trajectory is coloredby time: blue at the start transitioning to red at the end.(b) Constructed graph (red squares) and its correction (greensquares).

Fig. 6: Qualitative trajectory comparison between the globaltrajectory optimization (blue line) [3] and the proposedmethod (red line) of the map in Figure 7 (ii). (Upper) Topview. (Lower) Side view. Note that the trajectory includestwo traverses.

B. Trajectory Estimation

To evaluate the trajectory estimation accuracy of theproposed method, we compared the estimated trajectorywith the globally optimized trajectory from [3]. For thecomparison, we utilized the absolute trajectory Root MeanSquare Error (RMSE) metric [8], which estimates theEuclidean distances between the distortion-based trajectoryand the globally optimized trajectory. Table II shows theerror and trajectory statistics of each map. A qualitativecomparison of trajectories is presented in Figure 6. Note thatthe proposed method is not required to store the trajectory.They are saved and deformed only for a comparison.

C. Surface Estimation

For a quantitative comparison where ground truth is notavailable, a well-known structure such as a floor or wallis utilized for calculating the relative noise level. Table IIIshows statistical analysis of the multiple planner areas (redcircles in Figure 8 (b)). The error in the table representsthe mean projective distance, which is calculated from themean plane of each patch. Statistics shows that the estimated

(i) (ii) (iii)Fig. 7: Qualitative datasets. Starting from the top row, normalmap, colorized map, details of the map; (i) A small sizemeeting room; (ii) Multiple floor structure. Map dimensionand trajectory length is listed in the Table II; (iii) Indoor andoutdoor mixed environment.

Dataset Traj Error (m) Length (m) Time (min) Size (m)

Fig. 1 0.047 330 14.6 20×20Fig. 7 (i) 0.041 130 6.1 10×6Fig. 7 (ii) 0.056 300 11.4 55×20Fig. 7 (iii) 0.076 360 9.1 60×25

TABLE II: Trajectory estimation RMSE between the de-formed trajectory and the globally optimized trajectory (CT-SLAM [3]).

surface is up to three times less noisy than the point cloudgenerated using CT-SLAM [3]. Also, regardless of the num-ber of the original raw points in the proposed method thesurfel density in patches are uniformly maintained after thefusions. Additionally, with the surfel-based map representa-tion the proposed method is capable of producing a dense 2Dscenes even with sparser point cloud as shown in the last rowof Figure 7. Figure 8 (a) shows the qualitative comparison ofan extracted circular patch of [3] and the proposed method.Most of the noisy points in the original CT-SLAM pointcloud (Figure 8 (a) left) are removed and fused into theminimum number of surfels that are required to maintainthe map surface with the specified surface resolution. Seeour supplementary video to clearly visualize and understandthe proposed method1.

VIII. CONCLUSION

In this paper, a new approach for dense LiDAR-basedmap-centric CT-SLAM was presented. The proposed systemutilizes map deformation as a way for maintaining globalmap consistency instead of conventional global batchtrajectory optimization to improve the applicability of theconventional CT-SLAM in long-term operation applications.As a result, our system does not need to wait until theend of scanning to achieve a globally consistent map.Also, under the guarantee of global map consistency at alltimes, we were able to utilize an on-the-fly map fusionmethod in which all the measurements are fused into the

1https://youtu.be/QNNLncT9XmQ

Side View

Top View

(a) (b)

a

b c

d

Fig. 8: (a) Qualitative comparison on a well-known structure.A circular patch is extracted from the floor of the map in (b)and compared with the patch from [3]. Top and side view ofthe patch from CT-SLAM [3] (left) and our method (right).(b) Extracted patch locations in Table III. Each patch has0.7m radius.

Location No. Points No. Surfel CT-SLAM [3] Proposed

a 47.8×104 3.7×103 16.08 7.72b 37.8×104 4.1×103 15.78 5.79c 40.6×104 3.8×103 16.43 10.39d 56.3×104 3.8×103 19.40 13.07

TABLE III: Surface estimation statistics. The projectivedistance errors in the last two columns are in mm. Themaximum ideal number of 0.02m surfels that can fit into0.7m circular patch is 3.8×103.

global map. Therefore, measurements storage for globaloptimization is not necessary. Our experimental resultsindicate the deformed trajectory is marginally different tothe trajectory produced by the global batch optimization,however surface estimation is up to three times less noisythan the conventional CT-SLAM. In both conventionalCT-SLAM and the proposed method, motion distortioncaused by the moving platform is effectively handled bydeploying continuous-time trajectory representation, but theproposed method is able to prevent the global optimizationcost from increasing according to the operation time and istherefore more amenable for real-time applications.

ACKNOWLEDGMENTS

The authors gratefully acknowledge funding of the projectby the CSIRO and QUT. The institutional support of CSIROand QUT, and the help of several individual members ofstaff in the Autonomous Systems Laboratory including TomLowe, Gavin Catt and Mark Cox are greatly appreciated.

REFERENCES

[1] R. Dube, H. Sommer, A. Gawel, M. Bosse, and R. Siegwart, “Non-uniform sampling strategies for continuous correction based trajectory

estimation,” in 2016 IEEE International Conference on Robotics andAutomation (ICRA).

[2] J. Rehder, P. Beardsley, R. Siegwart, and P. Furgale, “Spatio-temporallaser to visual/inertial calibration with applications to hand-held, largescale scanning,” in IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), 2014, pp. 459–465.

[3] M. Bosse, R. Zlot, and P. Flick, “Zebedee: Design of a Spring-Mounted 3-D Range Sensor with Application to Mobile Mapping,”IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1104–1119, Oct.2012.

[4] C. Kerl, J. Stuckler, and D. Cremers, “Dense Continuous-Time Track-ing and Mapping with Rolling Shutter RGB-D Cameras,” in 2015IEEE International Conference on Computer Vision (ICCV), Dec.2015, pp. 2264–2272.

[5] S. Lovegrove, A. Patron-Perez, and G. Sibley, “Spline Fusion: Acontinuous-time representation for visual-inertial fusion with applica-tion to rolling shutter cameras.” British Machine Vision Association,2013, pp. 93.1–93.11.

[6] S. Vidas, P. Moghadam, and M. Bosse, “3D thermal mapping ofbuilding interiors using an RGB-D and thermal camera,” in IEEEInternational Conference on Robotics and Automation (ICRA), 2013,pp. 2311–2318.

[7] P. Furgale, J. Rehder, and R. Siegwart, “Unified temporal and spa-tial calibration for multi-sensor systems,” in IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), 2013, pp. 1280–1286.

[8] T. Whelan, R. F. Salas-Moreno, B. Glocker, A. J. Davison, andS. Leutenegger, “ElasticFusion: Real-time dense SLAM and lightsource estimation,” The International Journal of Robotics Research,vol. 35, no. 14, pp. 1697–1716, Dec. 2016.

[9] M. Bosse and R. Zlot, “Continuous 3D scan-matching with a spin-ning 2D laser,” in IEEE International Conference on Robotics andAutomation (ICRA), 2009, pp. 4312–4319.

[10] J. Zhang and S. Singh, “Visual-lidar odometry and mapping: Low-drift, robust, and fast,” in IEEE International Conference on Roboticsand Automation (ICRA), 2015, pp. 2174–2181.

[11] P. Moghadam, M. Bosse, and R. Zlot, “Line-based extrinsic calibrationof range and image sensors,” in IEEE International Conference onRobotics and Automation (ICRA), 2013, pp. 3685–3691.

[12] C. Park, S. Kim, P. Moghadam, C. Fookes, and S. Sridharan, “Prob-abilistic surfel fusion for dense lidar mapping,” in Proceedings of theIEEE Conference on Computer Vision and Pattern Recognition, 2017,pp. 2418–2426.

[13] M. Schadler, J. Stuckler, and S. Behnke, “Multi-resolution surfelmapping and real-time pose tracking using a continuously rotating 2dlaser scanner,” in IEEE International Symposium on Safety, Security,and Rescue Robotics (SSRR), 2013, pp. 1–6.

[14] H. Pfister, M. Zwicker, J. Van Baar, and M. Gross, “Surfels: Surfaceelements as rendering primitives,” in Proceedings of the 27th annualconference on Computer graphics and interactive techniques, 2000,pp. 335–342.

[15] J.-H. Kim, C. Cadena, and I. Reid, “Direct semi-dense slam forrolling shutter cameras,” in Robotics and Automation (ICRA), IEEEInternational Conference on, 2016, pp. 1308–1315.

[16] M. Keller, D. Lefloch, M. Lambers, S. Izadi, T. Weyrich, and A. Kolb,“Real-time 3D reconstruction in dynamic scenes using point-basedfusion,” in International Conference on 3D Vision (3DV), 2013, pp.1–8.

[17] M. Bosse and R. Zlot, “Place recognition using keypoint voting inlarge 3D lidar datasets,” in 2013 IEEE International Conference onRobotics and Automation, May 2013, pp. 2677–2684.

[18] R. Dube, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, and C. Cadena,“Segmatch: Segment based place recognition in 3d point clouds,” inInternational Conference on Robotics and Automation (ICRA), 2017,pp. 5266–5272.