Add GPU online 3D SLAM with switchable loop constraints

[rsasaki0109]

Summary

Online 3D SLAM with a switchable-constraint sliding-window back-end — the other 3D SLAM follow-up flagged in the handoff. It fuses two existing demos into the harder online setting:

• Add GPU online SLAM (sliding-window pose-graph backend) #63 gpu_online_slam gave a 2D sliding-window pose-graph optimiser that runs as frames stream in (+ iSAM-style global pass on loop), but assumed every detected loop was correct.
• Add GPU switchable-constraint 3D pose-graph SLAM demo #98 gpu_pose_graph_slam_3d_switchable gave a full SE(3) back-end that rejects bad loops with per-loop switch variables (Sünderhauf & Protzel, IROS 2012), but as a one-shot batch solve over a frozen graph.

The interesting and honest case is the combination: a robot streams poses, and true and false loop closures arrive incrementally — there is no chance to look at the whole graph and trim the worst loops after the fact. Each frame the per-loop switches are re-minimised in closed form alongside a sliding-window SE(3) Gauss-Newton solve, so a false loop is switched off the moment its residual blows up and a true loop earns its weight back over a few iterations. A global pass propagates a good loop's correction across the whole trajectory when one fires.

Two back-ends run lockstep on the same streamed edge set, differing only in whether switches are active:

• plain online — every loop weighted 1; false loops yank the live estimate as they arrive.
• switchable online — per-loop switches optimised live; false loops rejected as they appear.

Results

Metric	plain online	switchable online
Final translation RMSE (420 poses)	9.10 m	0.29 m
False loops rejected live	—	21 / 21
Final loop switch weight (clean / false)	—	1.000 / 0.000
Avg step time (both back-ends, W=80, GN=4, PCG=22)	~16 ms

420 streamed SE(3) poses, sliding window W=80 + global pass on loop, 213 true loops (GT proximity) and 21 gross false loops injected on a schedule.

Implementation notes

• Lifts Add GPU switchable-constraint 3D pose-graph SLAM demo #98's SE(3) GN + Jacobi-PCG + 6×6 Cholesky preconditioner + closed-form damped switch update into the windowed scheme of Add GPU online SLAM (sliding-window pose-graph backend) #63: active_lo/active_hi masking in assemble / matvec / preconditioner, per-window anchor pin (anchor_pose_kernel), edges to fixed poses contribute as constants.
• Switch update is asymmetric (off fast, on slow) so a false loop is killed before the graph bends to accommodate it.
• GPU-side SE(3) update with a per-iteration step clamp (no host line search in the streaming loop); LM damping keeps the windowed solve stable.
• Single file src/gpu_online_slam_3d_switchable.cu. Reuses cuda_check.cuh, cuda_blas.cuh, cuda_video.h.

Test plan

• cmake .. && make gpu_online_slam_3d_switchable — builds clean (no warnings)
• ./bin/gpu_online_slam_3d_switchable runs end-to-end (~10 s), prints plain 9.10 m vs switchable 0.29 m, 21/21 false loops rejected
• GIF ≤ 3 MB (2.83 MB), deployed to gh-pages, returns HTTP 200
• git diff --check clean; squash diff touches only its four files (CMakeLists.txt, plan.md, readme.md, src/gpu_online_slam_3d_switchable.cu)