Sancta Simplicitas -- On the efficiency and achievable results of SLAM using ICP-Based Incremental Registration

Abstract -- This paper presents an efficient combination of algorithms for SLAM in dynamic environments. The overall approach is based on range image registration using the ICP algorithm. Different extensions to this algorithm are used to incrementally construct point models of the robot's workspace. A simple heuristic allows for determining which points in a newly acquired range image are already contained in the point model and for adding only those points that provide new information. Furthermore, the means for dealing with environment dynamics are presented which allow for continuously conducting SLAM and updating the point model according to changes in a dynamic environment. The achievable results of the overall approach are compared to Rao-Blackwellized Particle Filters as a state-of-the-art solution to the SLAM problem and evaluated using a recently published benchmark by Burgard et al. (2009).

Manuscript (pdf):	Submitted to ICRA'10 and accepted for publication..
Videos+Logfiles:	see below

The incremental registration procedure using the Iterative Closest Point (ICP) algorithm together with the concept of Sparse Point Maps (SPMs) and the removal of less probable measurements according to a probabilistic grid maps allows for constructing globally consistent maps of small and medium-sized indoor environments.

• Compared to pairwise registration of laser range scans, smaller smaller registration errors accumulate, but are "corrected" in later registrations. By matching against the incrementally built map, the global pose estimate is corrected when encountering previously visited places inducing an error in the local pose shift estimate between the current and the latest laser scan.
• Compared to gmapping, an efficient and widely used state-of-the-art Rao-Blackwellized Particle Filter:
  1. By only maintaining a single hypothesis ICP+SPM is faster than gmapping (50 particles) by a factor of 100 (avg. processing time per scan: 8-15ms compared to approx. 1000ms with gmapping).
  2. The resulting maps, the estimated trajectories and the deviations from the ground truth trajectory are comparable (not considerably deviating between ICP+SPM and gmapping).
• Larger loops cannot be closed. That is, in cases where the global pose estimate deviates too much from ground thruth (after loop closure) accumulated registration errors cannot be corrected. This is the case for the MIT Infinite Corridor data set by Mike Bosse and John Leonard.
• Due to the robust registration and the additional heuristics larger errors in the odometry can be corrected (or the odometry estimates can be completely ignored) and a globally consitent map can be constructed. The FR79 data set by Cyrill Stachniss, for example, contains larger jumps in the odometric pose estimate. gmapping (without additional preprocessing) was not able to construct a global map (tested over 10 runs). The jumps from the odometry estimate were contained in the final trajectory determined by gmapping. With SPM+ICP, the odometry jumps are ignored (and corrected respectively) and the resulting map is globally consitent.

(Red: constructed map was globally not consistent) (1: Taking the best out of 10-20 runs from GMapping, rev. 40) (2: As provided by Burgard et al.: GMapping + Pre-processing)

Data set recorded by Patrick Beeson in the ACES building at the University of Texas in Austin.
(Slight inconsistency in upper left → minor errors in local pose shift, global estimate is corrected!)

Constructed grid map		Video visualization of incremental registration

Map, detail view of inconsistency and individual deviations from ground truth relations.

Trans. error distribution	Rot. error distribution	Measured Runtimes

Original Logfile:	aces.clf
Logfile by ICP+SPM:	aces_icp.clf
Logfile by gmapping:	aces_gmapping.clf
Relations and Evaluator:	http://kaspar.informatik.uni-freiburg.de/~slamEvaluation

Data set recorded by Dirk Hähnel at the Intel Research Lab in Seattle.

Constructed grid map		Video visualization of incremental registration

Trans. error distribution	Rot. error distribution	Measured Runtimes

Original Logfile:	intel.clf
Logfile by ICP+SPM:	intel_icp.clf
Logfile by gmapping:	intel_gmapping.clf
Relations and Evaluator:	http://kaspar.informatik.uni-freiburg.de/~slamEvaluation

Data set recorded by Cyrill Stachniss at the MIT CSAIL building.

Constructed grid map		Video visualization of incremental registration

Trans. error distribution	Rot. error distribution	Measured Runtimes

Original Logfile:	mitcsail.clf
Logfile by ICP+SPM:	mitcsail_icp.clf
Logfile by gmapping:	mitcsail_gmapping.clf
Relations and Evaluator:	http://kaspar.informatik.uni-freiburg.de/~slamEvaluation

Data set recorded by Cyrill Stachniss in Building 079 at the University of Freiburg.
This data set contains several errors and jumps in the odometry estimates. By using these erroneous pose shifts when sampling from the motion model, gmapping is not able to produce a globally consitent map (tested over 10 runs using 50 and 100 particles). ICP+SPM corrects these estimates and constructs a globally consistent map.
In domestic and office-like environments where the robot never traverses longer distances through previously unmodeled terrain but repeatedly encounters already modeled terrain, smaller registration errors do not considerably accumulate. Hence, the constructed maps are locally and globally consitent, and the robot is always well localized.

Constructed grid map		Video visualization of incremental registration

Correction of large jumps in odometry estimates.

Original Logfile:	fr079.clf
Logfile by ICP+SPM:	fr079_icp.clf
Logfile by gmapping:	fr079_gmapping.clf
Relations and Evaluator:	http://kaspar.informatik.uni-freiburg.de/~slamEvaluation

• Rainer Kümmerle, Bastian Steder, Christian Dornhege, Michael Ruhnke, Giorgio Grisetti, Cyrill Stachniss, and Alexander Kleiner. On measuring the accuracy of SLAM algorithms. Autonomous Robots, volume 27, issue 4, pages 487-407, 2009.
• Wolfram Burgard, Cyrill Stachniss, Giorgio Grisetti, Bastian Steder, Rainer Kümmerle, Christian Dornhege, Michael Ruhnke, Alexander Kleiner, Juan D. Tardos. A Comparison of SLAM Algorithms Based on a Graph of Relations. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), St. Louis, MO, pages 2089-2095, 2009.
• http://kaspar.informatik.uni-freiburg.de/~slamEvaluation