crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
25733af4ec38daf3d5543d918df8b128a006e083
zed-playground/py_workspace/.sisyphus/plans/icp-registration.md
T

35 KiB
Raw Blame History

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

  • uv run basedpyright passes with zero errors
  • uv run pytest passes with zero failures (all existing + new tests)
  • uv run python refine_ground_plane.py --help shows --icp, --icp-method, --icp-voxel-size flags
  • Running with --no-icp produces identical output to current behavior
  • 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

  • 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

  • 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):

      # 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

  • 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

  • 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

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

  • All "Must Have" items present and functional
  • All "Must NOT Have" items confirmed absent
  • All existing tests still pass (no regressions)
  • New ICP tests (≥19) all pass
  • basedpyright clean
  • CLI flags documented in help output
  • --no-icp produces identical behavior to current code
Reference in New Issue View Git Blame Copy Permalink