Closing multiple loops while mapping features in cyclic environments

In this paper we propose an offline feature mapping algorithm capable of identifying and correctly closing multiple loops in cyclic environments (see video). The proposed algorithm iteratively alternates between a Kalman smoother based localization step and a map features recalculation step. The identification of loops is done during the localization step by a hybrid localization algorithm that generates and tracks hypotheses generated each time the robot visits an already mapped area. The main contribution of this paper lies on the ability of the proposed algorithm to exploit information contained within the hypotheses his- tories in order to calculate correct maps, regardless of the complexity of the environment and the number of loops in the robot's path.