# 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