Dr-PoGO: Direct Radar Pose-Graph Optimization

Dr-PoGO is a radar-based SLAM framework (preprint here). It combines Direct Radar Odometry (DRO) with RaPlace loop-closure detection and a pose-graph optimizer to produce globally consistent trajectories from FMCW radar data.

This branch contains a ROS 2 implementation of the full Dr-PoGO pipeline for online operation and real-time visualization in RViz2. The branch offline contains the standalone version of the pipeline for offline processing as used in the original paper.

If you find this code useful, please consider citing our paper:

Dr-PoGO

@inproceedings{legentil2026drpogo,
  title={Dr-PoGO: Direct Radar Pose-Graph Optimization},
    author={{Le Gentil}, Cedric and Weican, Li and Brizi, Leonardo and Barfoot, Timothy D.},
  booktitle={IEEE International Conference on Robotics and Automation (ICRA)},
  year={2026}
}

DRO

@inproceedings{legentil2025dro,
  title={Dro: Doppler-aware direct radar odometry},
  author={{Le Gentil}, Cedric and Brizi, Leonardo and Lisus, Daniil and Qiao, Xinyuan and Grisetti, Giorgio and Barfoot, Timothy D.},
  booktitle={Robotics: Science and Systems (RSS)},
  year={2025}
}

The loop-closure detection module is directly adapted from RaPlace's original code here, so please also consider citing their work.

Architecture overview

Node	Language	Role
boreas_player	Python	Replays a Boreas sequence (radar + IMU) as ROS 2 topics
dro_node	Python	Doppler-aware direct radar odometry
raplace_node	Python	Loop-closure detection using RaPlace
registration_node	Python	Feature-based registration and direct refinement of loop-closure transformations
pogo_node	C++	Pose-graph optimizer (Ceres)

Dependencies

ROS 2 (provided by your ROS 2 installation)

• rclpy, rclcpp
• sensor_msgs, nav_msgs, geometry_msgs, std_msgs
• message_filters
• cv_bridge
• yaml-cpp

C++ libraries (system)

• Ceres Solver
• Eigen3

These can typically be installed via your package manager (e.g., sudo apt install libceres-dev libeigen3-dev on Ubuntu).

Python (pip)

Install all Python dependencies with:

pip install -r requirements.txt

The requirements.txt covers: numpy, pandas, scipy, scikit-learn, scikit-image, opencv-python, matplotlib, PyYAML, pyboreas, torch, torchvision.

Build

First clone this repository into your ROS 2 workspace's src/ directory, then build with:

cd <your_ros2_workspace>/src
git clone git@github.com:utiasASRL/dr_pogo.git

Then build the workspace with:

cd <your_ros2_workspace>
colcon build --packages-select dr_pogo --symlink-install
source install/setup.bash

Usage

1. Prepare the config files

Copy the config file config/config_dro_boreas_rt.yaml to config/config_dro.yaml and edit the parameters as needed. You can also customize the RaPlace, registration, and pose-graph config files if desired (all parameters should have reasonable defaults though).

Not that by default, the registration node is set to use GPU if available. Depending on your hardware and the size of the local maps, this may slow down the processing of DRO's odometry (both nodes can compete for GPU resources). If you want to disable GPU usage for loop registration, simply set use_gpu_if_available: false in the config/config_registration.yaml file. It will be slower to compute the loop registration (using only 1 CPU thread), but DRO's behaviour will be more predictable.

2. Launch the full pipeline

ros2 launch dr_pogo dr_pogo.launch.py

This starts all four estimation nodes plus an RViz2 visualizer with the bundled config/rviz.rviz preset.

Note: The DRO code will attempt to leverage torch compilation if the config/config_dro.yaml file contains the following parameters:

radar:
  nb_azimuths: XXX
  doppler_enabled: true/false
  resolution: Y.YYYY

If you chose to enable compilation, the initialization of the dro_node will take significantly longer (something like 30sec?). When the comilation is done and DRO is ready to process data, you should see the following message in the terminal:

If you want to disable compilation, simply remove one of the above parameters from the config file (e.g., resolution).

3. Play a Boreas sequence

ros2 run dr_pogo boreas_player -p <path_to_sequence> -r <playback_rate>
# Example:
ros2 run dr_pogo boreas_player -p /data/boreas/boreas-2024-12-03-12-54 -r 1.0

You can also make it play as fast as DRO allows by setting -r 0 (preventing to wait between messages if your hardware is fast enough to process the data faster than real-time, and allows for slower hardware to keep up by slowing down the playback rate as needed).

Configuration

All YAML config files live under config/.

File	Node	Key parameters
config_dro.yaml	dro_node	Sensor extrinsics (T_axle_radar), range limits, GP lengthscales
config_raplace.yaml	raplace_node	min_time_diff, max_odom_drift, max_img_size
config_registration.yaml	registration_node	lowe_ratio, ransac_thr, max_img_size, use_gpu_if_available
config_pogo.yaml	pogo_node	Odometry/loop noise std-devs, loss scales, estimate_bias

Custom messages

Message	Fields
RadarInfo	Radar scan metadata (timestamps, frequency, etc.)
LocalMapInfo	Accumulated local map header, resolution, 2-D pose (x, y, theta)
LoopCandidate	Query/candidate timestamps, match score, image paths, local map resolution

Output

Atop ROS2 topics shown in RViz, Dr-PoGO outputs the odometry and pose-graph optimized trajectories in files in an output directory specified in the launch file (default is in the install space under <ros2_ws>/install/dr_pogo/share/dr_pogo/<sequence_id>/):

• odometry_result/<sequence_id>.txt: DRO odometry trajectory using the Boreas format.
• pose_graph_traj.txt: Pose-graph optimized trajectory in with timestamp(us) x y theta format.

TODOs

• Making the dense loop registration refinement faster !! (need to profile first)
• Improving documentation
• Looking at making DRO even faster? (real time on my RTX 500 Mobile GPU (30W), so not a priority)
• Adding the 3D odometry output as for 3DRO