Real-Time Loop Closure in 2D LIDAR SLAM

1 Real-Time Loop Closure in 2D LIDAR SLAMW olfgang Hess1, Damon Kohler1, Holger Rapp1, Daniel Andor1 Abstract Portable laser range-finders, further referred to asLIDAR, and simultaneous localization and mapping (SLAM)are an efficient method of acquiring as-built floor and visualizing floor plans in Real-Time helps theoperator assess the quality and coverage of capture data. Build-ing a portable capture platform necessitates operating underlimited computational resources. We present the approach usedin our backpack mapping platform which achieves real-timemapping and loop Closure at a 5 cm resolution. To achieve Real-Time loop Closure , we use a branch-and-bound approach forcomputing scan-to-submap matches as constraints.

2 We provideexperimental results and comparisons to other well knownapproaches which show that, in terms of quality, our approachis competitive with established INTRODUCTIONAs-built floor plans are useful for a variety of surveys to collect this data for building managementtasks typically combine computed-aided design (CAD) withlaser tape measures. These methods are slow and, by em-ploying human preconceptions of buildings as collectionsof straight lines, do not always accurately describe the truenature of the space. Using SLAM, it is possible to swiftlyand accurately survey buildings of sizes and complexities thatwould take orders of magnitude longer to survey SLAM in this field is not a new idea and isnot the focus of this paper.

3 Instead, the contribution of thispaper is a novel method for reducing the computationalrequirements of computing loop Closure constraints fromlaser range data. This technique has enabled us to mapvery large floors, tens-of-thousands of square meters, whileproviding the operator fully optimized results in RELATED WORKScan-to-scan matching is frequently used to computerelative pose changes in laser-based SLAM approaches, forexample [1] [4]. On its own, however, scan-to-scan matchingquickly accumulates matching helps limit this accumulation oferror. One such approach, which uses Gauss-Newton to findlocal optima on a linearly interpolated map, is [5]. In thepresence of good initial estimates for the pose, provided inthis case by using a sufficiently high data rate LIDAR , locallyoptimized scan-to-map matching is efficient and unstable platforms, the laser fan is projected onto thehorizontal plane using an inertial measurement unit (IMU)to estimate the orientation of scan matching approaches, such as [1],further reduce local error accumulation.

4 Although compu-tationally more expensive, this approach is also useful for1 All authors are at Closure detection. Some methods focus on improvingon the computational cost by matching on extracted featuresfrom the laser scans [4]. Other approaches for loop closuredetection include histogram-based matching [6], feature de-tection in scan data, and using machine learning [7].Two common approaches for addressing the remaininglocal error accumulation are particle filter and graph-basedSLAM [2], [8].Particle filters must maintain a representation of the fullsystem state in each particle. For grid-based SLAM, thisquickly becomes resource intensive as maps become large; one of our test cases is22,000m2collected over a3kmtrajectory.

5 Smaller dimensional feature representations, suchas [9], which do not require a grid map for each particle, maybe used to reduce resource requirements. When an up-to-date grid map is required, [10] suggests computing submaps,which are updated only when necessary, such that the finalmap is the rasterization of all approaches work over a collection of nodesrepresenting poses and features. Edges in the graph are con-straints generated from observations. Various optimizationmethods may be used to minimize the error introduced byall constraints, [11], [12]. Such a system for outdoorSLAM that uses a graph-based approach, local scan-to-scanmatching, and matching of overlapping local maps based onhistograms of submap features is described in [13].

6 III. SYSTEM OVERVIEWG oogle sCartographerprovides a Real-Time solution forindoor mapping in the form of a sensor equipped backpackthat generates 2D grid maps with ar= 5cm resolution. Theoperator of the system can see the map being created whilewalking through a building. Laser scans are inserted into asubmapat the best estimated position, which is assumed to besufficiently accurate for short periods of time. Scan matchinghappens against a recent submap, so it only depends onrecent scans, and the error of pose estimates in the worldframe achieve good performance with modest hardware re-quirements, our SLAM approach does not employ a particlefilter. To cope with the accumulation of error, we regularlyrun apose optimization.

7 When a submap is finished, that isno new scans will be inserted into it anymore, it takes partin scan matching for loop Closure . All finished submaps andscans are automatically considered forloop Closure . If theyare close enough based on current pose estimates, a scanmatcher tries to find the scan in the submap. If a sufficientlygood match is found in a search window around the currentlyestimated pose, it is added as aloop closing constraintto theoptimization problem. By completing the optimization everyfew seconds, the experience of an operator is that loops areclosed immediately when a location is revisited. This leadsto the soft Real-Time constraint that the loop Closure scanmatching has to happen quicker than new scans are added,otherwise it falls behind noticeably.

8 We achieve this by usinga branch-and-bound approach and several precomputed gridsper finished LOCAL2D SLAMOur system combines separate local and global approachesto 2D SLAM. Both approaches optimize the pose, =( x, y, )consisting of a(x,y)translation and a rotation , of LIDAR observations, which are further referred toas scans. On an unstable platform, such as our backpack,an IMU is used to estimate the orientation of gravity forprojecting scans from the horizontally mounted LIDAR intothe 2D our local approach, each consecutive scan is matchedagainst a small chunk of the world, called a submapM,using a non-linear optimization that aligns the scan with thesubmap; this process is further referred to as scan matching accumulates error over time that is laterremoved by our global approach, which is described inSection ScansSubmap construction is the iterative process of repeatedlyaligning scan and submap coordinate frames, further referredto as frames.

9 With the origin of the scan at0 R2, wenow write the information about the scan points asH={hk}k=1,..,K,hk R2. The pose of the scan frame in thesubmap frame is represented as the transformationT , whichrigidly transforms scan points from the scan frame into thesubmap frame, defined asT p=(cos sin sin cos ) R p+( x y) t .(1)B. SubmapsA few consecutive scans are used to build a submap. Thesesubmaps take the form of probability gridsM:rZ rZ [pmin,pmax]which map from discrete grid points at a givenFig. points and associated , for example5cm, to values. These values can bethought of as the probability that a grid point is each grid point, we define the correspondingpixeltoconsist of all points that are closest to that grid a scan is to be inserted into the probability grid,a set of grid points forhitsand a disjoint set formissesarecomputed.

10 For every hit, we insert the closest grid point intothe hit set. For every miss, we insert the grid point associatedwith each pixel that intersects one of the rays between thescan origin and each scan point, excluding grid points whichare already in the hit set. Every formerly unobserved gridpoint is assigned a probabilityphitorpmissif it is in one ofthese sets. If the grid pointxhas already been observed, weupdate the odds for hits and misses asodds(p) =p1 p,(2)Mnew(x) = clamp(odds 1(odds(Mold(x)) odds(phit)))(3)and equivalently for scan and pixels associated withhits(shaded and crossed out)andmisses(shaded only).C. Ceres scan matchingPrior to inserting a scan into a submap, the scan pose isoptimized relative to the current local submap using aCeres-based [14] scan matcher.