Fast Iterative 3D Mapping for Large-Scale Outdoor Environments with Local Minima Escape Mechanism

This paper introduces a novel iterative 3D mapping framework for large scale natural terrain and complex environments. The framework is based on an Iterative-Closest-Point (ICP) algorithm and an iterative error minimization mechanism, allowing robust 3D map registration. This was accomplished by performing pairwise scan registrations without any prior known pose estimation information and taking into account the measurement uncertainties due to the 6D coordinates (translation and rotation) deviations in the acquired scans. Since the ICP algorithm does not guarantee to escape from local minima during the mapping, new algorithms for the local minima estimation and local minima escape process were proposed. The proposed framework is validated using large scale field test data sets. The experimental results were compared with those of standard, generalized and non-linear ICP registration methods and the performance evaluation is presented, showing improved performance of the proposed 3D mapping framework.