Simultaneous localization and mapping with multimodal probability distributions

Simultaneous Localization and Mapping (SLAM) has focused on noisy but unique data associations resulting in linear Gaussian uncertainty models. However, a unique decision is often not possible using only local information, giving rise to ambiguities that have to be resolved globally during optimization. To solve this problem, the pose graph data structure is extended here by multimodal constraints modeled by mixtures of Gaussians (MoG). Furthermore, optimization methods for this novel formulation are introduced, namely (a) robust iteratively reweighted least squares, and (b) Prefilter Stochastic Gradient Descent (SGD) where a preprocessing step determines globally consistent modes before applying SGD. In addition, a variant of the Prefilter method (b) is introduced in form of (c) Prefilter Levenberg–Marquardt. The methods are compared with traditional state-of-the-art optimization methods including (d) Stochastic Gradient Descent and (e) Levenberg–Marquardt as well as (f) Particle filter SLAM and with (g) an optimal exhaustive algorithm. Experiments show that ambiguities significantly impact state-of-the-art methods, and that the novel Prefilter methods (b) and (c) perform best. This is further substantiated with experiments using real-world data. To this end, a method to generate MoG constraints from a plane-based registration algorithm is introduced and used for 3D SLAM under ambiguities.