Abstract
We introduce an innovative method for incremental nonparametric probabilistic inference in high-dimensional state spaces. Our approach leverages slices from high-dimensional surfaces to efficiently approximate posterior distributions of any shape. Unlike many existing graph-based methods, our slices perspective eliminates the need for additional intermediate reconstructions, maintaining a more accurate representation of posterior distributions. Additionally, we propose a novel heuristic to balance between accuracy and efficiency, enabling real-time operation in nonparametric scenarios. In empirical evaluations on synthetic and real-world datasets, our slices approach consistently outperforms other state-of-the-art methods. It demonstrates superior accuracy and achieves a significant reduction in computational complexity, often by an order of magnitude.
Introduction
In robotics, Simultaneous Localization and Mapping (SLAM) is fundamental for navigation. Traditional SLAM algorithms often assume uncertainty follows a Gaussian distribution. However, real-world environments are frequently ambiguous and noisy, leading to multi-modal or non-parametric distributions. Existing nonparametric methods face significant challenges: high computational costs (e.g., mm-iSAM), intensive training overhead for neural networks (e.g., NF-iSAM), and accuracy loss due to intermediate approximations like Kernel Density Estimation (KDE).
This paper presents a "slices" perspective for SLAM that performs incremental, nonparametric inference without these computational burdens. By directly sampling from high-dimensional joint distribution surfaces, the proposed algorithm avoids intermediate reconstructions, achieving up to an order of magnitude speedup while maintaining or exceeding state-of-the-art accuracy.
The Slices Perspective
The core innovation is treating the joint probability function as a high-dimensional surface rather than trying to "learn" or "reconstruct" it at every step.
Methodology
- Slicing the Surface: Conditional distributions are found by taking a "slice" of the high-dimensional surface at a specific point. Marginal distributions are treated as mixtures of these slices.
- Forward-Backward on Factor Graphs: The method utilizes standard factor graph and Bayes tree architectures. In the forward pass, new factors are approximated using Monte Carlo estimators. In the backward pass, marginals are retrieved as mixtures of high-dimensional slices.
- Incremental Re-elimination: To maintain efficiency, only affected parts of the graph are updated when new measurements arrive.
- Nonparametric Early Stopping: A major contribution is the use of Maximum Mean Discrepancy (MMD) to measure the distance between distributions. This allows the algorithm to stop the backward pass early if the distribution has converged, significantly saving computation.
Experimental Evaluation
The approach was evaluated on two primary benchmarks: a synthetic "Multi-Modal Four Doors" 1D SLAM problem and the real-world "Plaza2" dataset involving range-only measurements.
Key Results
The "slices" approach demonstrated significant improvements in both efficiency and accuracy:
- Efficiency: It achieved a reduction in computational complexity by an order of magnitude compared to
NF-iSAMandmm-iSAM. - Accuracy: In the "Four Doors" test, it correctly recovered the true mode with fewer samples than competitors. It matched the accuracy of offline batch-processing methods like
NSFGwhile operating in real-time.
On the Plaza2 dataset, the method maintained a consistently low Root Mean Square Error (RMSE) while being significantly faster than other online nonparametric solvers.
Conclusion and Future Work
The "slices" perspective offers a powerful alternative for nonparametric SLAM by bypassing the need for secondary models like KDEs or neural networks. While highly effective, future work will focus on automating hyperparameter tuning and investigating scalability in even higher-dimensional state spaces, such as 3D SLAM with hundreds of landmarks.
Summary updated