{"id":"py_workspace-q4w","title":"Add type hints and folder-aware --svo input in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:01:13.943518267Z","created_by":"crosstyan","updated_at":"2026-02-06T10:03:09.855307397Z","closed_at":"2026-02-06T10:03:09.855307397Z","close_reason":"Implemented type hints and directory expansion for --svo"}
{"id":"py_workspace-q8j","title":"Add script to visualize generated camera extrinsics","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:22:35.151648893Z","created_by":"crosstyan","updated_at":"2026-02-07T08:27:27.034717788Z","closed_at":"2026-02-07T08:27:27.034717788Z","close_reason":"Implemented visualize_extrinsics.py utility script and verified with example data."}
{"id":"py_workspace-qf9","title":"Implement RMSE-based fallback for depth pooling","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T09:03:17.759148159Z","created_by":"crosstyan","updated_at":"2026-02-07T09:06:33.106901615Z","closed_at":"2026-02-07T09:06:33.106901615Z","close_reason":"Implemented RMSE-based fallback and verified with tests"}
{"id":"py_workspace-t4e","title":"Add --min-markers CLI and rejection debug logs in calibrate_extrinsics","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:21:51.846079425Z","created_by":"crosstyan","updated_at":"2026-02-06T10:22:39.870440044Z","closed_at":"2026-02-06T10:22:39.870440044Z","close_reason":"Added --min-markers (default 1), rejection debug logs, and clarified accepted-pose summary label"}
{"id":"py_workspace-th3","title":"Implement Best-Frame Selection for depth verification","status":"closed","priority":1,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:04:11.896109458Z","created_by":"crosstyan","updated_at":"2026-02-07T05:06:07.346747231Z","closed_at":"2026-02-07T05:06:07.346747231Z","close_reason":"Implemented best-frame selection with scoring logic and verified with tests."}
{"id":"py_workspace-tpz","title":"Refactor visualize_extrinsics.py to use true global basis conversion","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T17:41:09.345966612Z","created_by":"crosstyan","updated_at":"2026-02-07T17:43:35.501465973Z","closed_at":"2026-02-07T17:43:35.501465973Z","close_reason":"Refactored visualize_extrinsics.py to use true global basis conversion"}
- When monkeypatching for tests, ensure all internal calls are accounted for, especially when production code has bugs that need to be worked around or highlighted.
- Integrated ICP refinement into `refine_ground_plane.py` CLI, enabling optional global registration after ground plane alignment.
- Added `_meta.icp_refined` block to output JSON to track ICP configuration and success metrics.
## ICP Registration
- GICP method in requires normals, which are estimated internally if not provided.
- Synthetic tests for ICP should use deterministic seeds for point cloud generation to ensure stability.
## ICP Registration
- GICP method in `pairwise_icp` requires normals, which are estimated internally if not provided.
- Synthetic tests for ICP should use deterministic seeds for point cloud generation to ensure stability.
# 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.
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
- **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
- 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)
- 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
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
Blocking a user prevents them from interacting with repositories, such as opening or commenting on pull requests or issues. Learn more about blocking a user.
