716 lines
35 KiB
Markdown
716 lines
35 KiB
Markdown
# ICP Registration for Multi-Camera Extrinsic Refinement
|
||
|
||
## TL;DR
|
||
|
||
> **Quick Summary**: Add ICP-based pairwise registration with pose-graph optimization to `refine_ground_plane.py`, refining multi-camera extrinsics after RANSAC ground-plane leveling. Supports Point-to-Plane and GICP methods on near-floor-band point clouds with gravity-constrained DOF.
|
||
>
|
||
> **Deliverables**:
|
||
> - `aruco/icp_registration.py` — core ICP module (near-floor extraction, pairwise ICP, overlap detection, pose graph, gravity constraint)
|
||
> - Updated `refine_ground_plane.py` — CLI flags (`--icp`, `--icp-method`, `--icp-voxel-size`)
|
||
> - `tests/test_icp_registration.py` — unit tests for all core functions
|
||
>
|
||
> **Estimated Effort**: Medium
|
||
> **Parallel Execution**: YES — 2 waves
|
||
> **Critical Path**: Task 1 → Task 2 → Task 3 → Task 4
|
||
|
||
---
|
||
|
||
## Context
|
||
|
||
### Original Request
|
||
Add ICP registration to refine multi-camera extrinsics in a ZED depth camera calibration pipeline. ICP chains after existing RANSAC ground-plane correction. User also asked about colored ICP — deferred after analysis showed it requires extending the HDF5 pipeline to save RGB images (significant plumbing work not justified for floor-only scope). GICP chosen as alternative method option instead (same data pipeline, no RGB needed).
|
||
|
||
### Interview Summary
|
||
**Key Discussions**:
|
||
- ICP complements RANSAC leveling, does NOT replace it
|
||
- **Near-floor band** scope (floor_y to floor_y + ~30cm) — includes slight 3D structure (baseboards, table legs, cables) for better constraints; strict plane inliers are degenerate for yaw/XZ
|
||
- **Gravity-constrained**: pitch/roll regularized (soft penalty) to preserve RANSAC gravity alignment; ICP primarily refines yaw + XZ translation + small height
|
||
- **Two ICP methods**: Point-to-Plane (default) + GICP (optional via `--icp-method`)
|
||
- **Colored ICP deferred**: current HDF5 pipeline doesn't save RGB images; adding it requires extending `depth_save.py`, `load_depth_data`, and threading RGB through entire pipeline — not worth it for floor-only scope
|
||
- Global pose-graph optimization for multi-camera consistency
|
||
- Tests-after strategy (implement, then add tests)
|
||
|
||
**Research Findings**:
|
||
- `unproject_depth_to_points` already exists in `aruco/ground_plane.py` — reuse for point cloud generation
|
||
- `detect_floor_plane` does RANSAC segmentation — use the detected floor_y to define the near-floor band
|
||
- Open3D already a dependency — provides `registration_icp`, `registration_generalized_icp`, `PoseGraph`, `global_optimization`
|
||
- Multi-scale ICP recommended: derive from base voxel size as `[4×, 2×, 1×]`, `max_iter = [50, 30, 14]`
|
||
- `get_information_matrix_from_point_clouds` provides edge weights for pose graph
|
||
- All depth values in meters (ZED SDK `coordinate_units=sl.UNIT.METER`)
|
||
- GICP (`registration_generalized_icp`) models local structure as Gaussian distributions — more robust than point-to-plane for noisy stereo data
|
||
|
||
### Metis Review
|
||
**Identified Gaps** (addressed):
|
||
- **Floor points definition**: Resolved → near-floor band (not strict plane inliers) to provide 3D structure for stable ICP constraints
|
||
- **Gravity preservation**: Resolved → soft-constrain pitch/roll with regularization penalty after ICP
|
||
- **Planar degeneracy**: Mitigated by near-floor band including objects with 3D relief
|
||
- **Disconnected graph**: Handle by optimizing only connected component with reference camera, warn about unconnected cameras
|
||
- **Overlap detection robustness**: Inflate bounding boxes with margin to handle initial extrinsic error
|
||
- **Failure policies**: Skip pair with warning on non-convergence / low fitness; return original extrinsics if all ICP fails
|
||
- **Unit consistency**: All meters — assert in tests; voxel sizes only meaningful in meters
|
||
- **Reference camera**: Use first camera (sorted by serial) by default; not user-configurable initially
|
||
|
||
---
|
||
|
||
## Work Objectives
|
||
|
||
### Core Objective
|
||
Add ICP-based registration as a refinement step in `refine_ground_plane.py` that improves multi-camera alignment by running pairwise ICP on near-floor-band point clouds, followed by pose-graph global optimization, with gravity alignment preserved via soft constraints.
|
||
|
||
### Concrete Deliverables
|
||
- `aruco/icp_registration.py` — new module (~300-400 lines)
|
||
- Updated `refine_ground_plane.py` — 3 new CLI flags + integration call (~50-80 lines added)
|
||
- `tests/test_icp_registration.py` — comprehensive unit tests (~300-400 lines)
|
||
|
||
### Definition of Done
|
||
- [x] `uv run basedpyright` passes with zero errors
|
||
- [x] `uv run pytest` passes with zero failures (all existing + new tests)
|
||
- [x] `uv run python refine_ground_plane.py --help` shows `--icp`, `--icp-method`, `--icp-voxel-size` flags
|
||
- [x] Running with `--no-icp` produces identical output to current behavior
|
||
- [x] Running with `--icp` on synthetic data produces measurably improved extrinsics
|
||
|
||
### Must Have
|
||
- Near-floor-band point extraction (floor_y from RANSAC + configurable band height)
|
||
- Pairwise ICP between overlapping camera pairs (Point-to-Plane + GICP options)
|
||
- Multi-scale ICP (coarse→fine voxel sizes derived from base)
|
||
- Pose-graph global optimization with fixed reference camera
|
||
- Gravity constraint: soft-penalize pitch/roll deviation from RANSAC solution
|
||
- Overlap detection via world XZ bounding-box intersection (with margin)
|
||
- Edge gating: minimum fitness threshold to accept ICP pair result
|
||
- Graceful failure handling (skip pair, warn, return original if all fail)
|
||
- ICP metrics recorded in `_meta.icp_refined`
|
||
- Comprehensive unit tests with synthetic data
|
||
|
||
### Must NOT Have (Guardrails)
|
||
- ❌ Colored ICP or any RGB pipeline changes (explicitly deferred)
|
||
- ❌ Changes to HDF5 schema (`depth_save.py` / `load_depth_data`)
|
||
- ❌ New external dependencies beyond what's in `pyproject.toml`
|
||
- ❌ Removal or replacement of RANSAC ground-plane leveling logic
|
||
- ❌ Modification of existing function signatures in `aruco/ground_plane.py`
|
||
- ❌ Auto-selection of "best" reference camera
|
||
- ❌ Real-time / streaming ICP
|
||
- ❌ Full-scene (non-floor) point cloud registration
|
||
- ❌ Changes to existing JSON output schema beyond updated extrinsic matrices and optional ICP metrics block under `_meta`
|
||
- ❌ AI-slop: unnecessary abstractions, premature generalization, or over-documented code
|
||
- ❌ Documentation updates (out of scope — add later if needed)
|
||
|
||
---
|
||
|
||
## Verification Strategy
|
||
|
||
> **UNIVERSAL RULE: ZERO HUMAN INTERVENTION**
|
||
>
|
||
> ALL tasks MUST be verifiable WITHOUT any human action.
|
||
|
||
### Test Decision
|
||
- **Infrastructure exists**: YES (`pytest` configured in `pyproject.toml`)
|
||
- **Automated tests**: Tests-after (implement first, then add tests)
|
||
- **Framework**: `pytest` with `numpy.testing.assert_allclose`
|
||
|
||
### Agent-Executed QA Scenarios (MANDATORY — ALL tasks)
|
||
|
||
**Verification Tool by Deliverable Type:**
|
||
|
||
| Type | Tool | How Agent Verifies |
|
||
|------|------|-------------------|
|
||
| Python module | Bash (`uv run pytest`) | Run tests, assert pass |
|
||
| CLI integration | Bash (`uv run python refine_ground_plane.py --help`) | Run help, check flags |
|
||
| Type safety | Bash (`uv run basedpyright`) | Run type checker, assert zero errors |
|
||
|
||
---
|
||
|
||
## Execution Strategy
|
||
|
||
### Parallel Execution Waves
|
||
|
||
```
|
||
Wave 1 (Start Immediately):
|
||
└── Task 1: Core ICP module (aruco/icp_registration.py)
|
||
|
||
Wave 2 (After Wave 1):
|
||
├── Task 2: CLI integration (refine_ground_plane.py)
|
||
└── Task 3: Unit tests (tests/test_icp_registration.py)
|
||
|
||
Wave 3 (After Wave 2):
|
||
└── Task 4: Integration testing & type checking (verification only)
|
||
```
|
||
|
||
### Dependency Matrix
|
||
|
||
| Task | Depends On | Blocks | Can Parallelize With |
|
||
|------|------------|--------|---------------------|
|
||
| 1 | None | 2, 3 | None (foundational) |
|
||
| 2 | 1 | 4 | 3 |
|
||
| 3 | 1 | 4 | 2 |
|
||
| 4 | 2, 3 | None | None (final) |
|
||
|
||
### Agent Dispatch Summary
|
||
|
||
| Wave | Tasks | Recommended Agents |
|
||
|------|-------|-------------------|
|
||
| 1 | 1 | `task(category="unspecified-high")` — algorithmic Open3D work |
|
||
| 2 | 2, 3 | `task(category="quick")` for 2; `task(category="unspecified-high")` for 3 |
|
||
| 3 | 4 | `task(category="quick")` — verification only |
|
||
|
||
---
|
||
|
||
## TODOs
|
||
|
||
- [x] 1. Core ICP Registration Module (`aruco/icp_registration.py`)
|
||
|
||
**What to do**:
|
||
Create the core ICP registration module with the following functions and dataclasses:
|
||
|
||
**Dataclasses** (follow `aruco/ground_plane.py` pattern):
|
||
- `ICPConfig` — configuration parameters:
|
||
- `voxel_size: float = 0.02` (base voxel size in meters; multi-scale derives `[4×, 2×, 1×]`)
|
||
- `max_iterations: list[int] = [50, 30, 14]` (per scale)
|
||
- `method: str = "point_to_plane"` (or `"gicp"`)
|
||
- `band_height: float = 0.3` (near-floor band height in meters above detected floor)
|
||
- `min_fitness: float = 0.3` (minimum ICP fitness to accept pair result)
|
||
- `min_overlap_area: float = 1.0` (minimum XZ overlap area in m²)
|
||
- `overlap_margin: float = 0.5` (inflate bboxes by this margin in meters)
|
||
- `gravity_penalty_weight: float = 10.0` (soft constraint on pitch/roll deviation)
|
||
- `max_correspondence_distance_factor: float = 1.4` (multiplied by voxel_size per scale)
|
||
- `max_rotation_deg: float = 5.0` (safety bound on ICP delta)
|
||
- `max_translation_m: float = 0.1` (safety bound on ICP delta)
|
||
- `ICPResult` — per-pair result:
|
||
- `transformation: np.ndarray` (4x4)
|
||
- `fitness: float`
|
||
- `inlier_rmse: float`
|
||
- `information_matrix: np.ndarray` (6x6)
|
||
- `converged: bool`
|
||
- `ICPMetrics` — overall metrics:
|
||
- `success: bool`
|
||
- `num_pairs_attempted: int`
|
||
- `num_pairs_converged: int`
|
||
- `num_cameras_optimized: int`
|
||
- `num_disconnected: int`
|
||
- `per_pair_results: dict[tuple[str, str], ICPResult]`
|
||
- `reference_camera: str`
|
||
- `message: str`
|
||
|
||
**Functions**:
|
||
|
||
1. `extract_near_floor_band(points_world, floor_y, band_height, floor_normal)` → `np.ndarray`
|
||
- Given world-frame points and detected floor parameters, filter to points within the band:
|
||
- Project each point onto floor normal direction
|
||
- Keep points where `floor_y <= projection <= floor_y + band_height`
|
||
- Returns filtered `(M, 3)` array
|
||
- This captures floor + low objects (baseboards, table legs, cables) for 3D structure
|
||
|
||
2. `compute_overlap_xz(points_a, points_b, margin)` → `float`
|
||
- Project both point clouds onto XZ plane (ignore Y)
|
||
- Compute axis-aligned bounding box for each
|
||
- Inflate bboxes by `margin`
|
||
- Return intersection area (m²); 0.0 if no overlap
|
||
|
||
3. `pairwise_icp(source_pcd, target_pcd, config, init_transform)` → `ICPResult`
|
||
- Multi-scale ICP loop (coarse→fine):
|
||
- Derive voxel sizes: `[config.voxel_size * 4, * 2, * 1]`
|
||
- For each scale:
|
||
- Downsample with `voxel_down_sample(voxel_size)`
|
||
- Estimate normals: `estimate_normals(KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=30))`
|
||
- If `config.method == "point_to_plane"`:
|
||
- `registration_icp(source_down, target_down, voxel_size * config.max_correspondence_distance_factor, current_transform, TransformationEstimationPointToPlane(), ICPConvergenceCriteria(...))`
|
||
- If `config.method == "gicp"`:
|
||
- `registration_generalized_icp(source_down, target_down, voxel_size * config.max_correspondence_distance_factor, current_transform, TransformationEstimationForGeneralizedICP(), ICPConvergenceCriteria(...))`
|
||
- Chain: `current_transform = result.transformation`
|
||
- After final scale: compute information matrix via `get_information_matrix_from_point_clouds`
|
||
- Return `ICPResult`
|
||
|
||
4. `apply_gravity_constraint(T_icp, T_original, penalty_weight)` → `np.ndarray`
|
||
- Purpose: preserve RANSAC gravity alignment while allowing yaw + XZ + height refinement
|
||
- Decompose rotation of `T_icp` into pitch/roll/yaw (use scipy `Rotation.from_matrix().as_euler('xyz')` or manual decomposition)
|
||
- Decompose rotation of `T_original` similarly
|
||
- Blend pitch/roll: `blended = original + (icp - original) / (1 + penalty_weight)`
|
||
- Keep ICP yaw unchanged
|
||
- Recompose rotation from blended euler angles
|
||
- Keep ICP translation (yaw + XZ + height are all allowed to change)
|
||
- Return constrained 4x4 transform
|
||
|
||
5. `build_pose_graph(serials, extrinsics, pair_results, reference_serial)` → `o3d.pipelines.registration.PoseGraph`
|
||
- Create one node per camera
|
||
- Set reference camera node as anchor (identity odometry edge)
|
||
- For each converged pair: add loop-closure edge with the ICP transformation and information matrix
|
||
- Detect connected components: only include cameras reachable from reference
|
||
- Log warning for any cameras not in the reference's component
|
||
|
||
6. `optimize_pose_graph(pose_graph)` → `o3d.pipelines.registration.PoseGraph`
|
||
- Run `o3d.pipelines.registration.global_optimization` with:
|
||
- `GlobalOptimizationLevenbergMarquardt()`
|
||
- `GlobalOptimizationConvergenceCriteria()`
|
||
- `GlobalOptimizationOption(max_correspondence_distance=..., reference_node=0)`
|
||
- Return optimized graph
|
||
|
||
7. `refine_with_icp(camera_data, extrinsics, floor_planes, config)` → `tuple[dict[str, np.ndarray], ICPMetrics]`
|
||
- Main orchestrator (follows `refine_ground_from_depth` pattern):
|
||
1. For each camera that has a detected floor plane:
|
||
- Unproject depth to world points (reuse `unproject_depth_to_points` + world transform)
|
||
- Extract near-floor band using `floor_y` from detected `FloorPlane.d` and `FloorPlane.normal`
|
||
- Convert to Open3D PointCloud
|
||
2. Detect overlapping pairs via `compute_overlap_xz`
|
||
3. For each overlapping pair:
|
||
- Compute initial relative transform from current extrinsics
|
||
- Run `pairwise_icp`
|
||
- Apply `apply_gravity_constraint` to the result
|
||
4. Build pose graph from converged pairs
|
||
5. Run `optimize_pose_graph`
|
||
6. Extract refined extrinsics from optimized graph
|
||
7. Validate per-camera deltas against `max_rotation_deg` / `max_translation_m` (reject cameras that exceed bounds)
|
||
8. Return (new_extrinsics, metrics)
|
||
- If no overlapping pairs found or all ICP fails: return original extrinsics with `success=False`
|
||
|
||
**Must NOT do**:
|
||
- Do NOT modify `aruco/ground_plane.py` function signatures or dataclasses
|
||
- Do NOT add RGB/color handling
|
||
- Do NOT add dependencies not in `pyproject.toml`
|
||
- Do NOT import from `refine_ground_plane.py` (module should be self-contained)
|
||
- Do NOT write to files (the caller handles I/O)
|
||
|
||
**Recommended Agent Profile**:
|
||
- **Category**: `unspecified-high`
|
||
- Reason: Algorithmic module with Open3D integration, multiple interacting functions, mathematical constraints (gravity regularization, pose graph). Not purely frontend or quick-fix.
|
||
- **Skills**: []
|
||
- **Skills Evaluated but Omitted**:
|
||
- `playwright`: No browser interaction
|
||
- `frontend-ui-ux`: No UI work
|
||
- `git-master`: Commit handled at end
|
||
|
||
**Parallelization**:
|
||
- **Can Run In Parallel**: NO (foundational — all other tasks depend on this)
|
||
- **Parallel Group**: Wave 1 (solo)
|
||
- **Blocks**: Tasks 2, 3
|
||
- **Blocked By**: None (can start immediately)
|
||
|
||
**References** (CRITICAL):
|
||
|
||
**Pattern References** (existing code to follow):
|
||
- `aruco/ground_plane.py:19-68` — Dataclass pattern for Config/Metrics/Result (FloorPlane, FloorCorrection, GroundPlaneConfig, GroundPlaneMetrics). Follow same style for ICPConfig, ICPResult, ICPMetrics.
|
||
- `aruco/ground_plane.py:71-111` — `unproject_depth_to_points` implementation. Reuse this function directly (import from `aruco.ground_plane`) for point cloud generation.
|
||
- `aruco/ground_plane.py:114-157` — `detect_floor_plane` RANSAC pattern. The returned `FloorPlane.d` and `FloorPlane.normal` define where the floor is — use these to define the near-floor band.
|
||
- `aruco/ground_plane.py:358-540` — `refine_ground_from_depth` orchestrator pattern. Follow this structure for `refine_with_icp`: iterate cameras, validate, compute, apply bounds, return metrics.
|
||
- `aruco/ground_plane.py:1-16` — Import structure and type aliases (Vec3, Mat44, PointsNC). Reuse these.
|
||
- `aruco/depth_refine.py:71-227` — Optimization pattern with stats/metrics reporting and safety bounds.
|
||
|
||
**API/Type References** (contracts to implement against):
|
||
- `aruco/ground_plane.py:10-16` — Type aliases (Vec3, Mat44, PointsNC) — reuse these
|
||
- `aruco/ground_plane.py:20-23` — `FloorPlane` dataclass — `normal` and `d` fields define the floor for band extraction
|
||
- `aruco/ground_plane.py:54-68` — `GroundPlaneMetrics` — `camera_planes: Dict[str, FloorPlane]` provides per-camera floor parameters
|
||
|
||
**External References** (Open3D APIs to use):
|
||
- `o3d.pipelines.registration.registration_icp` — Point-to-plane ICP
|
||
- `o3d.pipelines.registration.TransformationEstimationPointToPlane` — estimation method for point-to-plane
|
||
- `o3d.pipelines.registration.registration_generalized_icp` — GICP variant
|
||
- `o3d.pipelines.registration.TransformationEstimationForGeneralizedICP` — estimation method for GICP
|
||
- `o3d.pipelines.registration.ICPConvergenceCriteria` — convergence parameters
|
||
- `o3d.pipelines.registration.PoseGraph` + `PoseGraphNode` + `PoseGraphEdge` — pose graph construction
|
||
- `o3d.pipelines.registration.global_optimization` — graph optimizer
|
||
- `o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt` — LM solver
|
||
- `o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria` — optimizer convergence
|
||
- `o3d.pipelines.registration.GlobalOptimizationOption` — optimizer options (reference_node)
|
||
- `o3d.pipelines.registration.get_information_matrix_from_point_clouds` — edge weight computation
|
||
- `o3d.geometry.PointCloud` + `voxel_down_sample` + `estimate_normals` — preprocessing
|
||
- `o3d.geometry.KDTreeSearchParamHybrid` — normal estimation parameters
|
||
|
||
**WHY Each Reference Matters**:
|
||
- `ground_plane.py` dataclasses: Follow same style so the ICP module feels native to the codebase
|
||
- `unproject_depth_to_points`: Don't reinvent — import and reuse for depth→point cloud conversion
|
||
- `FloorPlane.d` / `.normal`: These define the floor height and orientation from RANSAC — the band extraction needs these
|
||
- `refine_ground_from_depth`: The orchestrator pattern (iterate cameras, validate, compute, return metrics) should be followed exactly
|
||
- Open3D registration APIs: The core of the implementation — all ICP, pose graph, and optimization
|
||
|
||
**Acceptance Criteria**:
|
||
|
||
**Agent-Executed QA Scenarios (MANDATORY):**
|
||
|
||
```
|
||
Scenario: Module imports without errors
|
||
Tool: Bash
|
||
Preconditions: None
|
||
Steps:
|
||
1. uv run python -c "from aruco.icp_registration import ICPConfig, ICPResult, ICPMetrics, refine_with_icp, pairwise_icp, extract_near_floor_band, compute_overlap_xz, build_pose_graph, optimize_pose_graph, apply_gravity_constraint"
|
||
2. Assert: exit code 0
|
||
Expected Result: All symbols importable
|
||
Evidence: Command output
|
||
|
||
Scenario: Type check passes on new module
|
||
Tool: Bash
|
||
Preconditions: Module file exists
|
||
Steps:
|
||
1. uv run basedpyright aruco/icp_registration.py
|
||
2. Assert: exit code 0, no error-level diagnostics
|
||
Expected Result: Clean type check
|
||
Evidence: basedpyright output
|
||
|
||
Scenario: Smoke test with synthetic overlapping floor points
|
||
Tool: Bash
|
||
Preconditions: Module implemented
|
||
Steps:
|
||
1. uv run python -c "
|
||
import numpy as np
|
||
from aruco.icp_registration import refine_with_icp, ICPConfig
|
||
from aruco.ground_plane import FloorPlane
|
||
rng = np.random.default_rng(42)
|
||
# Two cameras with overlapping floor + small box
|
||
floor1 = np.column_stack([rng.uniform(-2, 2, 500), np.zeros(500) + rng.normal(0, 0.005, 500), rng.uniform(0, 4, 500)])
|
||
box1 = np.column_stack([rng.uniform(0, 0.5, 50), rng.uniform(0, 0.3, 50), rng.uniform(1.5, 2.0, 50)])
|
||
pts1 = np.vstack([floor1, box1])
|
||
floor2 = np.column_stack([rng.uniform(-1, 3, 500), np.zeros(500) + rng.normal(0, 0.005, 500), rng.uniform(1, 5, 500)])
|
||
box2 = np.column_stack([rng.uniform(0, 0.5, 50), rng.uniform(0, 0.3, 50), rng.uniform(1.5, 2.0, 50)])
|
||
pts2 = np.vstack([floor2, box2])
|
||
camera_data = {
|
||
'cam1': {'depth': np.ones((100,100)), 'K': np.eye(3)},
|
||
'cam2': {'depth': np.ones((100,100)), 'K': np.eye(3)},
|
||
}
|
||
extrinsics = {'cam1': np.eye(4), 'cam2': np.eye(4)}
|
||
planes = {
|
||
'cam1': FloorPlane(normal=np.array([0,1,0]), d=0.0),
|
||
'cam2': FloorPlane(normal=np.array([0,1,0]), d=0.0),
|
||
}
|
||
config = ICPConfig(voxel_size=0.05)
|
||
new_ext, metrics = refine_with_icp(camera_data, extrinsics, planes, config)
|
||
print(f'success={metrics.success}, pairs={metrics.num_pairs_attempted}')
|
||
print('smoke test passed')
|
||
"
|
||
2. Assert: exit code 0, prints "smoke test passed"
|
||
Expected Result: ICP runs without crashing on synthetic data
|
||
Evidence: stdout captured
|
||
```
|
||
|
||
**Commit**: YES
|
||
- Message: `feat(aruco): add ICP registration module with pose-graph optimization`
|
||
- Files: `aruco/icp_registration.py`
|
||
- Pre-commit: `uv run basedpyright aruco/icp_registration.py`
|
||
|
||
---
|
||
|
||
- [x] 2. CLI Integration (`refine_ground_plane.py`)
|
||
|
||
**What to do**:
|
||
Add ICP refinement as an optional post-processing step in `refine_ground_plane.py`:
|
||
|
||
1. **Add CLI flags** (follow existing click pattern at lines 19-92):
|
||
- `--icp/--no-icp` (default: `False`) — Enable ICP refinement after ground-plane correction
|
||
- `--icp-method` (type `click.Choice(["point_to_plane", "gicp"])`, default: `"point_to_plane"`) — ICP variant
|
||
- `--icp-voxel-size` (float, default: `0.02`) — Base voxel size in meters (multi-scale derived internally as `[4×, 2×, 1×]`)
|
||
|
||
2. **Add parameters to `main()` function** (line ~93-107): `icp: bool`, `icp_method: str`, `icp_voxel_size: float`
|
||
|
||
3. **Integration point**: After step 4 (line ~183, after `refine_ground_from_depth()`) and before step 5 (Save Output Extrinsics):
|
||
```python
|
||
# 4.5 Optional ICP Refinement
|
||
if icp:
|
||
from aruco.icp_registration import refine_with_icp, ICPConfig
|
||
icp_config = ICPConfig(
|
||
method=icp_method,
|
||
voxel_size=icp_voxel_size,
|
||
max_rotation_deg=max_rotation_deg,
|
||
max_translation_m=max_translation_m,
|
||
)
|
||
icp_extrinsics, icp_metrics = refine_with_icp(
|
||
camera_data_for_refine,
|
||
new_extrinsics, # post-ground-plane extrinsics
|
||
metrics.camera_planes, # FloorPlane per camera from RANSAC
|
||
icp_config,
|
||
)
|
||
if icp_metrics.success:
|
||
new_extrinsics = icp_extrinsics
|
||
logger.info(f"ICP refinement: {icp_metrics.message}")
|
||
else:
|
||
logger.warning(f"ICP refinement failed: {icp_metrics.message}. Using ground-plane-only extrinsics.")
|
||
```
|
||
|
||
4. **Add ICP metrics to output JSON** under `_meta.icp_refined` (same pattern as `_meta.ground_refined` at lines 218-238)
|
||
|
||
**Must NOT do**:
|
||
- Do NOT change the existing ground-plane refinement logic
|
||
- Do NOT change existing CLI flag behavior or defaults
|
||
- Do NOT make ICP enabled by default
|
||
- Do NOT add more than the 3 specified flags
|
||
- Do NOT modify existing function signatures
|
||
|
||
**Recommended Agent Profile**:
|
||
- **Category**: `quick`
|
||
- Reason: Small, well-scoped integration — adding click flags and calling an existing function. ~50-80 lines of changes.
|
||
- **Skills**: []
|
||
|
||
**Parallelization**:
|
||
- **Can Run In Parallel**: YES (with Task 3)
|
||
- **Parallel Group**: Wave 2 (with Task 3)
|
||
- **Blocks**: Task 4
|
||
- **Blocked By**: Task 1
|
||
|
||
**References**:
|
||
|
||
**Pattern References**:
|
||
- `refine_ground_plane.py:19-92` — Existing click options pattern. Follow exact style for new flags (decorator placement, help text style, defaults).
|
||
- `refine_ground_plane.py:93-107` — `main()` function signature. Add `icp: bool`, `icp_method: str`, `icp_voxel_size: float` parameters.
|
||
- `refine_ground_plane.py:179-183` — Integration point: after `refine_ground_from_depth` call, before save. ICP step goes here.
|
||
- `refine_ground_plane.py:218-238` — `_meta.ground_refined` output pattern. Copy this structure for `_meta.icp_refined`.
|
||
|
||
**API References**:
|
||
- `aruco/icp_registration.py:refine_with_icp` — The main function to call (from Task 1)
|
||
- `aruco/icp_registration.py:ICPConfig` — Configuration dataclass (from Task 1)
|
||
- `aruco/ground_plane.py:GroundPlaneMetrics.camera_planes` — `Dict[str, FloorPlane]` from RANSAC, passed to ICP
|
||
|
||
**WHY Each Reference Matters**:
|
||
- `refine_ground_plane.py` click patterns: Must match existing style exactly so CLI feels consistent
|
||
- `_meta.ground_refined`: Follow this exact pattern so output JSON is consistent
|
||
- `metrics.camera_planes`: This is the bridge — RANSAC detects floor planes, ICP uses them for band extraction
|
||
|
||
**Acceptance Criteria**:
|
||
|
||
**Agent-Executed QA Scenarios (MANDATORY):**
|
||
|
||
```
|
||
Scenario: CLI help shows new flags
|
||
Tool: Bash
|
||
Preconditions: refine_ground_plane.py updated
|
||
Steps:
|
||
1. uv run python refine_ground_plane.py --help
|
||
2. Assert: exit code 0
|
||
3. Assert: output contains "--icp / --no-icp"
|
||
4. Assert: output contains "--icp-method"
|
||
5. Assert: output contains "--icp-voxel-size"
|
||
Expected Result: All three new flags visible in help
|
||
Evidence: Help output captured
|
||
|
||
Scenario: --no-icp preserves existing behavior (no regression)
|
||
Tool: Bash
|
||
Preconditions: Test fixtures work
|
||
Steps:
|
||
1. uv run pytest tests/test_refine_ground_cli.py -x -vv
|
||
2. Assert: all tests pass (exit code 0)
|
||
Expected Result: Zero regressions in non-ICP path
|
||
Evidence: pytest output
|
||
|
||
Scenario: Type check passes
|
||
Tool: Bash
|
||
Steps:
|
||
1. uv run basedpyright refine_ground_plane.py
|
||
2. Assert: exit code 0
|
||
Expected Result: Clean type check
|
||
Evidence: basedpyright output
|
||
```
|
||
|
||
**Commit**: YES (groups with Task 3)
|
||
- Message: `feat(cli): add --icp/--icp-method/--icp-voxel-size to refine_ground_plane.py`
|
||
- Files: `refine_ground_plane.py`
|
||
- Pre-commit: `uv run basedpyright refine_ground_plane.py && uv run pytest tests/test_refine_ground_cli.py -x`
|
||
|
||
---
|
||
|
||
- [x] 3. Unit Tests (`tests/test_icp_registration.py`)
|
||
|
||
**What to do**:
|
||
Create comprehensive unit tests following existing test patterns in `tests/test_ground_plane.py`:
|
||
|
||
**Test functions to implement** (~19 tests):
|
||
|
||
*Near-floor band extraction:*
|
||
1. `test_extract_near_floor_band_basic` — synthetic points at known heights; verify only points within band are kept
|
||
2. `test_extract_near_floor_band_empty` — no points in band → returns empty array
|
||
3. `test_extract_near_floor_band_all_in_band` — all points within band → returns all
|
||
|
||
*Overlap detection:*
|
||
4. `test_compute_overlap_xz_full_overlap` — identical point clouds → overlap area ≈ full area
|
||
5. `test_compute_overlap_xz_no_overlap` — disjoint clouds → overlap = 0.0
|
||
6. `test_compute_overlap_xz_partial` — known partial overlap → verify area within tolerance
|
||
7. `test_compute_overlap_xz_with_margin` — margin inflates overlap detection
|
||
|
||
*Pairwise ICP:*
|
||
8. `test_pairwise_icp_identity` — source == target → transform ≈ identity, high fitness
|
||
9. `test_pairwise_icp_known_transform_point_to_plane` — source = target transformed by known small T → recovered T within tolerance (≤2cm, ≤1°)
|
||
10. `test_pairwise_icp_known_transform_gicp` — same as above but with method="gicp"
|
||
11. `test_pairwise_icp_insufficient_points` — too few points → converged=False or low fitness
|
||
|
||
*Gravity constraint:*
|
||
12. `test_apply_gravity_constraint_preserves_yaw` — yaw component unchanged
|
||
13. `test_apply_gravity_constraint_regularizes_pitch_roll` — pitch/roll pulled toward original values
|
||
14. `test_apply_gravity_constraint_identity` — no pitch/roll change → output ≈ input
|
||
|
||
*Pose graph:*
|
||
15. `test_build_pose_graph_basic` — 3 cameras, 2 converged pairs → graph has correct nodes/edges
|
||
16. `test_build_pose_graph_disconnected` — cameras with no overlap → warns, excludes disconnected
|
||
|
||
*Integration (orchestrator):*
|
||
17. `test_refine_with_icp_synthetic_offset` — 3+ cameras with known offset from ground truth → ICP reduces error
|
||
18. `test_refine_with_icp_no_overlap` — cameras with no floor overlap → returns original extrinsics, success=False
|
||
19. `test_refine_with_icp_single_camera` — only 1 camera → skip ICP, return original
|
||
|
||
**Synthetic data strategy** (follow `tests/test_ground_plane.py` patterns):
|
||
- Generate 3D point clouds of "floor + small box/wall" scene for 3D structure
|
||
- Apply known rigid transforms to simulate camera viewpoints
|
||
- Add Gaussian noise (~0.005m) to simulate depth sensor noise
|
||
- Use `np.random.default_rng(seed)` for reproducibility
|
||
- Use `numpy.testing.assert_allclose` for numerical comparisons with `atol` appropriate for ICP (1-2cm)
|
||
- Use `pytest.raises` for error cases
|
||
|
||
**Must NOT do**:
|
||
- Do NOT require real ZED data or SVO files
|
||
- Do NOT require network access
|
||
- Do NOT modify existing test files
|
||
- Do NOT use mocks for Open3D (test actual ICP convergence)
|
||
|
||
**Recommended Agent Profile**:
|
||
- **Category**: `unspecified-high`
|
||
- Reason: Many test functions (~19), synthetic 3D data generation with noise, numerical assertions with appropriate tolerances. Needs careful attention to geometry.
|
||
- **Skills**: []
|
||
|
||
**Parallelization**:
|
||
- **Can Run In Parallel**: YES (with Task 2)
|
||
- **Parallel Group**: Wave 2 (with Task 2)
|
||
- **Blocks**: Task 4
|
||
- **Blocked By**: Task 1
|
||
|
||
**References**:
|
||
|
||
**Pattern References** (testing patterns to follow):
|
||
- `tests/test_ground_plane.py:18-37` — `test_unproject_depth_to_points_simple`: How to create synthetic depth map + intrinsics matrix + assert shape/values
|
||
- `tests/test_ground_plane.py:80-97` — `test_detect_floor_plane_perfect`: How to create synthetic planar point clouds with known normal/d and assert detection
|
||
- `tests/test_ground_plane.py:123-136` — `test_detect_floor_plane_with_outliers`: Adding noise/outliers pattern
|
||
- `tests/test_ground_plane.py:298-311` — `test_compute_floor_correction_identity`: Testing identity transform and small deltas
|
||
- `tests/test_ground_plane.py:495-606` — Integration test `test_refine_ground_from_depth_*`: Constructing synthetic camera_data + extrinsics dictionaries
|
||
|
||
**API References**:
|
||
- `aruco/icp_registration.py` — All public functions and dataclasses (from Task 1)
|
||
|
||
**WHY Each Reference Matters**:
|
||
- `test_ground_plane.py` patterns: Follow the EXACT same style (assert_allclose, synthetic data, seed-based reproducibility) so tests feel native
|
||
- Integration test patterns: Show how to construct `camera_data` and `extrinsics` dicts matching the expected format
|
||
|
||
**Acceptance Criteria**:
|
||
|
||
**Agent-Executed QA Scenarios (MANDATORY):**
|
||
|
||
```
|
||
Scenario: All new tests pass
|
||
Tool: Bash
|
||
Preconditions: Task 1 complete (icp_registration.py exists)
|
||
Steps:
|
||
1. uv run pytest tests/test_icp_registration.py -x -vv
|
||
2. Assert: exit code 0
|
||
3. Assert: output shows ≥19 tests passed
|
||
Expected Result: All tests green
|
||
Evidence: pytest output with test names and pass counts
|
||
|
||
Scenario: Tests complete in reasonable time
|
||
Tool: Bash
|
||
Steps:
|
||
1. timeout 120 uv run pytest tests/test_icp_registration.py -x -vv
|
||
2. Assert: completes within 120 seconds
|
||
Expected Result: No individual test takes more than ~30s
|
||
Evidence: pytest timing output
|
||
|
||
Scenario: Known transform recovery within tolerance
|
||
Tool: Bash
|
||
Steps:
|
||
1. uv run pytest tests/test_icp_registration.py -k "known_transform" -x -vv
|
||
2. Assert: passes
|
||
Expected Result: ICP recovers known rigid transform within ≤2cm translation, ≤1° rotation
|
||
Evidence: Test output captured
|
||
```
|
||
|
||
**Commit**: YES (groups with Task 2)
|
||
- Message: `test(icp): add comprehensive unit tests for ICP registration`
|
||
- Files: `tests/test_icp_registration.py`
|
||
- Pre-commit: `uv run pytest tests/test_icp_registration.py -x`
|
||
|
||
---
|
||
|
||
- [x] 4. Integration Testing & Final Verification
|
||
|
||
**What to do**:
|
||
Run the full verification suite to ensure everything works together:
|
||
|
||
1. Run full test suite (existing + new) to catch regressions
|
||
2. Run basedpyright on entire project
|
||
3. Verify CLI end-to-end with `--help`
|
||
4. Verify `--no-icp` default preserves existing behavior
|
||
|
||
**Must NOT do**:
|
||
- Do NOT fix issues in other tasks' code (flag them instead)
|
||
- Do NOT add new features
|
||
|
||
**Recommended Agent Profile**:
|
||
- **Category**: `quick`
|
||
- Reason: Verification-only task — running commands and checking results. No new code.
|
||
- **Skills**: []
|
||
|
||
**Parallelization**:
|
||
- **Can Run In Parallel**: NO (final integration — depends on everything)
|
||
- **Parallel Group**: Wave 3 (solo)
|
||
- **Blocks**: None (final task)
|
||
- **Blocked By**: Tasks 2, 3
|
||
|
||
**References**:
|
||
- `pyproject.toml:41-43` — pytest configuration (testpaths, norecursedirs)
|
||
- `AGENTS.md` — build/test commands reference
|
||
|
||
**Acceptance Criteria**:
|
||
|
||
**Agent-Executed QA Scenarios (MANDATORY):**
|
||
|
||
```
|
||
Scenario: Full test suite passes
|
||
Tool: Bash
|
||
Steps:
|
||
1. uv run pytest -x -vv
|
||
2. Assert: exit code 0
|
||
3. Assert: no failures, no errors
|
||
Expected Result: All tests green including new ICP tests
|
||
Evidence: Full pytest output
|
||
|
||
Scenario: Full type check passes
|
||
Tool: Bash
|
||
Steps:
|
||
1. uv run basedpyright
|
||
2. Assert: exit code 0
|
||
Expected Result: Zero type errors across entire project
|
||
Evidence: basedpyright output
|
||
|
||
Scenario: CLI help is correct
|
||
Tool: Bash
|
||
Steps:
|
||
1. uv run python refine_ground_plane.py --help
|
||
2. Assert: output contains "--icp / --no-icp"
|
||
3. Assert: output contains "--icp-method"
|
||
4. Assert: output contains "--icp-voxel-size"
|
||
Expected Result: All flags documented
|
||
Evidence: Help output
|
||
```
|
||
|
||
**Commit**: NO (verification only — all code committed in Tasks 1-3)
|
||
|
||
---
|
||
|
||
## Commit Strategy
|
||
|
||
| After Task | Message | Files | Verification |
|
||
|------------|---------|-------|--------------|
|
||
| 1 | `feat(aruco): add ICP registration module with pose-graph optimization` | `aruco/icp_registration.py` | `uv run basedpyright aruco/icp_registration.py` |
|
||
| 2+3 | `feat(cli): add --icp to refine_ground_plane.py` + `test(icp): add ICP unit tests` | `refine_ground_plane.py`, `tests/test_icp_registration.py` | `uv run pytest -x` |
|
||
| 4 | (no commit — verification only) | — | `uv run pytest && uv run basedpyright` |
|
||
|
||
---
|
||
|
||
## Success Criteria
|
||
|
||
### Verification Commands
|
||
```bash
|
||
uv run pytest -x -vv # All tests pass
|
||
uv run basedpyright # Zero type errors
|
||
uv run python refine_ground_plane.py --help # Shows new flags
|
||
uv run pytest tests/test_icp_registration.py -x -vv # ICP tests specifically
|
||
```
|
||
|
||
### Final Checklist
|
||
- [x] All "Must Have" items present and functional
|
||
- [x] All "Must NOT Have" items confirmed absent
|
||
- [x] All existing tests still pass (no regressions)
|
||
- [x] New ICP tests (≥19) all pass
|
||
- [x] basedpyright clean
|
||
- [x] CLI flags documented in help output
|
||
- [x] `--no-icp` produces identical behavior to current code
|