Dr-PoGO: Direct Radar Pose-Graph Optimization

• Dr-PoGO is a radar SLAM framework that directly registers 2D FMCW radar intensities to estimate accurate poses in challenging weather conditions.
• It combines direct radar odometry, radar-specific place recognition, and coarse-to-fine loop closure to achieve reliable localization over large-scale environments.
• The system leverages local map filtering and robust pose-graph optimization to significantly reduce drift and improve loop closure consistency.

Dr-PoGO, short for Direct Radar Pose-Graph Optimization, is a radar-based Simultaneous Localization And Mapping (SLAM) system for a 2D spinning FMCW radar that combines direct radar odometry (DRO), radar-specific place recognition, coarse-to-fine loop-closure registration, and pose-graph optimization to estimate a planar trajectory $$\{{}^W\mathbf{T}_{t_m} \in \mathrm{SE}(2)\}_{m=1}^N$$ in automotive-scale environments (Gentil et al., 6 May 2026). The method is designed around the observation that millimetre-wave radar remains operational in dust, falling snow, rain, and related adverse conditions where cameras and lidars degrade, while avoiding the fragility of feature-only radar SLAM pipelines by operating directly on radar intensities and temporally filtered local maps (Gentil et al., 6 May 2026).

1. Definition, sensing model, and motivation

Dr-PoGO addresses SLAM with a 2D mechanically spinning FMCW radar (Navtech RAS6), optionally aided by a yaw gyroscope, and targets large-scale real-world trajectories including suburbs, industrial zones, skyways, forests, and farms (Gentil et al., 6 May 2026). Its state estimate is planar, with poses in $\mathrm{SE}(2)$, and its central architectural choice is to use direct registration rather than relying exclusively on point-cloud extraction or handcrafted radar features (Gentil et al., 6 May 2026).

The motivation is tied to the sensing characteristics of mm-wave radar. Compared with cameras and lidars, radar is described as robust to adverse weather, insensitive to lighting, and available in mature automotive hardware with long range and $360^\circ$ coverage (Gentil et al., 6 May 2026). At the same time, radar SLAM is difficult because radar returns are sparse, noisy, and speckled; the structural signal is weak relative to lidar; and a mechanically spinning scan is acquired over time rather than instantaneously, inducing motion distortion and Doppler-related distortion (Gentil et al., 6 May 2026). The paper positions Dr-PoGO as a response to these difficulties by preserving more of the raw intensity information and by using local maps that suppress transient effects (Gentil et al., 6 May 2026).

This suggests that Dr-PoGO is best understood not as a minor variant of feature-based radar SLAM, but as a system-level reorganization of the radar SLAM stack around direct, intensity-based objectives.

2. System architecture

The pipeline consists of six stages: raw radar input, direct radar odometry, radar place recognition, coarse loop-closure registration, fine direct registration, and pose-graph optimization (Gentil et al., 6 May 2026). The data flow is explicitly modular.

First, raw 2D spinning radar data in polar form, together with an optional yaw gyro, enters the odometry front end (Gentil et al., 6 May 2026). The front end is DRO, treated as an external direct radar odometry module, which produces scan-to-scan relative motions ${}^{t_m}\mathbf{T}_{t_{m+1}}$ and maintains a local radar map $M_m$ in Cartesian form through continuous-time scan-to-map registration (Gentil et al., 6 May 2026).

Second, RaPlace performs radar place recognition on these local maps rather than on raw scans (Gentil et al., 6 May 2026). RaPlace uses a Radon transform + FFT-based descriptor and produces loop-closure candidates as pairs of local maps $(M_i, M_j)$ with similarity scores, but it does not estimate a relative transform (Gentil et al., 6 May 2026).

Third, each candidate pair is converted into a metric loop-closure constraint through a coarse-to-fine registration procedure (Gentil et al., 6 May 2026). The coarse stage uses SIFT features, descriptor matching, and RANSAC in $\mathrm{SE}(2)$ to generate an initial transform; the fine stage then refines that initialization by maximizing a direct cross-correlation objective over local-map intensities (Gentil et al., 6 May 2026).

Finally, odometry edges and validated loop-closure edges are assembled into a full-batch SE(2) pose graph, optionally augmented with gyro bias states, and optimized with a robust loss on loop closures (Gentil et al., 6 May 2026).

A defining feature of the architecture is the central role of local maps. These maps are low-pass filtered in time and therefore attenuate dynamic objects and noise, which improves both place recognition and registration robustness (Gentil et al., 6 May 2026).

3. Direct radar odometry and local-map representation

Dr-PoGO inherits its odometry front end from DRO, described in the paper as a continuous direct method for scan-to-local-map registration that explicitly models motion distortion and Doppler effects during the azimuth sweep using a continuous-time motion model (Gentil et al., 6 May 2026). The paper does not re-derive DRO in full, but treats it as essential to the system’s performance.

The radar measurements originate as polar intensity images. DRO maintains a local map $M_m$ in Cartesian coordinates, and each incoming scan contributes through a continuous cross-correlation score while the map is updated online (Gentil et al., 6 May 2026). Local maps are described as per-pixel low-pass filtered intensity maps, emphasizing persistent static structure rather than transient returns (Gentil et al., 6 May 2026).

For each consecutive scan pair, DRO estimates the relative motion

${}^{t_m}\mathbf{T}_{t_{m+1}} \in \mathrm{SE}(2)$

by maximizing a continuous cross-correlation measure between the scan and the current local map (Gentil et al., 6 May 2026). The implementation reused in Dr-PoGO is gradient-based and runs in PyTorch on GPU (Gentil et al., 6 May 2026).

Within the overall system, DRO serves two functions simultaneously. It supplies the odometry edges for the graph, and it constructs the local maps that later support place recognition and loop-closure registration (Gentil et al., 6 May 2026). This coupling is important: the same direct-registration machinery that stabilizes odometry also produces the representation used for global consistency.

4. Loop closure: from place recognition to direct refinement

Loop closure in Dr-PoGO is intentionally hybrid. RaPlace is used for candidate generation, but the final metric constraint is obtained by feature-based coarse alignment followed by direct refinement (Gentil et al., 6 May 2026).

RaPlace operates on selected keyframe local maps and computes descriptors using the Radon transform and FFTs (Gentil et al., 6 May 2026). For each new keyframe, it searches previous keyframes within a radius determined by expected odometry drift (approximately $1\%$ of traveled distance) and returns the single best past map without thresholding descriptor similarity, with later filtering delegated to metric registration (Gentil et al., 6 May 2026). This maximizes recall at the candidate stage (Gentil et al., 6 May 2026).

Because RaPlace does not provide a relative transformation and because direct registration requires a reasonable initialization, Dr-PoGO introduces a coarse stage based on SIFT and RANSAC. Two Cartesian local maps $\mathrm{SE}(2)$0 and $\mathrm{SE}(2)$1 are processed as follows (Gentil et al., 6 May 2026):

• SIFT keypoints and descriptors are extracted.
• Descriptors are brute-force matched.
• OpenCV’s RANSAC-based solver estimates

$\mathrm{SE}(2)$2

• The pair is rejected if the estimated scale deviates from $\mathrm{SE}(2)$3 by more than $\mathrm{SE}(2)$4 (Gentil et al., 6 May 2026).

The fine stage then refines the transform via direct registration by maximizing

$\mathrm{SE}(2)$5

with

$\mathrm{SE}(2)$6

Here $\mathrm{SE}(2)$7 is evaluated by bilinear interpolation in Cartesian coordinates (Gentil et al., 6 May 2026).

Because the raw correlation magnitude depends strongly on scene energy, Dr-PoGO uses the scaled cross-correlation

$\mathrm{SE}(2)$8

A loop closure is accepted only if $\mathrm{SE}(2)$9 (Gentil et al., 6 May 2026). The paper reports that this refinement increases inlier ratios from approximately 39–78% to 94–99%, reduces inlier position RMSE from approximately 0.38–0.51 m to 0.27–0.36 m, and reduces rotation RMSE from approximately 0.26° to 0.17–0.21° (Gentil et al., 6 May 2026).

A plausible implication is that the coarse stage is not merely a convenience for initialization; it is what makes direct loop closure practical under large accumulated drift.

5. Pose-graph formulation and optimization

The back end is a pose graph whose nodes are the world-frame poses

$360^\circ$0

and, optionally, per-scan yaw gyro biases $360^\circ$1 (Gentil et al., 6 May 2026). The edge set contains odometry constraints between successive scans and loop-closure constraints from the refined local-map registrations (Gentil et al., 6 May 2026).

For a loop closure between nodes $360^\circ$2 and $360^\circ$3, the predicted relative transform is

$360^\circ$4

and the residual is

$360^\circ$5

with $360^\circ$6 giving the minimal $360^\circ$7 parameterization (Gentil et al., 6 May 2026). The odometry residual is defined analogously (Gentil et al., 6 May 2026).

When a yaw gyroscope is present, Dr-PoGO can also optimize bias states. The DRO scan-to-scan estimate is corrected by

$360^\circ$8

with

$360^\circ$9

and a bias smoothness residual

${}^{t_m}\mathbf{T}_{t_{m+1}}$0

under a Brownian-motion prior (Gentil et al., 6 May 2026).

The total optimization problem is

${}^{t_m}\mathbf{T}_{t_{m+1}}$1

where ${}^{t_m}\mathbf{T}_{t_{m+1}}$2 is a Cauchy robust loss applied to loop closures (Gentil et al., 6 May 2026). Optimization is triggered whenever a new loop closure is added and is run first with a large Cauchy scale and then with a smaller scale (Gentil et al., 6 May 2026).

This structure places Dr-PoGO within the standard Lie-group pose-graph tradition, but its loop-closure measurements are unusual in that they are produced by a direct radar correlation objective rather than by point-cloud alignment or descriptor-only matching.

6. Empirical performance, implementation, and context

Dr-PoGO is reported to demonstrate state-of-the-art performance over 300 km of data in real-world automotive environments (Gentil et al., 6 May 2026). The evaluation uses Boreas and Boreas-RT, with ground truth from GNSS/INS and lidar-based mapping (Gentil et al., 6 May 2026). The metrics are Absolute Trajectory Error (ATE) after ${}^{t_m}\mathbf{T}_{t_{m+1}}$3 alignment and End-Pose Error (EPE) for loop consistency (Gentil et al., 6 May 2026).

On Boreas across 13 sequences totaling approximately 225 km, Dr-PoGO achieved mean ATE ${}^{t_m}\mathbf{T}_{t_{m+1}}$4 m and max ATE ${}^{t_m}\mathbf{T}_{t_{m+1}}$5 m, outperforming the radar baselines TBV-SLAM and Navtech-SLAM on both ATE and EPE (Gentil et al., 6 May 2026). On Boreas-RT, Dr-PoGO was the only radar SLAM system reported to succeed on the most challenging Skyway, Forest, and Farm sequences (Gentil et al., 6 May 2026).

Environment	Dr-PoGO ATE / EPE	Outcome
Suburbs	0.75 m / 0.42 m	No failures
Industrial	1.58 m / 0.77 m	No failures
Skyway	3.16 m / 0.23 m	No failures
Forest	4.31 m / 0.75 m	No failures
Farm	4.19 m / 0.56 m	No failures

For context, on the Suburbs route the paper reports Fast-LIO2: 13.82 m, 2Fast-2Lamaa: 2.26 m, and Dr-PoGO: 0.75 m ATE (Gentil et al., 6 May 2026). The comparison is notable because the lidar baselines are lidar-inertial scan-to-map systems, whereas Dr-PoGO uses only 2D radar and a yaw gyro (Gentil et al., 6 May 2026).

The ablations identify the dominant performance factors. Replacing local maps with scan-based inputs increases errors substantially; skipping direct loop-refinement worsens ATE and causes failures; removing gyro bias estimation has only a small effect; and removing the gyro entirely degrades performance sharply, with complete failure on Skyway (Gentil et al., 6 May 2026). The paper states that radar-only odometry without yaw is not yet robust enough for large-scale SLAM in these scenarios (Gentil et al., 6 May 2026).

Implementation details reinforce the system-level design. On a laptop with an Intel i7-13850HX and NVIDIA RTX 5000 Mobile, the reported timings are 0.104 s per scan for DRO, 0.646 s per detection for RaPlace on one CPU core, 0.170 s per operation for coarse registration, 0.389 s per operation for direct fine registration, and 0.093 s per optimization for the pose graph, yielding a summed per-frame cost below 0.25 s, which satisfies real-time operation at 4 Hz (Gentil et al., 6 May 2026). The implementation is publicly available at the URL given in the paper (Gentil et al., 6 May 2026).

A common misconception is that “Dr-PoGO” refers generically to any “PoGO” method. In the literature represented here, that is not the case. Dr-PoGO is the radar SLAM method described above (Gentil et al., 6 May 2026); PoGO also names a balloon-borne hard X-ray polarimeter family (Chauvin et al., 2016) and a blockchain protocol, “Proof of Gradient Optimization” (Orlicki, 10 Apr 2025), while DRPO denotes “Decoupled Reward Policy Optimization” for reasoning models (Li et al., 6 Oct 2025). The shared acronymal structure is nominal rather than methodological.

Overall, Dr-PoGO is positioned as the first radar SLAM framework using direct registration both for odometry and loop closures, with a hybrid loop-closure front end that uses SIFT+RANSAC only to obtain an initialization for direct refinement (Gentil et al., 6 May 2026). The results suggest that, for 2D spinning radar, direct intensity-based objectives combined with local-map statistics and robust pose-graph optimization are sufficient to reach, and in some settings surpass, strong lidar-inertial baselines (Gentil et al., 6 May 2026).