FlashICP — GPU point-cloud registration for AUVs

// why build it

The point-cloud → ICP step is the hot path of a visual SLAM frontend.

Large clouds, per-point work, run every keyframe. That is exactly the shape a GPU is built for. FlashICP rebuilds that path in CUDA — partly to learn the core GPU patterns end to end, and partly to produce a result that is genuinely useful and benchmarkable on real data.

It's the bottleneck worth attacking

Correspondence search and the normal-equation build dominate per-keyframe cost. Move them to the GPU and the whole frontend keeps pace with the camera.

Learn the GPU patterns for real

Spatial hashing, parallel reduction, and memory coalescing — implemented against an actual workload instead of a toy kernel.

Every kernel earns its place

Each kernel ships with a CPU baseline and a measured CPU-vs-GPU result on real rosbag data — not a synthetic benchmark, and never a speedup claimed without a baseline behind it. The first sort-based port measured 0.7× — slower than the CPU — and the readout said so; rewriting it as a single-pass atomic hash is what earned the 3.0×.

// the path, end to end

From a rosbag cloud to a relative pose.

One ZED-registered cloud goes in; a 6-DoF transform comes out. Each stage is a GPU kernel with its own CPU baseline — voxel-downsample preprocess and GPU correspondence are implemented; build & solve and pose are the roadmap.

Preprocess

voxel · crop · outlier · normals

Downsample the ZED-registered cloud on a voxel grid, crop, drop outliers, estimate normals.

▸

Correspondence

nearest-neighbor via spatial hash grid

Each source point probes its 27 neighbor cells in a fixed-radius hash grid over the target — CPU-verified, Jetson benchmark next.

▸

Build & solve

6×6 normal equations

Accumulate the linear system with a parallel reduction, then solve for the incremental transform.

▸

Pose

transform T

Emit the relative pose between scans — the frontend measurement a factor-graph backend consumes.

input: rosbag point cloud (ZED registered) → output: pose T for the SLAM graph

// build & run

Three commands. No ROS install required.

Dump a cloud straight from a bag, build CPU-always with CUDA-if-available, then race the two paths.

Dump a cloud from a rosbag no ROS needed

tools/dump_cloud.py

python3 tools/dump_cloud.py /path/to/bag.db3 --out-dir data --max 2
# -> data/cloud0.bin, data/cloud1.bin
# int32 n, then n*(float x, y, z)

Build CUDA auto-disables if there's no nvcc — e.g. on a Mac

cmake

cmake -B build -DUSE_CUDA=ON
cmake --build build

Benchmark CPU vs GPU leaf size 0.05 m · 20 iters

flashicp bench

./build/flashicp bench data/cloud0.bin 0.05 20

// the readout

CPU baseline, GPU kernel, correctness check — every run.

The bench CLI times both paths on the same cloud and verifies the GPU result matches the CPU one. The first kernel in place is the voxel-grid downsample.

bench — Jetson AGX Orin · CUDA 12.6

# loaded 113301 points, leaf=0.050 m, iters=50
CPU voxel downsample: 2.991 ms  -> 1358 voxels
GPU voxel (sort): 4.683 ms  -> 1358 voxels  (0.6x vs CPU)
  check: PASS (matches CPU)
GPU voxel (hash): 0.998 ms  -> 1358 voxels  (3.0x vs CPU)
  check: PASS (matches CPU)

▲Measured on the target hardware against a real Barracuda cloud (zed_20260621_225845). The naive sort-based port loses to the CPU; the single-pass atomic-hash kernel wins 3.0× — full story in docs/jetson_runlog.md.

On Jetson Orin / desktop GPU

Full CPU-vs-GPU timing per kernel, plus the correctness check.

On a Mac · no CUDA

CPU baseline only — still useful to profile the C++ path before it ever touches a GPU.

The optimization arc

First port (thrust sort): 0.7× — overhead-bound at 113k pts → 1.4k voxels. Rewrite as one atomicCAS hash pass with persistent device buffers: 0.998 ms, 3.0× vs CPU, 4.7× vs the sort — output still bit-exact.

// the repository

Five files do the work.

tools/dump_cloud.py

rosbag PointCloud2 → flat x, y, z binary

src/flashicp.hpp

Point type, cloud IO, CPU voxel-downsample baseline

src/voxel_gpu.cu

CUDA voxel downsample — thrust sort baseline + single-pass atomic hash

src/main.cpp

bench CLI: timing plus CPU/GPU correctness check

CMakeLists.txt

CPU-always, CUDA-if-available build