CFEAR-Teach-and-Repeat: Fast and Accurate Radar-only Localization
Authors: Maximilian Hilger, Daniel Adolfsson, Ralf Becker, Henrik Andreasson, Achim J. Lilienthal
Organizations: Chair of Perception for Intelligent Systems, Munich Institute of Robotics and Machine Intelligence, Technical University of Munich, Munich, Germany · Robot Navigation and Perception Lab of the AASS Research Center, Örebro University, Örebro, Sweden · Bosch Rexroth, Germany
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 & tables
Fig. 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.
Fig. 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.
Fig. 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.
Fig. 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.
Method
radar
gyro.
accel.
transl.
rot.
VTR3 [ 4 ]
✓
1.97
0.565
STEAM-RO [ 5 ]
✓
1.43
0.410
CFEAR [ 35 ]
✓
0.61
0.206
CFEAR-TR (ours)
✓
0.42
0.136
CFEAR++ [ 6 ]
✓
✓
0.51
0.140
DRO-G [ 7 ]
✓
✓
0.26
0.049
TABLE I : KITTI odometry benchmark metrics for various radar-only and gyroscope-aided approaches evaluated on test sequences from the Boreas dataset.. Translational error is given in %, rotational error in \SIUnitSymbolDegree / 100m . Best and second best results are bold / underlined .
transl. error
rot. error
full
0.46
0.14
w/o doppler
0.47
0.15
w/o range offset
0.47
0.15
w/o azimuth
0.49
0.16
w/o all
0.62
0.19
TABLE II : KITTI odometry benchmark metrics on the training set, showing the effect of omitting different undistortion steps. Translational error is given in meter, and rotational error in \SIUnitSymbolDegree / 100m .
lateral [m]
longitudinal [m]
heading [°]
full
0.053
0.080
0.085
w/o doppler
0.067
0.212
0.093
w/o range offset
0.063
0.079
0.088
w/o azimuth
0.088
0.087
0.275
w/o all
0.099
0.213
0.277
TABLE III : Localization errors on Boreas training sequences when different undistortion steps are omitted.
sm=1
sm=3
sm=5
sl=1
0.057/0.109/0.110
0.055/0.080/0.089
0.054 /0.081/ 0.087
sl=2
0.065/0.081/0.105
0.054 / 0.079 / 0.087
0.054 /0.081/ 0.087
sl=3
0.058/0.080/0.095
0.054 / 0.079 / 0.087
0.053 /0.080/ 0.085
sl=4
0.072/0.081/0.105
0.067/0.080/0.098
0.054 / 0.079 / 0.085
sl=5
0.081/0.082/0.108
0.054 / 0.079 / 0.085
0.054 / 0.078 / 0.087
TABLE IV : Localization errors for different amounts of map frames sm and live frames sl . The RSME is given as (lateral [ m ] / longitudinal [ m ] / heading [ \SIUnitSymbolDegree ]). Best and second best results are bold / underlined .
Fig. 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 sm . Lateral (lat.) error is reduced, Longitudinal (long.) and Heading (head.) are not influenced.
Fig. 6 : Trajectory estimate of CFEAR-TR on the Boreas dataset, sequence 2021-03-09.
Fig. 7 : Distribution of localization errors using CFEAR-TR on the Boreas dataset, sequence 2021-03-09.
CFEAR-TR (ours)
VTR3 [ 4 ]
lat.
long.
head.
lat.
long.
head.
2020-12-04
0.070
0.088
0.088
0.072
0.082
0.211
2021-01-26
0.044
0.050
0.102
0.048
0.055
0.227
2021-02-09
0.063
0.056
0.105
0.053
0.051
0.235
2021-03-09
0.043
0.061
0.072
0.051
0.053
0.233
2021-06-29
0.052
0.094
0.083
0.069
0.095
0.246
TABLE V : Comparison with the state of the art approach VTR3 [ 4 ] . The RMSE is given as (lateral [ m ] / longitudinal [ m ] / heading [ \SIUnitSymbolDegree ]).