💻 computer science

FD-SLAM: Fast Dense Radar-Inertial SLAM with Frequency-Domain Loop Closure and Pose Graph Optimization

This paper presents FD-SLAM, a fast dense radar-inertial SLAM system that enhances trajectory accuracy through frequency-domain loop closure and pose graph optimization, demonstrating competitive performance and real-time efficiency on CPU-only hardware in visually degraded environments.

Original authors: Nader J. Abu-Alrub, Nathir A. Rawashdeh

Published 2026-06-16

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Nader J. Abu-Alrub, Nathir A. Rawashdeh

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are driving a car through a thick fog, a heavy snowstorm, or a pitch-black tunnel. Your eyes (cameras) can't see anything, and your laser scanner (LiDAR) might get confused by the falling snow or dust. But your car has a special "radar eye" that can see right through all that mess. The problem is, radar is a bit "noisy" and blurry, like looking at a reflection in a wavy pond. Over a long drive, this blurriness adds up, and the car starts to think it's in a different place than it actually is.

This paper introduces FD-SLAM, a new way to help self-driving cars figure out exactly where they are using this "radar eye," even when the weather is terrible.

Here is how it works, broken down into simple steps:

1. The "Radar Eye" and the "Inner Ear"

Think of the car's radar as a camera that takes pictures of the world, but instead of colors, it sees heat and reflections. These pictures look a bit like static on an old TV.

The Problem: If you just look at these static pictures one by one, you might get lost because the static makes it hard to tell if you've moved a little or a lot.
The Solution: The system combines the radar with an IMU (Inertial Measurement Unit). Think of the IMU as the car's "inner ear" (like your balance system). It feels every tiny turn and bump instantly.
The Result: The system uses the "inner ear" to guess where the car is between radar pictures, and uses the "radar eye" to correct the guess. This creates a very accurate local map of where the car is right now.

2. The "Frequency Fingerprint" (Loop Closure)

The hardest part of driving is realizing, "Wait, I've been here before!" In the real world, this is called Loop Closure.

The Challenge: Because radar images are so noisy and blurry, it's hard to match a picture from 10 minutes ago with a picture from right now. It's like trying to recognize a friend in a crowd when they are wearing a mask and the lights are flickering.
The Magic Trick: Instead of trying to match the messy pictures directly, FD-SLAM turns the radar images into a compact "frequency fingerprint."
- Imagine taking a photo of a city and turning it into a musical chord. Even if the photo is blurry, the chord (the frequency pattern) stays the same.
- The system creates a short, simple code for every radar scan. When the car drives, it constantly checks: "Does this new code sound like any code I heard earlier?"
- If it finds a match, it knows, "Aha! I've visited this spot before!"

3. The "Detective's Checklist"

Just because two fingerprints look similar doesn't mean they are the same person (you might have a twin). To avoid mistakes, the system doesn't just trust the first match. It runs a multi-stage detective check:

Time Check: "Did we just pass this spot 5 seconds ago?" If yes, ignore it (that's just moving forward, not a loop).
Phase Check: "Do the waves in the radar image line up perfectly?"
Shape Check: "Do the shapes of the buildings match up?"
Logic Check: "Does the distance and angle make physical sense?"

Only if the candidate passes all these tests does the system say, "Yes, this is a real loop!"

4. The "Global Map Correction"

Once the system confirms it has visited a spot before, it draws a line on a map connecting the "then" and "now."

The Effect: Imagine you were walking and thought you walked in a straight line, but you actually drifted slightly left. Every time you realize you've been here before, the system pulls your entire path back into a straight line. It fixes all the small mistakes you made over the last hour, making the final map perfectly accurate.

Why is this special?

Most other systems try to turn the blurry radar pictures into sharp points or dots before they can work with them. This paper says, "No, let's keep the picture exactly as it is!"

They treat the radar scan like a dense image (like a photo) rather than a list of dots.
They use math (frequency domain) to find matches without needing to clean up the noise first.
It's fast enough to run on a standard computer chip (CPU) without needing expensive graphics cards.

The Bottom Line

The authors tested this on real driving data (the MulRan dataset) in various conditions. They found that:

It is much more accurate than just using the radar and "inner ear" alone.
It performs just as well as, or better than, the best existing radar systems currently available.
It is fast enough to run in real-time, meaning a car could actually use this to drive itself safely in fog, rain, or snow.

In short, FD-SLAM gives self-driving cars a superpower: the ability to navigate confidently through the worst weather by turning noisy radar static into a reliable, self-correcting map.

Technical Summary of FD-SLAM: Fast Dense Radar-Inertial SLAM with Frequency-Domain Loop Closure and Pose Graph Optimization

Problem Statement
Autonomous ground vehicles require robust Simultaneous Localization and Mapping (SLAM) capabilities, particularly in GPS-denied or visually degraded environments (e.g., fog, dust, darkness) where cameras and LiDAR often fail. While scanning Frequency-Modulated Continuous-Wave (FMCW) radars offer dense, 360-degree, long-range measurements suitable for these conditions, they present significant challenges: measurements are noisy, contain multipath reflections and speckle artifacts, suffer from motion distortion, and operate at lower sampling rates than other sensors. Existing radar SLAM systems often convert dense radar scans into sparse point sets or feature representations, potentially discarding valuable information inherent in the dense "image-like" structure of the radar data. Furthermore, many systems rely on descriptors or verification mechanisms adapted from LiDAR or camera pipelines rather than leveraging the native frequency-domain structure of radar scans. This paper addresses the question of whether the dense, image-like structure of scanning radar can be preserved and utilized throughout the entire SLAM pipeline, from local motion estimation to loop closure and global optimization.

Methodology
The proposed system, FD-SLAM, is a complete graph-based SLAM pipeline that extends the authors' previous Fast Dense Radar-Inertial Odometry (FD-RIO) front-end. The system is designed around a dense, radar-native representation, avoiding the reduction of scans to sparse landmarks. The architecture consists of three main components:

Radar-Inertial Odometry Front-End:
- This module provides local motion estimates between consecutive poses.
- It fuses dense correlation-based radar odometry with high-rate IMU measurements using a Kalman filter.
- The radar component estimates relative rotation in the Fourier domain using magnitude spectra in polar coordinates, followed by translation estimation in the spatial domain. This avoids explicit feature extraction.
- The IMU constrains drift between radar scans, while the radar provides exteroceptive corrections.
- These estimates form sequential odometry constraints in a pose graph.
Loop Closure Detection and Verification:
- Descriptor Generation: Each radar scan is converted into a compact descriptor derived from its frequency-domain representation. The process involves computing the 2D Fourier magnitude spectrum, transforming it into polar coordinates, applying log-compression, and aggregating column-wise mean and standard deviation profiles. This results in a 182-dimensional vector that is relatively invariant to rotation and translation.
- Candidate Retrieval: Loop-closure search is triggered at selected keyframes. Cosine similarity between the current descriptor and a database of previous descriptors is used to retrieve the top $K$ candidates.
- Multi-Stage Verification: To ensure robustness against false positives, candidates undergo a deterministic, multi-stage verification pipeline:
  - Temporal Filtering: Removes candidates too close in time to the current scan.
  - Phase-Correlation Screening: Uses normalized cross-power spectrum in the polar frequency domain to score alignment.
  - Scan-Alignment Similarity: Evaluates how well the aligned candidate scan explains the reference scan intensity pattern.
  - Geometric Consistency Checks: Verifies that the estimated relative transform (rotation, translation, and residual) falls within physically reasonable limits.
Pose Graph Optimization:
- Verified loop closures are added as non-sequential constraints to an SE(2) pose graph alongside the sequential odometry factors.
- A prior factor anchors the first pose.
- The system solves a weighted nonlinear least-squares problem to minimize the error between measured constraints and the estimated trajectory, thereby reducing accumulated drift and improving global consistency.
- The system operates asynchronously: the front-end runs in real-time, while loop closure detection and graph optimization execute in the background.

Key Contributions
The paper claims three primary contributions:

A Complete Dense Radar-Inertial SLAM Pipeline: It extends correlation-based radar-inertial odometry with descriptor-based loop closure detection and pose graph optimization, maintaining a dense representation throughout.
A Compact Frequency-Domain Polar Descriptor and Verification Pipeline: The method introduces a specific descriptor and a deterministic multi-stage verification process that preserves the dense scanning radar structure, enabling efficient candidate retrieval and robust verification over long trajectories without relying on learned features or sparse point clouds.
Comprehensive Evaluation: The system is evaluated on a publicly available dataset (MulRan) using standard KITTI metrics, demonstrating performance comparable to state-of-the-art methods.

Results
The system was evaluated on ten sequences from the MulRan dataset using an Intel i7-13700 CPU (no GPU).

Accuracy: FD-SLAM improves upon the FD-RIO odometry baseline in terms of translational and rotational accuracy on most sequences. For example, on sequence DCC03, translational error decreased from 2.30% to 1.17%.
Comparison: The method achieves competitive performance against current state-of-the-art radar SLAM methods, including TBV SLAM and RadarSLAM. Notably, FD-SLAM demonstrates favorable rotational accuracy, often achieving the lowest or tied-lowest rotational error across sequences, indicating effective constraint of heading drift.
Runtime: The radar-inertial front-end operates at approximately 14 Hz, significantly faster than the sensor's 4 Hz sampling rate, ensuring real-time performance. Loop closure detection and graph optimization run in the background with accumulated times remaining below the total sequence processing time, supporting asynchronous execution.
Qualitative: Visual results show that the optimized trajectories correct global drift while preserving the local structure of the odometry front-end.

Significance and Claims
The paper positions FD-SLAM as a "complementary design point" in the field of radar SLAM. Its significance lies in demonstrating that dense frequency-domain radar representations can support a complete SLAM pipeline without converting data into sparse features or relying on visual-feature descriptors. By preserving the native image-like structure of scanning radar and exploiting its frequency-domain properties for registration and loop closure, the system offers a robust alternative to sparse-feature-based approaches. The authors claim that this approach allows for effective loop candidate retrieval, verification, and global trajectory correction while maintaining computational efficiency suitable for real-time autonomous navigation. The work suggests that dense radar-native pipelines can achieve accuracy on par with methods that rely on complex preprocessing or learned models.