GPU-Accelerated LiDAR Localization (CUDA ICP + ROS 2)

Real-time, map-based LiDAR localization for autonomous vehicles. Incoming LiDAR scans are aligned against a pre-generated point cloud map with a CUDA implementation of the Iterative Closest Point (ICP) algorithm, giving a 6-DoF pose estimate in a few milliseconds per scan.

This is a pure localization node — it does no mapping itself. The map is a PCD file built beforehand with any LiDAR SLAM pipeline, e.g. LIO-SAM or lidarslam_ros2; the localizer loads it once at startup and tracks the vehicle inside it.

Tested end-to-end both on real Ouster LiDAR data and in a Gazebo simulation of an autonomous vehicle (see the demos below). In the simulation (~5,700 points/scan, 50,000-point map on the GPU):

• 5–30 ms per scan while tracking (2–5 ICP iterations) on an RTX 3060 Laptop GPU — comfortably real-time at 10 Hz scan rate.
• Converges from a 0.5 m + 5° off initial guess to ~1 cm accuracy.
• Compared against a CPU-based localizer on the same route, the GPU ICP localizer stayed noticeably more stable, especially through sharp turns.

Demos

In both clips RViz shows the pre-generated map as the height-colored (green) point cloud, the ICP-aligned live scan as the yellow rings and the estimated pose as the red arrow.

Real data — Ouster LiDAR

Live scans from an Ouster sensor, localized against a point cloud map that was generated beforehand from the same environment with a LiDAR SLAM pipeline (LIO-SAM / lidarslam_ros2). The localizer only consumes the pre-built PCD and the raw /ouster/points scans.

In this clip the initial pose is deliberately set wrong — watch the scan snap onto the map almost instantly. Because the ICP scan matching runs on the GPU, each correction costs only a few milliseconds, so the estimate recovers within a scan or two instead of drifting:

Simulation — Gazebo

The Gazebo dataset from the Google Drive link below: a simulated LiDAR on an autonomous vehicle, localizing in the map of the simulation world (gazebo_map.pcd) while the sample rosbag plays:

Repository layout

Path	Description
cuda_icp_localizer/	ROS 2 (Humble) package — the localization node, launch file, RViz config and parameters
src/	CUDA ICP core: correspondence kernels, thrust reductions, SVD (Kabsch) solve
docker/	Docker image (CUDA 12 + ROS 2 Humble) and Compose setup, driven by the top-level Makefile

Requirements

• ROS 2 Humble
• CUDA Toolkit 11+ (tested with 12.x), NVIDIA GPU (sm_86 by default — adjust CUDA_NVCC_FLAGS in cuda_icp_localizer/CMakeLists.txt for your GPU)
• PCL, GLM (libglm-dev), GCC 10 (gcc-10 g++-10)

Map & sample data

The point cloud map (gazebo_map.pcd, ~1.5M points) and a sample LiDAR rosbag recorded in the simulation are hosted on Google Drive:

• Download map + sample bag (Google Drive)

After downloading:

1. Put gazebo_map.pcd into the cuda_icp_localizer/maps/ folder (create the folder if it doesn't exist; it is gitignored).

2. In cuda_icp_localizer/config/icp_localizer_params.yaml point the localizer at the simulation data — the shipped defaults are configured for the real-sensor setup shown in the demo above:

   source_topic: "/lidar_front/points_in"
   target_pcd_file: "install/cuda_icp_localizer/share/cuda_icp_localizer/maps/gazebo_map.pcd"

3. Play the sample rosbag to feed the localizer (it publishes /lidar_front/points_in):

ros2 bag play <path-to-downloaded-bag>

Using your own map (real sensor)

Any pre-generated PCD map works — record a drive with your sensor, build the map offline with a LiDAR SLAM pipeline such as LIO-SAM or lidarslam_ros2, export it as a .pcd, then in cuda_icp_localizer/config/icp_localizer_params.yaml:

• point target_pcd_file at your map,
• set source_topic to your sensor's point cloud topic (the shipped default is /ouster/points, an Ouster sensor),
• set the initial pose (or use RViz's 2D Pose Estimate).

This is exactly the setup shown in the real-data demo above.

Build & run

Docker (recommended)

Requires the NVIDIA Container Toolkit. The container runs with the host network (ROS 2 nodes inside and outside the container see each other directly), GPU access and X11 forwarding for RViz.

make build    # build the image (CUDA 12 + ROS 2 Humble)
make up       # start the container in the background
make colcon   # colcon build inside the container
make shell    # open a shell inside the container
make down     # stop the container

The repository is mounted at /ws inside the container; in-container build artifacts live in named Docker volumes, so they never mix with host builds. Note that paths in icp_localizer_params.yaml (e.g. target_pcd_file) must use container paths — the map placed in cuda_icp_localizer/maps/ is visible inside the container at /ws/cuda_icp_localizer/maps/gazebo_map.pcd. Inside the shell the workspace is already sourced — launch directly:

ros2 launch cuda_icp_localizer icp_localizer.launch.py

A CI workflow builds the image and compiles the package on every push.

Native

cd cuda_icp_localizer
colcon build --symlink-install
source install/setup.bash

ros2 launch cuda_icp_localizer icp_localizer.launch.py

# in another terminal, play the sample bag (publishes /lidar_front/points_in):
ros2 bag play <path-to-downloaded-bag>

By default (use_initial_pose: true) localization starts automatically from the pose configured in the YAML. Set it to false to have the node wait until you provide a pose with RViz's 2D Pose Estimate tool instead. In both modes you can re-initialize at any time from RViz.

RViz opens preconfigured: the map and the ICP-aligned scan as height-colored point clouds and the estimated pose as a red arrow — the same view as in the demo clips above.

Full topic/parameter reference: cuda_icp_localizer/README.md.

How it works

Point-to-point ICP, fully parallelized on the GPU:

1. Correspondences — one CUDA thread per source point does a nearest-neighbour search over the map points.
2. Alignment — means and the cross-covariance matrix are computed with thrust parallel reductions; the rotation comes from a 3×3 SVD (Kabsch / orthogonal Procrustes).
3. Iterate — source points are transformed in place until the incremental motion drops below a convergence threshold.

The ROS 2 node feeds each scan to this core with the previous pose as the initial guess (tracking), composes the accumulated ICP transform with it, rejects implausible jumps, and publishes the pose, the aligned cloud and the map → lidar TF. The map is voxel-downsampled once at startup; live scans are consumed with a keep-last-1, best-effort QoS so the localizer never lags behind the sensor even if an occasional scan takes longer to converge.

Credits

This project builds on thegyro/Project4-Scan-Matching by Srinath Rajagopalan — a CUDA scan matching project from the University of Pennsylvania course CIS 565: GPU Programming and Architecture (Project 4). The CUDA ICP core in src/ originates from that work; see its README for the theory write-up and benchmarks on the Stanford bunny/dragon models.

Additions in this repository:

• ROS 2 Humble integration (cuda_icp_localizer package): live LiDAR input, map loading from PCD, pose/TF publishing, RViz setup
• Accumulated-transform API on the CUDA core (getTransform), convergence early-exit, per-frame GPU memory cleanup
• Hash-grid voxel downsampling for large maps, initial-pose handling (parameters + /initialpose), pose tracking with outlier rejection
• CUDA 12 / GCC 10 build fixes

The 3×3 SVD implementation is svd3 by Eric Jang.