# 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