zed-playground/py_workspace/.sisyphus/drafts/icp-registration.md

Draft: ICP Registration for Multi-Camera Extrinsic Refinement

Requirements (confirmed)

• ICP role: Complement existing RANSAC ground-plane — chain after RANSAC leveling
• Multi-camera strategy: Global pose-graph optimization (pairwise ICP → pose graph)
• Point cloud scope: Near-floor band (floor_y to floor_y + band_height, ~30cm default) — includes slight 3D structure (baseboards, table legs) for better ICP constraints
• DOF constraint: Gravity-constrained — ICP refines yaw + XZ translation + small height; pitch/roll regularized (soft penalty) to preserve RANSAC gravity alignment

Technical Decisions

• Open3D already a dependency — no new deps needed
• Two ICP methods: Point-to-Plane (default) + GICP (optional via --icp-method)
• Voxel downsampling for performance (3-5cm voxel size)
• Reference camera fixed during optimization
• Robust kernel (Tukey/Huber) for outlier rejection
• Colored ICP deferred (requires RGB pipeline plumbing — see analysis below)

Research Findings

• unproject_depth_to_points already exists in aruco/ground_plane.py
• detect_floor_plane already does RANSAC segmentation → can reuse inlier indices for floor filtering
• Open3D registration_icp + PoseGraph + global_optimization = full pipeline
• Multi-scale ICP (coarse→fine voxel) recommended for robustness
• get_information_matrix_from_point_clouds provides edge weights for pose graph
• Existing pipeline: unproject → RANSAC detect → consensus → correct (pitch/roll/Y only)
• ICP addition: after RANSAC correction → extract floor points → pairwise ICP → pose graph → refine all 6 DOF

Resolved Questions

• Overlap detection: Bounding-box overlap check on world XZ projections
• DOF: Full 6-DOF refinement (ICP refines all rotation + translation)
• CLI integration: Flag on refine_ground_plane.py (--icp/--no-icp)
• CLI complexity: Minimal flags + defaults (--icp, maybe --icp-voxel-size, rest uses hardcoded defaults)
• Test strategy: Tests-after (implement ICP, then add tests)

Open Questions

• (none remaining)

Colored ICP Analysis (2025-02-09)

What Colored ICP Does

Open3D's registration_colored_icp (Park et al., ICCV 2017) optimizes a joint objective: E = (1-λ)·E_geom + λ·E_photo where λ_geometric defaults to 0.968. It combines point-to-plane geometric distance with photometric (color) consistency.

When It Helps

• Planar/low-geometry environments: Floor is exactly this — a flat plane where geometric ICP can "slide" along the tangent plane. Color information "locks" the translation along axes where geometry alone is degenerate.
• Sub-millimeter polish: Color provides a dense signal that geometry misses due to depth quantization in stereo cameras.

When It Hurts / Failure Modes

• Lighting inconsistency: If cameras have different auto-exposure/white-balance, the photometric term introduces bias instead of helping.
• Textureless floors: Plain concrete/linoleum floors have near-zero color gradient, making the photometric term useless (falls back to geometric ICP anyway).
• Computational overhead: Requires RGB data, color gradient computation, ~2-3x slower.

Critical Data Pipeline Issue

The current HDF5 depth storage pipeline does NOT save RGB images.

• depth_save.py only stores: pooled_depth, pooled_confidence, intrinsics, raw_frames
• raw_frames only contain depth_map and confidence_map — no image field
• FrameData in svo_sync.py DOES have an image field (BGRA from ZED), but it's discarded when saving to HDF5
• To enable colored ICP, we'd need to:
  1. Extend save_depth_data to also store RGB images (significant HDF5 size increase)
  2. Extend load_depth_data to return images
  3. Modify refine_ground_plane.py to pass images through the pipeline
  4. Create RGBD → colored PointCloud conversion using o3d.geometry.RGBDImage

Recommendation

Defer colored ICP to a future iteration. Reasons:

1. Floor-only scope means we're aligning planar geometry — the exact scenario where point-to-plane ICP is already optimal (when floor HAS texture, colored ICP helps; when it doesn't, colored ICP is equivalent to geometric ICP).
2. Significant plumbing work to save/load/pass RGB through the pipeline.
3. The initial pose from ArUco markers is already very good (~cm accuracy), so ICP only needs to refine by a few mm — well within geometric ICP's capability.
4. Can be added later as an enhancement flag (--icp-method color) without redesigning the core ICP module.
5. If later we expand beyond floor-only to full scene registration, colored ICP becomes much more compelling and worth the investment.

Alternative: Generalized ICP (GICP)

• Purely geometric, no RGB needed — same data pipeline as point-to-plane
• Models local structure as Gaussian distributions ("plane-to-plane")
• More robust than point-to-plane for noisy stereo data
• Available as o3d.pipelines.registration.registration_generalized_icp
• Worth considering as a --icp-method option alongside point-to-plane

Scope Boundaries

• INCLUDE: ICP registration module, pose-graph optimization, CLI integration, tests, docs
• INCLUDE (stretch): GICP as alternative ICP method option (same data pipeline, no extra plumbing)
• EXCLUDE: colored ICP (requires RGB pipeline work — future enhancement)
• EXCLUDE: real-time/streaming ICP