Globally consistent 3D mapping with scan matching

A globally consistent solution to the simultaneous localization and mapping (SLAM) problem in 2D with three degrees of freedom (DoF) poses was presented by Lu and Milios [F. Lu, E. Milios, Globally consistent range scan alignment for environment mapping, Autonomous Robots 4 (April) (1997) 333–349]. To create maps suitable for natural environments it is however necessary to consider the 6DoF pose case, namely the three Cartesian coordinates and the roll, pitch and yaw angles. This article describes the extension of the proposed algorithm to deal with these additional DoFs and the resulting non-linearities. Simplifications using Taylor expansion and Cholesky decomposition yield a fast application that handles the massive amount of 3D data and the computational requirements due to the 6DoF. Our experiments demonstrate the functionality of estimating the exact poses and their covariances in all 6DoF, leading to a globally consistent map. The correspondences between scans are found automatically by use of a simple distance heuristic.