From 21ba476348c19dd1e038f66446c14b857e10f4b6 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Tue, 10 Feb 2026 06:32:35 +0000 Subject: [PATCH] chore:icp draft --- .../.sisyphus/drafts/icp-registration.md | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 py_workspace/.sisyphus/drafts/icp-registration.md diff --git a/py_workspace/.sisyphus/drafts/icp-registration.md b/py_workspace/.sisyphus/drafts/icp-registration.md new file mode 100644 index 0000000..f14ccff --- /dev/null +++ b/py_workspace/.sisyphus/drafts/icp-registration.md @@ -0,0 +1,93 @@ +# 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