crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
2c93c77db883a7e09cc891756d299f0e1ba6e768
zed-playground/py_workspace/.sisyphus/plans/full-icp-pipeline.md
T

36 KiB
Raw Blame History

Full-Scene ICP Pipeline Upgrade

TL;DR

Quick Summary: Upgrade the current floor-band-only ICP registration to support hybrid (floor + vertical structure) and full-scene modes, with optional FPFH+RANSAC global pre-alignment, fixed success gates, robust kernels, and per-pair diagnostic logging.

Deliverables:

  • --icp-region floor|hybrid|full CLI flag (default: hybrid)
  • --icp-global-init optional FPFH+RANSAC pre-alignment
  • Fixed success gate (>0 instead of >1)
  • Per-pair diagnostic logging (all pairs, not just converged)
  • Statistical outlier removal preprocessing
  • TukeyLoss robust kernel support
  • 3D AABB overlap check for hybrid/full modes
  • Exposed CLI flags: --icp-min-overlap, --icp-band-height
  • Relaxed defaults: gravity penalty, correspondence distance factor, min_overlap_area
  • Comprehensive tests covering all new modes + edge cases

Estimated Effort: Medium (3–5 days) Parallel Execution: YES - 3 waves Critical Path: Task 1 → Task 2 → Task 3 → Task 5 → Task 7 → Task 9

Context

Original Request

User observed that camera SN44289123 appears floor-disconnected by ~3–5 cm after ground-plane refinement. Diagnostic sweeps showed ICP consistently failing: only 1 of 6 pairs converges, graph stays disconnected, and the success gate (>1) rejects valid single-camera corrections. Oracle analysis identified floor-band degeneracy (planar sliding), tight gating, and over-aggressive gravity penalty as root causes. User requested upgrading from floor-only to full-scene/hybrid ICP.

Interview Summary

Key Discussions:

  • User chose hybrid as default ICP region mode (floor + vertical structure)
  • User chose FPFH+RANSAC as optional global pre-alignment (--icp-global-init)
  • User chose to include success gate fix in this plan (not a separate patch)
  • Oracle recommended: lower gravity penalty (10→2), widen correspondence factor (1.4→2.5), lower min_overlap_area (1.0→0.5), add per-pair logging, add robust kernels

Research Findings:

  • Open3D multiway registration: pairwise ICP → pose graph → global optimization (LM)
  • FPFH features + RANSAC for global pre-alignment when init is poor
  • Statistical outlier removal (nb_neighbors=20, std_ratio=2.0) standard for ZED
  • TukeyLoss(k=0.05–0.1) effective for depth sensor noise at 3–5 m range
  • Hybrid floor+structure recommended over pure floor (adds XZ constraints)
  • Voxel size: 0.05 m for global/coarse, 0.02 m for fine refinement

Metis Review

Identified Gaps (addressed):

  • Backward compatibility: --icp-region floor preserves legacy behavior exactly
  • Hybrid degeneracy: if no vertical structure found, gracefully falls back to floor-only with warning
  • FPFH failure fallback: when RANSAC fails, continues with extrinsic-based init (no crash)
  • Determinism: RANSAC seeds controlled for reproducibility; tests use synthetic geometry
  • Ceiling/overhang dominance in full mode: gravity constraint prevents upside-down alignment
  • Symmetric environments: transform sanity bounds catch FPFH mis-registration

Work Objectives

Core Objective

Replace the floor-band-only ICP pipeline with a configurable region selection system (floor/hybrid/full) that provides stronger geometric constraints, better pair convergence, and more reliable multi-camera correction.

Concrete Deliverables

  • Modified aruco/icp_registration.py: new extraction functions, 3D overlap, robust kernel, relaxed defaults, diagnostic logging
  • Modified refine_ground_plane.py: new CLI flags for region, global-init, overlap, band-height
  • New/extended tests/test_icp_registration.py: ~15 new test cases
  • Updated README.md: new flags documented

Definition of Done

  • uv run refine_ground_plane.py --help shows --icp-region, --icp-global-init, --icp-min-overlap, --icp-band-height
  • uv run pytest -x -vv → all tests pass (existing + new)
  • uv run basedpyright aruco/icp_registration.py refine_ground_plane.py → 0 errors
  • --icp-region floor produces identical output to current behavior (regression)
  • --icp-region hybrid produces ≥ as many converged pairs as floor on test data

Must Have

  • Region selection: floor, hybrid, full modes
  • Backward compatibility: floor mode matches current behavior
  • Success gate: num_cameras_optimized > 0 (not > 1)
  • Per-pair diagnostic logging at INFO level
  • Robust kernel (TukeyLoss) support
  • Statistical outlier removal preprocessing
  • 3D AABB overlap check for hybrid/full
  • Optional FPFH+RANSAC global init with clean fallback

Must NOT Have (Guardrails)

  • No changes to HDF5 schema or aruco/depth_save.py
  • No additional global registration methods beyond FPFH+RANSAC
  • No auto-parameter search or adaptive voxel size loops
  • No multiple outlier strategies (SOR only, no radius outlier removal)
  • No multiple robust kernels (TukeyLoss only, no Huber/Cauchy menus)
  • No refactoring of pose graph optimization or information matrix computation
  • No new persistent output files beyond normal console logging
  • No changes to aruco/ground_plane.py core functions (only consume them)

Verification Strategy

UNIVERSAL RULE: ZERO HUMAN INTERVENTION

ALL tasks in this plan MUST be verifiable WITHOUT any human action.

Test Decision

  • Infrastructure exists: YES
  • Automated tests: YES (Tests-after for new functionality)
  • Framework: pytest

Agent-Executed QA Scenarios (MANDATORY — ALL tasks)

Verification Tool by Deliverable Type:

Type Tool How Agent Verifies
CLI flags Bash uv run refine_ground_plane.py --help and grep for flags
Library/Module Bash (pytest) uv run pytest tests/test_icp_registration.py -v
Type safety Bash (basedpyright) uv run basedpyright aruco/icp_registration.py
Regression Bash (pipeline run) Compare outputs with floor-only baseline

Execution Strategy

Parallel Execution Waves

Wave 1 (Start Immediately):
├── Task 1: Fix success gate + per-pair diagnostic logging
├── Task 2: Add point extraction functions (hybrid/full/SOR)
└── Task 3: Add 3D AABB overlap check

Wave 2 (After Wave 1):
├── Task 4: Add TukeyLoss robust kernel support
├── Task 5: Integrate region selection into refine_with_icp
└── Task 6: Add FPFH+RANSAC global pre-alignment

Wave 3 (After Wave 2):
├── Task 7: Wire CLI flags in refine_ground_plane.py
├── Task 8: Relax ICPConfig defaults
└── Task 9: Tests + README + regression verification

Critical Path: Task 1 → Task 2 → Task 5 → Task 7 → Task 9
Parallel Speedup: ~40% faster than sequential

Dependency Matrix

Task Depends On Blocks Can Parallelize With
1 None 5, 9 2, 3
2 None 5 1, 3
3 None 5 1, 2
4 None 5 1, 2, 3
5 1, 2, 3, 4 7 6
6 None 7 4, 5
7 5, 6 9 8
8 None 9 7
9 7, 8 None None (final)

Agent Dispatch Summary

Wave Tasks Recommended Agents
1 1, 2, 3 task(category="quick") for Task 1; task(category="unspecified-high") for Tasks 2, 3
2 4, 5, 6 task(category="unspecified-high") for all three
3 7, 8, 9 task(category="unspecified-high") for Task 7, task(category="quick") for Task 8, task(category="unspecified-high") for Task 9

TODOs

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

    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:

    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:

    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

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
Reference in New Issue View Git Blame Copy Permalink