Add GPU MegaParticles 6-DoF SE(3) relocalization demo

[rsasaki0109]

Summary

MegaParticles-style 6-DoF / SE(3) Monte Carlo relocalization — the SE(2) MegaParticles demos (gpu_megaparticles_stein_mcl, gpu_megaparticles_lsh) pushed up to full 6-DoF poses. A flying sensor moves through a 3D voxel world; range scans are scored against a GPU 3D Euclidean distance field (built with the JFA-3D from comparison_esdf_3d.cu), particles take quaternion-based Gauss-Newton SE(3) steps (right-perturbation: world-frame translation, body-frame rotation), neighbor-consensus Stein motion pulls the cloud together, and the filter recovers after a hidden kidnap and scan blackout.

The key point: in 6-DoF a dense neighbor grid is combinatorially infeasible, so the explicit p-stable LSH neighbor index from #101 is generalised to the 6-D pose feature (x, y, z, s·rotation-vector) and becomes essential rather than optional — neighbor consensus stays O(L) per particle independent of pose-space dimension. This is the natural continuation of the #86 → #101 localization line.

Two filter paths run side by side: a 64K local bootstrap MCL that tracks well before the kidnap but has no support at the hidden relocated pose, and the 1M-particle 6-DoF MegaParticles path that re-seeds globally uniform over SE(3) during the blackout (the honest "lost → search everywhere" relocalization behaviour) and re-localizes the instant scans return.

This is a repo-sized SE(3) demonstration of the MegaParticles ideas, not a full reproduction of Koide et al.'s system (no GICP distribution-to-distribution scoring, no iterative LSH neighbor lists, no full posterior backend). The honest scope: 6-DoF poses, a 3D distance-field range likelihood, an explicit 6-D p-stable LSH neighbor consensus, and hidden-kidnap relocalization.

Results

Metric	64K local bootstrap	1M 6-DoF MegaParticles
Post-kidnap position RMSE	5.97 m	0.22 m
Final error	6.14 m	0.12 m / 1.9°
Reacquisition after blackout	never	0 frames
Avg GPU step	2.0 ms	13.9 ms

Before the kidnap both paths track tightly (~0.14 m / 2.5°). After the hidden kidnap the local bootstrap stays tied to the stale mode, while the 6-DoF MegaParticles path relocalizes globally over SE(3) as soon as scans return.

Test plan

• cmake .. && make gpu_megaparticles_6dof -j$(nproc) builds clean
• ./bin/gpu_megaparticles_6dof runs end-to-end, writes the GIF
• pre-kidnap tracking ~0.14 m; post-kidnap RMSE local 5.97 m vs mega 0.22 m / 1.9°, 0-frame reacquisition
• git diff --check clean (no whitespace errors)
• GIF ≤ 3 MB (1.6 MB), deployed to gh-pages, URL returns HTTP 200