CFEAR-Teach-and-Repeat: Fast and Accurate Radar-only Localization

TL;DR

CFEAR-TR, a teach-and-repeat localization pipeline using a single spinning radar, which is designed for easily deployable, lightweight, and robust navigation in adverse conditions, and makes the C++ implementation of the work available to the community.

Abstract

Reliable localization in prior maps is essential for autonomous navigation, particularly under adverse weather, where optical sensors may fail. We present CFEAR-TR, a teach-and-repeat localization pipeline using a single spinning radar, which is designed for easily deployable, lightweight, and robust navigation in adverse conditions. Our method localizes by jointly aligning live scans to both stored scans from the teach mapping pass, and to a sliding window of recent live keyframes. This ensures accurate and robust pose estimation across different seasons and weather phenomena. Radar scans are represented using a sparse set of oriented surface points, computed from Doppler-compensated measurements. The map is stored in a pose graph that is traversed during localization. Experiments on the held-out test sequences from the Boreas dataset show that CFEAR-TR can localize with an accuracy as low as 0.117 m and 0.096°, corresponding to improvements of up to 63% over the previous state of the art, while running efficiently at 29 Hz. These results substantially narrow the gap to lidar-level localization, particularly in heading estimation. We make the C++ implementation of our work available to the community.

Figures (7)

• Figure 1: Visualization of the easily deployable, radar-only CFEAR-Teach-and-Repeat localization pipeline. The current scan (magenta) is simultaneously registered to keyframe scans from the teach phase (yellow) and recent live keyframes from the repeat phase (cyan). By matching sparse feature sets extracted from carefully undistorted radar scans, the system achieves both efficient and accurate localization without the need for additional sensors.
• Figure 2: Architecture of our teach-and-repeat localization framework. Incoming radar scans are filtered and undistorted. In the teach pass (odometry and mapping), the scan is registered to a set of recent keyframes. In the repeat pass (localization), the scan is jointly registered with both map frames from the teach-phase and recent scans form the repeat-phase.
• Figure 3: Optimization problems in (a) teach and (b) repeat phase, depicted as factor graphs. Grey triangles denote fixed scans; the white triangle is the optimized state. Blue dots show the scan-to-keyframe residuals.
• Figure 4: The live robot scan (red) is registered with map frames (green) and recent live frames (blue). Despite the presence of a dynamic object in the map (circled), the localization successfully aligns with the static environment.
• Figure 5: Localization errors in a teach graph generated using odometry and in a graph optimized with SLAM, depending on the used number of map frames $s_m$. Lateral (lat.) error is reduced, Longitudinal (long.) and Heading (head.) are not influenced.