chore:icp draft
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user