A multi-hypothesis constraint network optimizer for maximum likelihood mapping

Loop closure is one of the most difficult task in localization and mapping problems since it suffers from perceptual aliasing. Multi-hypothesis topological SLAM algorithms have been developed to exploit connectivity and disambiguate such difficult task. In this paper, we propose a multi-hypothesis constraint network algorithm that tracks multiple map topologies and simultaneously keeps metric information. The map is stored as a graph consisting of poses and constraints and each constraint is associated to a loop closure hypothesis. Hypotheses are stored in a hypothesis tree that is expanded whenever possible loop closure may occur. Network poses are computed according to the most likely topological configuration, but alternative pose values are also computed for the poses that are adjacent to a hypothesis constraint to recover quickly the new configuration when required. Results provide a validation of the proposed approach.