A Multi-Hypothesis Constraint Network Optimizer for Maximum Likelihood Mapping Dario Lodi Rizzini, Stefano Caselli RIMLab - Robotics and Intelligent Machines Laboratory Dipartimento di Ingegneria dell’Informazione University of Parma, Italy E-mail {dlr,caselli}@ce.unipr.it Abstract— Loop closure is one of the most difficult task in localization and mapping problems since it suffers from per- ceptual 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. I. INTRODUCTION Simultaneous localization and mapping (SLAM) is the problem of concurrently building a map from the sensor mea- surements acquired by a robot, while concurrently estimating the position in the map. Mapping has become an important task for several robotic applications including navigation and human-robot interaction. In literature, several methods, either based on Bayesian filtering or on maximum-likelihood (ML) estimation, have been proposed to address SLAM and mapping problems. If map construction were achieved by integrating raw odometry and observations, the resulting map would be degraded by the uncertainty of measurements. To recover the degraded information mapping algorithms try to detect when a robot has returned to a previously visited region of the environment. The loop closure is a crucial operation for the estimation map since map metric error can be reduced by recovering the topological consistency. However, the identification of previously visited places can be difficult when multiple places appear indistinguishable through sensor observations. This perceptual aliasing can lead to wrong data associations and to the final corruption of estimated map. Several data association techniques have been proposed to close the loop reliably. In feature-based maps loop clo- sure is achieved by matching features extracted from raw observation with map landmarks. Individual comparison is commonly performed with validation gate based on Maha- lanobis distance bewteen landmark and feature parameters. Joint compatibility branch and bound (JCBB) [1] and the Hungarian algorithm [2], [3] have been proposed to find the optimal assignment between two collections of features instead of nearest neighbor strategy. All these techniques require an approximated estimation of robot pose in order to match the local features in the map reference frame. Combined constraint data association (CCDA) [4] performs association by building the constraint graphs both of features and of map landmarks and searching maximum clique. In the view-based representation of the environment (see e.g. [5]) each observation is associated to the robot pose from which the observation has been acquired. Thus, loop closure is performed comparing the current view with the other local maps stored in the map and the outcome is a constraint between the two related poses. The method for estimating the likelihood of a candidate association depends on the sensor data category. Occupancy grid maps are usually compared using correlation techniques [6]. Visual appearence [7] is a popular method to close loops with vision sensors. The methods listed before provide an estimation of loop closure using the current measurement. Map building al- gorithms usually applies a data association technique and, once a wrong association is estabished, the resulting map cannot be corrected. Since loop closure decisions based only on current measurement are difficult due to perceptual aliasing, the sequence of observations acquired during robot exploration can be exploited. In [8]) loop closure is estab- lished only after observing a sequence of images. Several methods track multiple outcomes of association by building an hypothesis tree. Dudek et al [9] pioneered the multi- hypothesis topological map approach using an exploration tree. In [10] the multi-hypothesis approach is compared with FastSLAM [11] and other methods that track multiple hy- potheses and an algorithm for “repairing” data associations is described. Recently, a multi-hypothesis topological algorithm has been proposed to solve perceptual aliasing [12]. In this paper, we illustrate a multi-hypothesis maximum- likelihood mapping algorithm. The algorithm formulates the mapping problem as a graph of robot poses and constraints between poses encoding both metric and topological in- formation [13]. Loop closing operation is performed by inserting in the graph an edge representing a spatial connec- tion between the current pose and a previously visited one. Thus, each association hypothesis corresponds to a different graph topology like for other topological maps. However,