chore: update beads and sisyphus tracking

2026-02-10 15:32:10 +00:00
parent 25733af4ec
commit 1d2e66a34c
12 changed files with 1104 additions and 0 deletions
@@ -0,0 +1,889 @@
+# Full-Scene ICP Pipeline Upgrade
+
+## TL;DR
+
+> **Quick Summary**: Upgrade the current floor-band-only ICP registration to support hybrid (floor + vertical structure) and full-scene modes, with optional FPFH+RANSAC global pre-alignment, fixed success gates, robust kernels, and per-pair diagnostic logging.
+> 
+> **Deliverables**:
+> - `--icp-region floor|hybrid|full` CLI flag (default: hybrid)
+> - `--icp-global-init` optional FPFH+RANSAC pre-alignment
+> - Fixed success gate (`>0` instead of `>1`)
+> - Per-pair diagnostic logging (all pairs, not just converged)
+> - Statistical outlier removal preprocessing
+> - TukeyLoss robust kernel support
+> - 3D AABB overlap check for hybrid/full modes
+> - Exposed CLI flags: `--icp-min-overlap`, `--icp-band-height`
+> - Relaxed defaults: gravity penalty, correspondence distance factor, min_overlap_area
+> - Comprehensive tests covering all new modes + edge cases
+> 
+> **Estimated Effort**: Medium (3–5 days)
+> **Parallel Execution**: YES - 3 waves
+> **Critical Path**: Task 1 → Task 2 → Task 3 → Task 5 → Task 7 → Task 9
+
+---
+
+## Context
+
+### Original Request
+User observed that camera SN44289123 appears floor-disconnected by ~3–5 cm after ground-plane refinement. Diagnostic sweeps showed ICP consistently failing: only 1 of 6 pairs converges, graph stays disconnected, and the success gate (`>1`) rejects valid single-camera corrections. Oracle analysis identified floor-band degeneracy (planar sliding), tight gating, and over-aggressive gravity penalty as root causes. User requested upgrading from floor-only to full-scene/hybrid ICP.
+
+### Interview Summary
+**Key Discussions**:
+- User chose **hybrid** as default ICP region mode (floor + vertical structure)
+- User chose **FPFH+RANSAC** as optional global pre-alignment (`--icp-global-init`)
+- User chose to **include success gate fix** in this plan (not a separate patch)
+- Oracle recommended: lower gravity penalty (10→2), widen correspondence factor (1.4→2.5), lower min_overlap_area (1.0→0.5), add per-pair logging, add robust kernels
+
+**Research Findings**:
+- Open3D multiway registration: pairwise ICP → pose graph → global optimization (LM)
+- FPFH features + RANSAC for global pre-alignment when init is poor
+- Statistical outlier removal (nb_neighbors=20, std_ratio=2.0) standard for ZED
+- TukeyLoss(k=0.05–0.1) effective for depth sensor noise at 3–5 m range
+- Hybrid floor+structure recommended over pure floor (adds XZ constraints)
+- Voxel size: 0.05 m for global/coarse, 0.02 m for fine refinement
+
+### Metis Review
+**Identified Gaps** (addressed):
+- Backward compatibility: `--icp-region floor` preserves legacy behavior exactly
+- Hybrid degeneracy: if no vertical structure found, gracefully falls back to floor-only with warning
+- FPFH failure fallback: when RANSAC fails, continues with extrinsic-based init (no crash)
+- Determinism: RANSAC seeds controlled for reproducibility; tests use synthetic geometry
+- Ceiling/overhang dominance in full mode: gravity constraint prevents upside-down alignment
+- Symmetric environments: transform sanity bounds catch FPFH mis-registration
+
+---
+
+## Work Objectives
+
+### Core Objective
+Replace the floor-band-only ICP pipeline with a configurable region selection system (floor/hybrid/full) that provides stronger geometric constraints, better pair convergence, and more reliable multi-camera correction.
+
+### Concrete Deliverables
+- Modified `aruco/icp_registration.py`: new extraction functions, 3D overlap, robust kernel, relaxed defaults, diagnostic logging
+- Modified `refine_ground_plane.py`: new CLI flags for region, global-init, overlap, band-height
+- New/extended `tests/test_icp_registration.py`: ~15 new test cases
+- Updated `README.md`: new flags documented
+
+### Definition of Done
+- [ ] `uv run refine_ground_plane.py --help` shows `--icp-region`, `--icp-global-init`, `--icp-min-overlap`, `--icp-band-height`
+- [ ] `uv run pytest -x -vv` → all tests pass (existing + new)
+- [ ] `uv run basedpyright aruco/icp_registration.py refine_ground_plane.py` → 0 errors
+- [ ] `--icp-region floor` produces identical output to current behavior (regression)
+- [ ] `--icp-region hybrid` produces ≥ as many converged pairs as floor on test data
+
+### Must Have
+- Region selection: `floor`, `hybrid`, `full` modes
+- Backward compatibility: `floor` mode matches current behavior
+- Success gate: `num_cameras_optimized > 0` (not `> 1`)
+- Per-pair diagnostic logging at INFO level
+- Robust kernel (TukeyLoss) support
+- Statistical outlier removal preprocessing
+- 3D AABB overlap check for hybrid/full
+- Optional FPFH+RANSAC global init with clean fallback
+
+### Must NOT Have (Guardrails)
+- No changes to HDF5 schema or `aruco/depth_save.py`
+- No additional global registration methods beyond FPFH+RANSAC
+- No auto-parameter search or adaptive voxel size loops
+- No multiple outlier strategies (SOR only, no radius outlier removal)
+- No multiple robust kernels (TukeyLoss only, no Huber/Cauchy menus)
+- No refactoring of pose graph optimization or information matrix computation
+- No new persistent output files beyond normal console logging
+- No changes to `aruco/ground_plane.py` core functions (only consume them)
+
+---
+
+## Verification Strategy
+
+> **UNIVERSAL RULE: ZERO HUMAN INTERVENTION**
+>
+> ALL tasks in this plan MUST be verifiable WITHOUT any human action.
+
+### Test Decision
+- **Infrastructure exists**: YES
+- **Automated tests**: YES (Tests-after for new functionality)
+- **Framework**: pytest
+
+### Agent-Executed QA Scenarios (MANDATORY — ALL tasks)
+
+**Verification Tool by Deliverable Type:**
+
+| Type | Tool | How Agent Verifies |
+|------|------|-------------------|
+| **CLI flags** | Bash | `uv run refine_ground_plane.py --help` and grep for flags |
+| **Library/Module** | Bash (pytest) | `uv run pytest tests/test_icp_registration.py -v` |
+| **Type safety** | Bash (basedpyright) | `uv run basedpyright aruco/icp_registration.py` |
+| **Regression** | Bash (pipeline run) | Compare outputs with floor-only baseline |
+
+---
+
+## Execution Strategy
+
+### Parallel Execution Waves
+
+```
+Wave 1 (Start Immediately):
+├── Task 1: Fix success gate + per-pair diagnostic logging
+├── Task 2: Add point extraction functions (hybrid/full/SOR)
+└── Task 3: Add 3D AABB overlap check
+
+Wave 2 (After Wave 1):
+├── Task 4: Add TukeyLoss robust kernel support
+├── Task 5: Integrate region selection into refine_with_icp
+└── Task 6: Add FPFH+RANSAC global pre-alignment
+
+Wave 3 (After Wave 2):
+├── Task 7: Wire CLI flags in refine_ground_plane.py
+├── Task 8: Relax ICPConfig defaults
+└── Task 9: Tests + README + regression verification
+
+Critical Path: Task 1 → Task 2 → Task 5 → Task 7 → Task 9
+Parallel Speedup: ~40% faster than sequential
+```
+
+### Dependency Matrix
+
+| Task | Depends On | Blocks | Can Parallelize With |
+|------|------------|--------|---------------------|
+| 1 | None | 5, 9 | 2, 3 |
+| 2 | None | 5 | 1, 3 |
+| 3 | None | 5 | 1, 2 |
+| 4 | None | 5 | 1, 2, 3 |
+| 5 | 1, 2, 3, 4 | 7 | 6 |
+| 6 | None | 7 | 4, 5 |
+| 7 | 5, 6 | 9 | 8 |
+| 8 | None | 9 | 7 |
+| 9 | 7, 8 | None | None (final) |
+
+### Agent Dispatch Summary
+
+| Wave | Tasks | Recommended Agents |
+|------|-------|-------------------|
+| 1 | 1, 2, 3 | task(category="quick") for Task 1; task(category="unspecified-high") for Tasks 2, 3 |
+| 2 | 4, 5, 6 | task(category="unspecified-high") for all three |
+| 3 | 7, 8, 9 | task(category="unspecified-high") for Task 7, task(category="quick") for Task 8, task(category="unspecified-high") for Task 9 |
+
+---
+
+## TODOs
+
+- [x] 1. Fix success gate + add per-pair diagnostic logging
+
+  **What to do**:
+  - Change `metrics.success = metrics.num_cameras_optimized > 1` to `metrics.success = metrics.num_cameras_optimized > 0` (line 475)
+  - Log ALL pair outcomes (not just converged) at INFO level: fitness, RMSE, converged status
+  - Always record to `metrics.per_pair_results` (currently only converged pairs are stored, line 417)
+  - Add `logger.info(f"Pair ({s1},{s2}): fitness={result.fitness:.3f}, rmse={result.inlier_rmse:.4f}, converged={result.converged}")` after each pairwise ICP
+  - Add `logger.debug(f"Pair ({s1},{s2}) overlap {area:.2f} m² < {config.min_overlap_area}. Skipping.")` before overlap skip
+
+  **Must NOT do**:
+  - Do not change pairwise_icp algorithm or pose graph logic
+  - Do not modify ICPConfig defaults (separate task)
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+    - Reason: Small, focused change — two lines of logic + logging additions
+  - **Skills**: []
+  - **Skills Evaluated but Omitted**:
+    - `git-master`: No git operations needed during implementation
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES
+  - **Parallel Group**: Wave 1 (with Tasks 2, 3)
+  - **Blocks**: Tasks 5, 9
+  - **Blocked By**: None
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:414-421` — Current convergence check and pair_results storage (change to always store)
+  - `aruco/icp_registration.py:475` — Success gate line to modify
+  - `aruco/icp_registration.py:382-386` — Overlap skip without logging (add debug log)
+
+  **API/Type References**:
+  - `aruco/icp_registration.py:48-59` — ICPMetrics dataclass (per_pair_results field)
+  - `aruco/icp_registration.py:37-45` — ICPResult dataclass (fitness, inlier_rmse, converged fields)
+
+  **WHY Each Reference Matters**:
+  - Line 475: the exact success gate that Oracle identified as too strict
+  - Lines 414-421: where pair results are conditionally stored; need to always store for diagnostics
+  - Lines 382-386: overlap rejection currently silent; need logging for operator debugging
+
+  **Acceptance Criteria**:
+
+  - [ ] `metrics.success` is `True` when `num_cameras_optimized == 1`
+  - [ ] All pair results appear in `metrics.per_pair_results` regardless of convergence
+  - [ ] Log output at INFO shows fitness/RMSE for every attempted pair
+  - [ ] `uv run pytest tests/test_icp_registration.py -v` → all existing tests pass
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Success gate accepts single-camera optimization
+    Tool: Bash (pytest)
+    Preconditions: test_icp_registration.py has test_success_gate_single_camera
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "success_gate" -v
+      2. Assert: exit code 0
+      3. Assert: output contains "PASSED"
+    Expected Result: Test passes confirming success=True when 1 camera optimized
+    Evidence: pytest output captured
+
+  Scenario: Per-pair logging visible in debug output
+    Tool: Bash (pipeline run)
+    Preconditions: output/e2e_refine_depth.json and .h5 exist
+    Steps:
+      1. uv run refine_ground_plane.py --input-extrinsics output/e2e_refine_depth.json --input-depth output/e2e_refine_depth.h5 --output-extrinsics /tmp/test_logging.json --icp --icp-method gicp --icp-voxel-size 0.04 --seed 42 --debug
+      2. Assert: stdout contains "fitness=" for multiple pairs
+      3. Assert: stdout contains "overlap" for skipped pairs
+    Expected Result: All 6 pairs produce diagnostic output
+    Evidence: Terminal output captured
+  ```
+
+  **Commit**: YES
+  - Message: `fix(icp): relax success gate to >0 and add per-pair diagnostic logging`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 2. Add point extraction functions (hybrid/full/SOR)
+
+  **What to do**:
+  - Add `extract_scene_points(points_world, floor_y, floor_normal, mode, band_height)` function to `aruco/icp_registration.py`
+    - `mode="floor"`: delegate to existing `extract_near_floor_band` (backward compat)
+    - `mode="hybrid"`: floor band UNION with points whose surface normals are near-horizontal (walls/vertical structure). Use Open3D normal estimation on the full cloud, then filter by `abs(normal · floor_normal) < 0.3` for "vertical" points. Combine with floor band.
+    - `mode="full"`: return all points after SOR filtering
+  - Add `preprocess_point_cloud(pcd, voxel_size)` function:
+    - Statistical outlier removal: `pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)`
+    - Return cleaned point cloud
+  - Add `region: str = "floor"` field to `ICPConfig` dataclass (values: "floor", "hybrid", "full")
+  - If hybrid mode finds no vertical structure points, log warning and fall back to floor-only points
+
+  **Must NOT do**:
+  - Do not modify `extract_near_floor_band` (keep it as-is for floor mode)
+  - Do not add multiple outlier strategies
+  - Do not change ground_plane.py
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-high`
+    - Reason: New extraction logic with mode dispatch, normal-based filtering, and SOR integration
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES
+  - **Parallel Group**: Wave 1 (with Tasks 1, 3)
+  - **Blocks**: Task 5
+  - **Blocked By**: None
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:62-82` — `extract_near_floor_band` (floor mode delegate, keep unchanged)
+  - `aruco/icp_registration.py:20-34` — `ICPConfig` dataclass (add `region` field here)
+
+  **API/Type References**:
+  - `aruco/ground_plane.py:114-145` — `detect_floor_plane` returns `FloorPlane(normal, d)` — used for floor_y and floor_normal inputs
+  - Open3D `remove_statistical_outlier(nb_neighbors, std_ratio)` — returns (cleaned_pcd, indices)
+  - Open3D `estimate_normals(KDTreeSearchParamHybrid(radius, max_nn))` — for hybrid vertical detection
+
+  **External References**:
+  - Open3D SOR: https://www.open3d.org/docs/release/tutorial/geometry/pointcloud.html#Statistical-outlier-removal
+
+  **WHY Each Reference Matters**:
+  - Lines 62-82: the existing extraction function that floor mode must delegate to unchanged
+  - ICPConfig: where `region` field lives so `refine_with_icp` can dispatch
+  - Open3D SOR API: exact function signature for outlier removal
+
+  **Acceptance Criteria**:
+
+  - [ ] `extract_scene_points` exists and dispatches correctly for all 3 modes
+  - [ ] `preprocess_point_cloud` applies SOR and returns cleaned cloud
+  - [ ] `ICPConfig.region` field exists with default `"floor"`
+  - [ ] Floor mode produces identical output to `extract_near_floor_band`
+  - [ ] Hybrid mode includes vertical-normal points when present
+  - [ ] Hybrid mode falls back to floor-only with warning when no vertical points
+  - [ ] `uv run basedpyright aruco/icp_registration.py` → 0 errors
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Floor mode identical to legacy
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "floor_mode_legacy" -v
+      2. Assert: exit code 0
+    Expected Result: Floor extraction unchanged
+    Evidence: pytest output
+
+  Scenario: Hybrid includes vertical structure
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "hybrid_vertical" -v
+      2. Assert: exit code 0
+    Expected Result: Hybrid returns more points than floor-only when walls present
+    Evidence: pytest output
+
+  Scenario: Hybrid fallback when no vertical structure
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "hybrid_fallback" -v
+      2. Assert: exit code 0
+    Expected Result: Falls back to floor-only points, logs warning
+    Evidence: pytest output
+  ```
+
+  **Commit**: YES
+  - Message: `feat(icp): add hybrid/full point extraction with SOR preprocessing`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 3. Add 3D AABB overlap check
+
+  **What to do**:
+  - Add `compute_overlap_3d(points_a, points_b, margin)` function to `aruco/icp_registration.py`
+    - Compute 3D axis-aligned bounding box intersection volume
+    - Return volume in m³
+  - Add `overlap_mode: str = "xz"` field to `ICPConfig` (values: "xz", "3d")
+    - `"xz"`: use existing `compute_overlap_xz` (backward compat for floor mode)
+    - `"3d"`: use new `compute_overlap_3d` (for hybrid/full modes)
+  - Keep `compute_overlap_xz` unchanged
+
+  **Must NOT do**:
+  - Do not remove or modify `compute_overlap_xz`
+  - Do not add OBB or complex overlap metrics
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+    - Reason: Simple geometric function + config field addition
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES
+  - **Parallel Group**: Wave 1 (with Tasks 1, 2)
+  - **Blocks**: Task 5
+  - **Blocked By**: None
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:85-105` — `compute_overlap_xz` (follow same pattern for 3D version)
+
+  **WHY Each Reference Matters**:
+  - Lines 85-105: exact pattern to follow (min/max bounding box, intersection, area/volume)
+
+  **Acceptance Criteria**:
+
+  - [ ] `compute_overlap_3d` exists and returns volume in m³
+  - [ ] `ICPConfig.overlap_mode` field exists with default `"xz"`
+  - [ ] Disjoint clouds return 0.0
+  - [ ] Fully overlapping clouds return correct volume
+  - [ ] `uv run basedpyright aruco/icp_registration.py` → 0 errors
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: 3D overlap correctness
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "overlap_3d" -v
+      2. Assert: exit code 0
+    Expected Result: Correct volume for known geometries
+    Evidence: pytest output
+  ```
+
+  **Commit**: YES
+  - Message: `feat(icp): add 3D AABB overlap check for hybrid/full modes`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 4. Add TukeyLoss robust kernel support
+
+  **What to do**:
+  - Add `robust_kernel: str = "none"` field to `ICPConfig` (values: "none", "tukey")
+  - Add `robust_kernel_k: float = 0.1` field to `ICPConfig`
+  - In `pairwise_icp`, when `config.robust_kernel == "tukey"`:
+    - Create `loss = o3d.pipelines.registration.TukeyLoss(k=config.robust_kernel_k)`
+    - Pass loss to `TransformationEstimationPointToPlane(loss)` or `TransformationEstimationForGeneralizedICP(loss)`
+  - When `config.robust_kernel == "none"`: use current behavior (no loss function)
+
+  **Must NOT do**:
+  - Do not add Huber, Cauchy, or other kernels
+  - Do not change default behavior (default is "none")
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+    - Reason: Config field + conditional in pairwise_icp
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES
+  - **Parallel Group**: Wave 2 (with Tasks 5, 6)
+  - **Blocks**: Task 5
+  - **Blocked By**: None
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:177-213` — `pairwise_icp` estimation method dispatch (add kernel here)
+
+  **External References**:
+  - Open3D robust kernels: https://www.open3d.org/docs/release/tutorial/t_pipelines/t_robust_kernel.html
+  - `TukeyLoss(k)` where k ≈ expected noise std dev in meters
+
+  **WHY Each Reference Matters**:
+  - Lines 177-213: exact location where estimation objects are created; kernel wraps them
+
+  **Acceptance Criteria**:
+
+  - [ ] `ICPConfig.robust_kernel` and `robust_kernel_k` fields exist
+  - [ ] `pairwise_icp` uses TukeyLoss when configured
+  - [ ] Default behavior unchanged (no kernel applied)
+  - [ ] `uv run basedpyright aruco/icp_registration.py` → 0 errors
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Tukey kernel applied correctly
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "robust_kernel" -v
+      2. Assert: exit code 0
+    Expected Result: ICP runs with Tukey kernel without errors
+    Evidence: pytest output
+  ```
+
+  **Commit**: YES
+  - Message: `feat(icp): add TukeyLoss robust kernel support`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 5. Integrate region selection into refine_with_icp
+
+  **What to do**:
+  - Modify `refine_with_icp()` to use `extract_scene_points` instead of `extract_near_floor_band`
+  - Dispatch overlap check based on `config.overlap_mode`:
+    - `"xz"` → `compute_overlap_xz` (floor mode)
+    - `"3d"` → `compute_overlap_3d` (hybrid/full)
+  - Auto-set `overlap_mode` based on `region`:
+    - `floor` → `overlap_mode="xz"`
+    - `hybrid` or `full` → `overlap_mode="3d"`
+  - Apply `preprocess_point_cloud` (SOR) to all point clouds before ICP
+  - When `config.robust_kernel != "none"`, pass kernel config through to `pairwise_icp`
+  - Use world-frame points for ICP when in hybrid/full mode (current floor mode transforms back to camera frame — keep that for floor; for hybrid/full, ICP in world frame is more stable with mixed geometry)
+
+  **Must NOT do**:
+  - Do not change pose graph construction or optimization logic
+  - Do not change the validation/safety bounds logic
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-high`
+    - Reason: Core integration task connecting all new components
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES (with Task 6)
+  - **Parallel Group**: Wave 2
+  - **Blocks**: Task 7
+  - **Blocked By**: Tasks 1, 2, 3, 4
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:327-478` — `refine_with_icp` full function (the integration target)
+  - `aruco/icp_registration.py:346-372` — Current point extraction loop (replace with extract_scene_points)
+  - `aruco/icp_registration.py:378-386` — Current overlap check (dispatch based on mode)
+
+  **WHY Each Reference Matters**:
+  - Lines 346-372: the exact loop to modify for region-aware extraction
+  - Lines 378-386: where overlap gating happens, needs mode dispatch
+
+  **Acceptance Criteria**:
+
+  - [ ] `refine_with_icp` uses `extract_scene_points` for point extraction
+  - [ ] Overlap check dispatches based on region mode
+  - [ ] SOR preprocessing applied to all point clouds
+  - [ ] Floor mode produces identical behavior to current implementation
+  - [ ] Hybrid/full modes extract more points and use 3D overlap
+  - [ ] `uv run pytest tests/test_icp_registration.py -v` → all pass
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Floor mode regression
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "floor_mode" -v
+      2. Assert: exit code 0
+    Expected Result: Floor mode unchanged
+    Evidence: pytest output
+
+  Scenario: Hybrid mode integration
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "hybrid_integration" -v
+      2. Assert: exit code 0
+    Expected Result: Hybrid uses mixed-geometry extraction + 3D overlap
+    Evidence: pytest output
+  ```
+
+  **Commit**: YES
+  - Message: `feat(icp): integrate region selection, SOR, and robust kernel into refine_with_icp`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 6. Add FPFH+RANSAC global pre-alignment
+
+  **What to do**:
+  - Add `global_init: bool = False` field to `ICPConfig`
+  - Add `compute_fpfh_features(pcd_down, voxel_size)` function:
+    - Compute FPFH features using `o3d.pipelines.registration.compute_fpfh_feature`
+    - Use `KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=100)`
+  - Add `global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size)` function:
+    - Use `registration_ransac_based_on_feature_matching`
+    - Correspondence checkers: EdgeLength(0.9), Distance(voxel_size*1.5)
+    - Convergence: 4M iterations, 500 validations
+    - Return transformation matrix
+  - In `refine_with_icp` pairwise loop: when `config.global_init` is True:
+    - Attempt global registration first
+    - Validate result (fitness > 0.1, transform magnitude within bounds)
+    - If valid: use as init_T for pairwise_icp instead of extrinsic-derived init
+    - If invalid: log warning, fall back to extrinsic-derived init
+  - When `config.global_init` is False: use current extrinsic-based init (unchanged)
+
+  **Must NOT do**:
+  - Do not add other global registration methods
+  - Do not make global_init the default
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-high`
+    - Reason: New FPFH pipeline with validation and fallback logic
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES (with Tasks 4, 5)
+  - **Parallel Group**: Wave 2
+  - **Blocks**: Task 7
+  - **Blocked By**: None (uses only Open3D APIs + ICPConfig)
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:388-407` — Current pairwise ICP loop where init_T is computed and used
+
+  **External References**:
+  - Open3D FPFH: https://www.open3d.org/docs/release/tutorial/pipelines/global_registration.html
+  - Open3D `registration_ransac_based_on_feature_matching` API
+
+  **WHY Each Reference Matters**:
+  - Lines 388-407: where init_T is computed; global_init replaces this when enabled
+
+  **Acceptance Criteria**:
+
+  - [ ] `ICPConfig.global_init` field exists (default False)
+  - [ ] `compute_fpfh_features` and `global_registration` functions exist
+  - [ ] When enabled: uses FPFH transform as init if valid, falls back otherwise
+  - [ ] When disabled: behavior unchanged
+  - [ ] Fallback logged at WARNING level
+  - [ ] `uv run basedpyright aruco/icp_registration.py` → 0 errors
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Global init fallback on bad data
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "global_init_fallback" -v
+      2. Assert: exit code 0
+    Expected Result: Falls back to extrinsic init when FPFH fails
+    Evidence: pytest output
+
+  Scenario: Global init disabled by default
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest tests/test_icp_registration.py -k "global_init_disabled" -v
+      2. Assert: exit code 0
+    Expected Result: No FPFH computation when global_init=False
+    Evidence: pytest output
+  ```
+
+  **Commit**: YES
+  - Message: `feat(icp): add optional FPFH+RANSAC global pre-alignment`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 7. Wire CLI flags in refine_ground_plane.py
+
+  **What to do**:
+  - Add CLI options to `refine_ground_plane.py`:
+    - `--icp-region`: type=click.Choice(["floor", "hybrid", "full"]), default="hybrid"
+    - `--icp-global-init / --no-icp-global-init`: default False
+    - `--icp-min-overlap`: type=float, default=0.5
+    - `--icp-band-height`: type=float, default=0.3
+    - `--icp-robust-kernel`: type=click.Choice(["none", "tukey"]), default="none"
+    - `--icp-robust-k`: type=float, default=0.1
+  - Pass these through to `ICPConfig` construction
+  - Include new config values in `_meta.icp_refined.config` output JSON
+
+  **Must NOT do**:
+  - Do not change existing CLI flag names or defaults for non-ICP flags
+  - Do not change ground-plane refinement logic
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-high`
+    - Reason: Multiple CLI flags with proper Click integration and JSON metadata
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES (with Task 8)
+  - **Parallel Group**: Wave 3
+  - **Blocks**: Task 9
+  - **Blocked By**: Tasks 5, 6
+
+  **References**:
+
+  **Pattern References**:
+  - `refine_ground_plane.py` — Existing `--icp`, `--icp-method`, `--icp-voxel-size` flags (follow same pattern)
+  - `refine_ground_plane.py` — `_meta.icp_refined.config` section in output JSON
+
+  **WHY Each Reference Matters**:
+  - Existing ICP flags: exact pattern for Click option declaration + ICPConfig construction
+
+  **Acceptance Criteria**:
+
+  - [ ] `uv run refine_ground_plane.py --help` shows all new flags
+  - [ ] `--icp-region` accepts floor, hybrid, full
+  - [ ] `--icp-global-init` is a boolean flag
+  - [ ] Output JSON `_meta.icp_refined.config` includes region, global_init, min_overlap, band_height
+  - [ ] `uv run basedpyright refine_ground_plane.py` → 0 errors
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: New CLI flags appear in help
+    Tool: Bash
+    Steps:
+      1. uv run refine_ground_plane.py --help
+      2. Assert: output contains "--icp-region"
+      3. Assert: output contains "floor|hybrid|full"
+      4. Assert: output contains "--icp-global-init"
+      5. Assert: output contains "--icp-min-overlap"
+      6. Assert: output contains "--icp-band-height"
+    Expected Result: All flags documented
+    Evidence: Help output captured
+
+  Scenario: Flags pass through to ICPConfig
+    Tool: Bash (pipeline run)
+    Steps:
+      1. uv run refine_ground_plane.py --input-extrinsics output/e2e_refine_depth.json --input-depth output/e2e_refine_depth.h5 --output-extrinsics /tmp/test_cli_flags.json --icp --icp-region hybrid --icp-voxel-size 0.04 --seed 42
+      2. Parse /tmp/test_cli_flags.json
+      3. Assert: _meta.icp_refined.config.region == "hybrid"
+    Expected Result: Config values in output JSON
+    Evidence: JSON file contents
+  ```
+
+  **Commit**: YES
+  - Message: `feat(cli): wire ICP region, global-init, overlap, kernel CLI flags`
+  - Files: `refine_ground_plane.py`
+  - Pre-commit: `uv run pytest -q`
+
+---
+
+- [ ] 8. Relax ICPConfig defaults
+
+  **What to do**:
+  - Change `ICPConfig` defaults:
+    - `min_fitness: float = 0.3` → `0.15` (more permissive pair acceptance)
+    - `min_overlap_area: float = 1.0` → `0.5` (allow sparser overlap)
+    - `gravity_penalty_weight: float = 10.0` → `2.0` (allow useful tilt corrections)
+    - `max_correspondence_distance_factor: float = 1.4` → `2.5` (wider capture at coarse scale)
+    - `max_translation_m: float = 0.1` → `0.3` (allow reasonable corrections)
+    - `max_rotation_deg: float = 5.0` → `10.0` (allow moderate rotation corrections)
+  - Keep `region: str = "floor"` as default in ICPConfig (CLI default is "hybrid", but library default stays conservative)
+
+  **Must NOT do**:
+  - Do not change method, voxel_size, or band_height defaults
+  - Do not change any algorithmic logic
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+    - Reason: Pure config value changes, no logic
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: YES (with Task 7)
+  - **Parallel Group**: Wave 3
+  - **Blocks**: Task 9
+  - **Blocked By**: None
+
+  **References**:
+
+  **Pattern References**:
+  - `aruco/icp_registration.py:20-34` — ICPConfig dataclass (all defaults here)
+
+  **WHY Each Reference Matters**:
+  - Lines 20-34: the exact defaults to modify, with Oracle's recommendations as rationale
+
+  **Acceptance Criteria**:
+
+  - [ ] All default values updated as specified
+  - [ ] Existing tests still pass with new defaults
+  - [ ] `uv run pytest tests/test_icp_registration.py -v` → all pass
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Defaults changed correctly
+    Tool: Bash (python)
+    Steps:
+      1. uv run python -c "from aruco.icp_registration import ICPConfig; c=ICPConfig(); print(c.min_fitness, c.min_overlap_area, c.gravity_penalty_weight, c.max_correspondence_distance_factor, c.max_translation_m, c.max_rotation_deg)"
+      2. Assert: output is "0.15 0.5 2.0 2.5 0.3 10.0"
+    Expected Result: All defaults match spec
+    Evidence: Terminal output
+  ```
+
+  **Commit**: YES
+  - Message: `chore(icp): relax ICPConfig defaults per Oracle recommendations`
+  - Files: `aruco/icp_registration.py`
+  - Pre-commit: `uv run pytest tests/test_icp_registration.py -q`
+
+---
+
+- [ ] 9. Tests + README + regression verification
+
+  **What to do**:
+  - Add new tests to `tests/test_icp_registration.py`:
+    - `test_success_gate_single_camera`: verify success=True when 1 camera optimized
+    - `test_floor_mode_legacy_equivalence`: floor extraction matches extract_near_floor_band
+    - `test_hybrid_includes_vertical_structure`: hybrid returns more points with walls
+    - `test_hybrid_fallback_no_vertical`: falls back to floor-only with warning
+    - `test_full_mode_all_points`: full mode returns all valid points after SOR
+    - `test_overlap_3d_disjoint`: 3D overlap returns 0 for disjoint clouds
+    - `test_overlap_3d_partial`: 3D overlap returns correct volume
+    - `test_robust_kernel_tukey`: ICP runs with TukeyLoss without errors
+    - `test_global_init_fallback`: falls back when FPFH fails
+    - `test_global_init_disabled_default`: no FPFH when global_init=False
+    - `test_per_pair_logging_all_pairs`: all pairs stored in metrics regardless of convergence
+    - `test_preprocess_sor`: SOR removes outliers correctly
+  - Update `README.md`:
+    - Document `--icp-region`, `--icp-global-init`, `--icp-min-overlap`, `--icp-band-height`, `--icp-robust-kernel`, `--icp-robust-k` flags
+    - Add example command with hybrid mode
+  - Run full regression:
+    - `uv run pytest -x -vv` → all pass
+    - `uv run basedpyright aruco/icp_registration.py refine_ground_plane.py` → 0 errors
+
+  **Must NOT do**:
+  - Do not add tests for features not implemented in Tasks 1-8
+  - Do not modify code logic (test-only + docs-only task)
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-high`
+    - Reason: Large test suite + documentation + regression verification
+  - **Skills**: []
+
+  **Parallelization**:
+  - **Can Run In Parallel**: NO (final task)
+  - **Parallel Group**: Wave 3 (after Tasks 7, 8)
+  - **Blocks**: None (final)
+  - **Blocked By**: Tasks 7, 8
+
+  **References**:
+
+  **Pattern References**:
+  - `tests/test_icp_registration.py` — Existing test patterns (synthetic point clouds, monkeypatching, numpy assertions)
+
+  **Test References**:
+  - `tests/test_icp_registration.py:test_pairwise_icp_known_transform` — Pattern for synthetic ICP tests
+  - `tests/test_icp_registration.py:test_refine_with_icp_synthetic_offset` — Pattern for integration tests
+
+  **WHY Each Reference Matters**:
+  - Existing tests: follow same synthetic data patterns, assertion styles, and monkeypatching approaches
+
+  **Acceptance Criteria**:
+
+  - [ ] ≥12 new test cases added
+  - [ ] `uv run pytest tests/test_icp_registration.py -v` → all pass (existing + new)
+  - [ ] `uv run pytest -x -vv` → all pass (full suite)
+  - [ ] `uv run basedpyright aruco/icp_registration.py refine_ground_plane.py` → 0 errors
+  - [ ] README.md documents all new CLI flags with examples
+
+  **Agent-Executed QA Scenarios:**
+
+  ```
+  Scenario: Full test suite passes
+    Tool: Bash (pytest)
+    Steps:
+      1. uv run pytest -x -vv
+      2. Assert: exit code 0
+      3. Assert: output contains "passed" and no "FAILED"
+    Expected Result: All tests pass
+    Evidence: pytest output captured
+
+  Scenario: Type check clean
+    Tool: Bash (basedpyright)
+    Steps:
+      1. uv run basedpyright aruco/icp_registration.py refine_ground_plane.py
+      2. Assert: output contains "0 errors"
+    Expected Result: No type errors
+    Evidence: basedpyright output
+
+  Scenario: README documents new flags
+    Tool: Bash (grep)
+    Steps:
+      1. grep -c "icp-region" README.md
+      2. Assert: count > 0
+      3. grep -c "icp-global-init" README.md
+      4. Assert: count > 0
+    Expected Result: Flags documented
+    Evidence: grep output
+  ```
+
+  **Commit**: YES
+  - Message: `test(icp): add comprehensive tests for full-scene ICP pipeline + update docs`
+  - Files: `tests/test_icp_registration.py`, `README.md`
+  - Pre-commit: `uv run pytest -x -vv`
+
+---
+
+## Commit Strategy
+
+| After Task | Message | Files | Verification |
+|------------|---------|-------|--------------|
+| 1 | `fix(icp): relax success gate to >0 and add per-pair diagnostic logging` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 2 | `feat(icp): add hybrid/full point extraction with SOR preprocessing` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 3 | `feat(icp): add 3D AABB overlap check for hybrid/full modes` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 4 | `feat(icp): add TukeyLoss robust kernel support` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 5 | `feat(icp): integrate region selection, SOR, and robust kernel into refine_with_icp` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 6 | `feat(icp): add optional FPFH+RANSAC global pre-alignment` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 7 | `feat(cli): wire ICP region, global-init, overlap, kernel CLI flags` | `refine_ground_plane.py` | `uv run pytest -q` |
+| 8 | `chore(icp): relax ICPConfig defaults per Oracle recommendations` | `aruco/icp_registration.py` | `uv run pytest tests/test_icp_registration.py -q` |
+| 9 | `test(icp): add comprehensive tests for full-scene ICP pipeline + update docs` | `tests/test_icp_registration.py`, `README.md` | `uv run pytest -x -vv` |
+
+---
+
+## Success Criteria
+
+### Verification Commands
+```bash
+uv run refine_ground_plane.py --help  # Expected: shows --icp-region, --icp-global-init, etc.
+uv run pytest -x -vv                  # Expected: all tests pass
+uv run basedpyright aruco/icp_registration.py refine_ground_plane.py  # Expected: 0 errors
+```
+
+### Final Checklist
+- [ ] All "Must Have" present
+- [ ] All "Must NOT Have" absent
+- [ ] All tests pass
+- [ ] `--icp-region floor` backward compatible
+- [ ] `--icp-region hybrid` default in CLI
+- [ ] README documents all new flags