Dr-PoGO: Direct Radar Pose-Graph Optimization

Abstract: This paper introduces Dr-PoGO, a method for Simultaneous Localization And Mapping (SLAM) using a 2D spinning radar. Unlike cameras or lidars that require line-of-sight, millimetre-wave radars can `see' through dust, falling snow, rain, etc. Accordingly, it is a great modality for robust perception regardless of the weather conditions. While most existing radar-based SLAM methods rely on the extraction of point clouds or features to perform ego-motion estimation, Dr-PoGO leverages direct registration techniques for odometry (DRO) and loop-closure registration. An off-the-shelf radar-focused place recognition algorithm, RaPlace, provides loop-closure candidates. As RaPlace does not provide relative transformations, Dr-PoGO introduces a coarse-to-fine registration that uses visual features and descriptors to obtain an initial guess for the direct transformation refinement. The global trajectory is optimized in a pose-graph optimization. Dr-PoGO demonstrates state-of-the-art performance over 300km of data in various real-world automotive environments. Our implementation is publicly available: https://github.com/utiasASRL/dr_pogo.

• The paper presents a radar-centric SLAM framework that combines direct odometry with loop-closure registration for robust trajectory estimation in adverse conditions.
• It utilizes a coarse-to-fine pipeline, integrating SIFT+RANSAC with direct refinement to reduce outliers and achieve sub-meter positioning accuracy.
• The system’s pose-graph optimization, which incorporates gyroscope bias, demonstrates superior performance over feature-based and lidar methods on extensive real-world datasets.

Authoritative Summary of "Dr-PoGO: Direct Radar Pose-Graph Optimization" (2605.04806)

---

Introduction and Motivation

"Dr-PoGO: Direct Radar Pose-Graph Optimization" proposes a radar-centric SLAM framework aimed at robust trajectory estimation in adverse environmental conditions. Unlike vision- or lidar-based systems, millimeter-wave radars maintain sensing capability through occlusions such as snow, rain, dust, and smoke. Dr-PoGO leverages a 2D spinning FMCW radar paired with direct registration techniques for both odometry and loop-closure registration. The system uses RaPlace for radar-specific loop-closure detection and introduces a coarse-to-fine registration pipeline integrating visual feature extraction and direct refinement to determine relative transformations between local radar maps. The global trajectory is subsequently refined through pose-graph optimization incorporating both odometry and loop-closure constraints.

Figure 1: Dr-PoGO performs SLAM using 2D radar data; validation was conducted on 225 km of real-world data across diverse environments.

---

Methodological Framework

Direct Odometry and Loop-Closure Registration

Dr-PoGO deploys Direct Radar Odometry (DRO) for scan-to-local-map registration, maximizing a continuous cross-correlation between raw radar scans and incrementally built local maps. Unlike feature extraction–based approaches, which discard substantial information and are susceptible to scene dynamics, direct methods utilize all scan intensity data for higher robustness and precision. For loop closures, Dr-PoGO extends DRO to perform registration between local maps, addressing the lack of initial pose guess through feature-based (SIFT + RANSAC) coarse alignment, followed by direct refinement via continuous cross-correlation maximization.

Figure 2: Example of local-map registration; scaled cross-correlation score $s$ robustly discriminates between good and poor registrations irrespective of scene intensity.

Pose-Graph Optimization

Pose-graph optimization incorporates sequential DRO odometry constraints and loop-closure transformations via binary factors in $\mathrm{SE}(2)$, refining the entire trajectory batchwise. The gyroscope bias is also integrated as an optimization variable, modeled via Brownian motion priors. Robustness to loop-closure outliers is improved using a Cauchy loss in the corresponding residuals.

---

Experimental Validation

Datasets and Baselines

Dr-PoGO was evaluated on over 300 km of automotive data, including the Boreas and Boreas-RT datasets, which feature multi-season, multi-condition environments, including natural and industrial settings, unstructured roads, and environmental challenges such as snowstorms and dust.

Figure 3: Challenging environmental conditions in Boreas-RT, including snowstorms, weak structure, and natural scenes.

Baselines included Navtech-SLAM and TBV-SLAM, which use feature-based methods for odometry and loop-closure (ORORA, CFEAR-3, ScanContext, CorAl) and rely on pose-graph optimization for trajectory estimation.

Quantitative Results

On the Boreas public dataset, Dr-PoGO exhibited lowest average Absolute Trajectory Error (ATE; 0.82 m) and End-Pose Error (EPE; 0.42 m), consistently outperforming feature-based baselines. TBV-SLAM yielded moderate ATE ($\sim$9.61 m), with results highly sensitive to loop-closure detection quality.

Figure 4: Absolute Trajectory Error and End-Pose Error (log scale) for Dr-PoGO and baselines across 13 Boreas sequences.

Dr-PoGO maintained GPS-level trajectory accuracy (<5 m ATE) across all architecture variants and environmental types, including complex Skyway, Forest, and Farm routes where feature-based SLAM failed. Comparisons with Fast-LIO2 and 2Fast-2Lamaa (lidar-inertial baselines) demonstrated that Dr-PoGO achieved at least fourfold improvement over the best lidar benchmark, even in environments challenging for lidar, owing to radar’s resilience to occlusions and dynamic objects.

Trajectory examples further confirm alignment to ground truth over diverse environments.

Figure 5: Dr-PoGO trajectory estimations over three environment types from self-collected dataset, showing alignment to ground truth.

---

Loop-Closure Registration Analysis

A coarse-to-fine pipeline was quantitatively evaluated, demonstrating significant reduction in outliers and improvement in registration RMSE. Coarse SIFT+RANSAC alignment, while prone to outliers, preserves candidate diversity. Direct refinement, validated via scaled cross-correlation thresholding, nearly eliminates outliers while achieving sub-meter positional and sub-degree rotational RMSE.

Figure 6: Strip plots: Direct refinement reduces position and rotation alignment error compared to feature-based coarse registration across 15 sequences.

---

Ablation and Computational Analysis

Ablation studies indicate that (i) absence of gyroscope or local maps degrades performance, causing frequent failures, (ii) coarse registration alone underperforms direct refinement (errors up to four times higher), and (iii) gyroscope bias modeling, while beneficial in theory, is less impactful given the sensor quality used.

Dr-PoGO achieves real-time operation (<250 ms/frame), scalable by adjusting frequency of keyframes and loop-closure computation.

---

Theoretical and Practical Implications

Dr-PoGO substantiates the efficacy of direct methods in radar-based SLAM, outperforming alternatives using point or feature extraction. By integrating both odometry and loop-closure registration via direct cross-correlation, Dr-PoGO minimizes drift and enhances robustness to dynamic and unstructured environments. The practical superiority over both radar- and lidar-based systems suggests promising applicability in autonomous vehicles and mobile robotics operating under degraded visual or geometric conditions. Theoretical implications include validating direct registration pipelines with scalable pose-graph optimization and opening avenues for global radar bundle adjustment beyond relative pose constraints.

---

Conclusion

Dr-PoGO advances radar-based SLAM by leveraging direct odometry and direct loop-closure registration, validated by comprehensive field experiments and robust ablation studies. The framework reliably matches and registers radar local maps, integrates pose-graph optimization with gyroscope bias, and achieves superior trajectory accuracy in environments unsuited for vision or lidar methods. Future developments may focus on integrating radar-global map bundle adjustment for enhanced direct localization and extending the method to multi-modal sensor fusion.