Abstract
This work is dedicated to simultaneous continuous-time trajectory estimation and mapping based on Gaussian Processes (GP). State-of-the-art GP-based models for Simultaneous Localization and Mapping (SLAM) are computationally efficient but can only be used with a restricted class of kernel functions. This paper provides the algorithm based on GP with Random Fourier Features (RFF) approximation for SLAM without any constraints. The advantages of RFF for continuous-time SLAM are that we can consider a broader class of kernels and, at the same time, maintain computational complexity at reasonably low level by operating in the Fourier space of features. The accuracy-speed trade-off can be controlled by the number of features. Our experimental results on synthetic and real-world benchmarks demonstrate the cases in which our approach provides better results compared to the current state-of-the-art.
Introduction
Simultaneous Localization and Mapping (SLAM) is a foundational capability for mobile robotics. As systems move toward continuous-time operations to handle asynchronous sensor data—such as LiDAR, IMU, and camera frames arriving at different rates—the mathematical framework must evolve. Continuous-time SLAM (often referred to as STEAM) treats the robot's trajectory as a continuous function, $x(t)$, typically modeled using Gaussian Processes (GPs).
While standard GPs offer a powerful non-parametric framework, they suffer from $O(N^3)$ computational complexity. Current state-of-the-art methods mitigate this by imposing a Markovian structure via Linear Time-Variant Stochastic Differential Equations (LTV SDE). While this reduces complexity to $O(N)$, it restricts the choice of kernel functions, potentially limiting accuracy in the presence of significant sensor noise.
Methodology: SLAM with Random Fourier Features
This paper introduces RFF-SLAM, an approach that leverages Random Fourier Features (RFF) to approximate GP kernels without the Markovian constraint.
The Fourier Approximation
Based on Bochner’s Theorem, any shift-invariant kernel can be approximated by a finite set of Monte Carlo samples in the Fourier domain. This allows the transformation of a complex, infinite-dimensional GP into a linear model in a high-dimensional feature space. Instead of solving for the trajectory directly in the original space, the algorithm solves for a set of weights associated with these Fourier features.
Computational Efficiency and Flexibility
By operating in the weight space, the computational complexity is reduced to $O(N)$, linear with respect to the number of observations. Crucially, this method does not require the kernel to have a specific state-space form, allowing the use of a broader class of kernels, such as the Radial Basis Function (RBF), to better model diverse motion profiles.
Experimental Evaluation
The proposed RFF-SLAM was evaluated against the LTV SDE method (the current gold standard) across three distinct scenarios:
- Synthetic Trajectories: Random 2D paths with varying noise levels and landmark counts.
- Plaza Dataset: A real-world dataset from an autonomous lawn-mower featuring range measurements and odometry.
- KITTI-Projected Dataset: A 2D bearing-only SLAM problem derived from the KITTI vision-based odometry dataset to test large-scale performance.
Results and Discussion
Experimental results indicate that RFF-SLAM consistently outperforms LTV SDE in rotation accuracy and resilience to noise.
As shown in the synthetic tests, RFF-SLAM maintains lower Absolute Pose Error (APE) as noise levels increase, particularly in rotation. The number of features ($M$) serves as a tunable hyperparameter, allowing a direct trade-off between computational speed and estimation accuracy. Higher values of $M$ generally lead to better performance, converging toward the true GP solution.
In the KITTI-projected experiments, the robustness of the RFF approach becomes even more apparent. When Gaussian noise is added to the observations, the error for the LTV SDE method increases significantly, whereas RFF-SLAM remains stable.
Limitations
While RFF-SLAM offers significant advantages, it is currently a Python-based proof of concept. Real-time deployment will require optimization in C++. Additionally, the use of periodic basis functions can occasionally introduce slight oscillations in the estimated trajectory, and kernel hyperparameters currently require manual tuning.
Conclusion
RFF-SLAM provides a flexible and efficient alternative for continuous-time SLAM. By breaking the Markovian constraint through Fourier space approximations, it achieves superior accuracy in noisy environments while maintaining linear computational complexity. Future work will focus on real-time optimization and automated hyperparameter selection.
Summary updated