Nonlinear Constraint Network Optimization for Efficient Map Learning

Learning models of the environment is one of the fundamental tasks of mobile robots since maps are needed for a wide range of robotic applications, such as navigation and transportation tasks, service robotic applications, and several others. In the past, numerous efficient approaches to map learning have been proposed. Most of them, however, assume that the robot lives on a plane. In this paper, we present a highly efficient maximum-likelihood approach that is able to solve 3-D and 2-D problems. Our approach addresses the so-called graph-based formulation of simultaneous localization and mapping (SLAM) and can be seen as an extension of Olson's algorithm toward non-flat environments. It applies a novel parameterization of the nodes of the graph that significantly improves the performance of the algorithm and can cope with arbitrary network topologies. The latter allows us to bound the complexity of the algorithm to the size of the mapped area and not to the length of the trajectory. Furthermore, our approach is able to appropriately distribute the roll, pitch, and yaw error over a sequence of poses in 3-D mapping problems. We implemented our technique and compared it with multiple other graph-based SLAM solutions. As we demonstrate in simulated and real-world experiments, our method converges faster than the other approaches and yields accurate maps of the environment.