SPM-ICP: Incremental Registration using the ICP Algorithm and Sparse Point Maps
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).
||Submitted to ICRA'10 and accepted for publication..
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:
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).
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)
Benchmarking: the ACES data set
Benchmarking: the INTEL data set
Benchmarking: the CSAIL data set
Benchmarking: the FR079 data set
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.
Correction of large jumps in odometry estimates.
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.