Random Fourier Features based SLAM

Yermek Kapushev, Anastasia Kishkun, Gonzalo Ferrer, Evgeny Burnaev

Simultaneous Localization and Mapping (SLAM) is the "bread and butter" of mobile robotics, but as robots move toward continuous-time operations with asynchronous sensors, the math gets complicated. A new paper, "Random Fourier Features based SLAM," proposes a way to make continuous-time SLAM both more flexible and computationally efficient by leveraging a clever trick from the world of Gaussian Processes.

TL;DR

The authors introduce a continuous-time SLAM algorithm that uses Random Fourier Features (RFF) to approximate Gaussian Process (GP) kernels. This allows robots to use a much wider variety of kernel functions for trajectory estimation without the usual computational "tax," outperforming state-of-the-art methods in noisy environments.

What Problem Does This Paper Address?

Most SLAM systems operate in discrete time steps. However, real-world robots often deal with asynchronous data—a LiDAR scan arrives at one millisecond, an IMU reading at the next, and a camera frame somewhere in between. Continuous-time SLAM (or STEAM) solves this by treating the robot's trajectory as a continuous function, $x(t)$, often modeled using Gaussian Processes (GPs).

The catch? Standard GPs are computationally expensive ($O(N^3)$ complexity). To make them work for SLAM, current state-of-the-art methods (like those based on Linear Time-Variant Stochastic Differential Equations) force the GP to have a "Markovian" structure. This means they assume the robot's future state only depends on its current state. While fast, this restriction limits the types of "kernels" (functions that describe how points on the trajectory relate to each other) you can use, which can lead to lower accuracy when sensor data is noisy.

Methods

The authors break free from the Markovian constraint by using Random Fourier Features (RFF).

  1. The Fourier Trick: Based on Bochner’s Theorem, any shift-invariant kernel can be approximated by a finite set of Monte Carlo samples in the Fourier domain. Essentially, they transform a complex, infinite-dimensional GP into a linear model in a high-dimensional "feature space."
  2. Weight-Space View: Instead of solving for the trajectory directly, the algorithm solves for a set of weights ($b$) associated with these Fourier features.
  3. Efficiency: By using a low-rank approximation of the kernel matrix, the computational complexity drops from cubic to linear relative to the number of observations ($O(N)$).
  4. Flexibility: Unlike previous methods, this approach doesn't require the kernel to have a specific state-space form. You can use a broader class of kernels (like the popular Radial Basis Function) to better model different types of motion.

Datasets

The researchers evaluated their approach on three fronts:

  • Synthetic Trajectories: 10 different random 2D paths with varying noise levels and landmark counts (5 to 100).
  • Autonomous Lawn-Mower (Plaza Dataset): A real-world dataset featuring range measurements (time-of-flight) and odometry.
  • KITTI-Projected Dataset: They took the famous KITTI vision-based odometry dataset and re-projected it into a 2D bearing-only SLAM problem to test the algorithm's performance on large-scale, real-world visual data.

Key Insights and Ideas

  • Breaking the Markov Chain: The most significant "aha moment" is that removing the conditional independence (Markov) constraint actually helps. By using a dense (but low-rank) approximation, the model can better utilize the entire history of the trajectory to correct the current estimate, which is vital when sensors are noisy.
  • The Accuracy-Speed Knob: The number of features ($D$) acts as a tunable hyperparameter. You can increase $D$ for higher precision or decrease it for faster performance, providing a clear trade-off path for engineers.
  • Robustness to Noise: The RFF approach showed a much slower error growth rate as sensor noise increased compared to the standard "exactly sparse" GP methods.

Key Results

The authors compared their RFF-SLAM against the LTV SDE (the current gold standard for continuous-time GP SLAM).

  • Rotation Accuracy: RFF-SLAM consistently provided better rotation error results across almost all tests.
  • Noise Resilience: In the KITTI experiments, as Gaussian noise was added to the observations, the Absolute Pose Error (APE) for the LTV SDE method spiked significantly (up to 12.915), while RFF-SLAM remained relatively stable (around 5.077).
  • Landmark Estimation: The method also showed superior accuracy in estimating the positions of landmarks, often by several orders of magnitude in synthetic tests.

Limitations and Future Work

The authors provide a "proof of concept" implementation in Python, meaning they haven't yet optimized the code for real-time deployment on a robot. They also note that:

  • Hyperparameter Tuning: Choosing the right "lengthscale" for the kernel and the optimal number of features is currently a manual process (model selection).
  • Oscillations: In some real-world tests (like the lawn-mower), the Fourier features caused slight oscillations in the estimated trajectory, a known artifact of periodic basis functions.

Future work likely involves optimizing the implementation for C++/ROS and developing automated ways to tune the kernel parameters on the fly.

Summary updated

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.
BibTeX
@inproceedings{kapushev2021random,
  author = {Yermek Kapushev and Anastasia Kishkun and Gonzalo Ferrer and Evgeny Burnaev},
  title = {{Random Fourier Features based SLAM}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  url = {https://openreview.net/forum?id=zb0z0wGvysv},
}
Figure