@@ -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–
> **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– `>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– –
- 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
