test(icp): add comprehensive tests for full-scene ICP pipeline + update docs
This commit is contained in:
@@ -1,54 +1,20 @@
|
||||
- Corrected success gate logic to `> 0`.
|
||||
- Added INFO logging for all attempted ICP pairs.
|
||||
- Ensured all pairs are stored in `metrics.per_pair_results`.
|
||||
- Fixed overlap skip logging to use DEBUG level.
|
||||
- Fixed syntax and indentation errors in `aruco/icp_registration.py` that were causing unreachable code and malformed control flow.
|
||||
- Relaxed success gate to `metrics.num_cameras_optimized > 0`, allowing single-camera optimizations to be considered successful.
|
||||
- Implemented comprehensive per-pair diagnostic logging: INFO for ICP results (fitness, RMSE, convergence) and DEBUG for overlap skips.
|
||||
- Ensured all attempted ICP results are stored in `metrics.per_pair_results` for better downstream diagnostics.
|
||||
- Updated `tests/test_icp_registration.py` to reflect the new success gate logic.
|
||||
## Task 5 Regression Fix
|
||||
- Restored missing Task 6 tests (`test_compute_fpfh_features`, `test_global_registration_known_transform`, `test_refine_with_icp_global_init_*`) that were accidentally overwritten during Task 5 integration.
|
||||
- Verified all 37 tests pass (33 from Task 5 + 4 restored from Task 6).
|
||||
- Confirmed type safety remains clean.
|
||||
|
||||
## Task 3: 3D AABB Overlap Check
|
||||
- Implemented `compute_overlap_3d` in `aruco/icp_registration.py`.
|
||||
- Added `overlap_mode` to `ICPConfig` (defaulting to "xz").
|
||||
- Verified 3D overlap logic with new tests in `tests/test_icp_registration.py`.
|
||||
- Confirmed that empty inputs return 0.0 volume.
|
||||
- Confirmed that disjoint boxes return 0.0 volume.
|
||||
- Confirmed that partial and full overlaps return correct hand-calculable volumes.
|
||||
## Task 7: CLI Flags
|
||||
- Added `--icp-region`, `--icp-global-init`, `--icp-min-overlap`, `--icp-band-height`, `--icp-robust-kernel`, `--icp-robust-k` to `refine_ground_plane.py`.
|
||||
- Verified flags appear in `--help`.
|
||||
- Verified type safety with `basedpyright` (0 errors, only existing warnings).
|
||||
- Flags are correctly passed to `ICPConfig` and recorded in output JSON metadata.
|
||||
|
||||
## Task 2: Point Extraction & Preprocessing
|
||||
- Implemented `extract_scene_points` with floor, hybrid, and full modes.
|
||||
- Implemented `preprocess_point_cloud` with statistical outlier removal (SOR).
|
||||
- Added `region` field to `ICPConfig` dataclass.
|
||||
- Added comprehensive tests for new extraction modes and preprocessing.
|
||||
- Verified backward compatibility for floor mode.
|
||||
- Verified hybrid mode behavior (vertical structure inclusion and fallback).
|
||||
- Verified full mode behavior.
|
||||
- Verified SOR preprocessing effectiveness.
|
||||
|
||||
## Task 4: TukeyLoss Robust Kernel Support
|
||||
- Added `robust_kernel` and `robust_kernel_k` to `ICPConfig`.
|
||||
- Implemented TukeyLoss application in `pairwise_icp` for both Point-to-Plane and Generalized ICP.
|
||||
- Verified that TukeyLoss correctly handles outliers in synthetic tests, maintaining convergence accuracy.
|
||||
- Default behavior remains backward-compatible with `robust_kernel="none"`.
|
||||
|
||||
## Task 6: FPFH+RANSAC Global Pre-alignment
|
||||
- Implemented `compute_fpfh_features` and `global_registration` using Open3D RANSAC.
|
||||
- Added `global_init` flag to `ICPConfig` (default False).
|
||||
- Integrated global registration into `refine_with_icp` as a pre-alignment step before pairwise ICP.
|
||||
- Added safety checks: global registration result is only used if fitness > 0.1 and the resulting transform is within `max_rotation_deg` and `max_translation_m` bounds relative to the initial extrinsic guess.
|
||||
- Verified with synthetic tests:
|
||||
- `test_compute_fpfh_features`: Validates feature dimension and count.
|
||||
- `test_global_registration_known_transform`: Confirms RANSAC can recover a known large transform (30 deg rotation).
|
||||
- `test_refine_with_icp_global_init_success`: End-to-end test showing global init can recover from a very bad initial guess (90 deg error) where local ICP would fail.
|
||||
|
||||
## Task 8: Relax ICPConfig defaults
|
||||
- Relaxed defaults for ICPConfig to improve convergence and allow more flexible corrections.
|
||||
- New defaults:
|
||||
- min_fitness: 0.15
|
||||
- min_overlap_area: 0.5
|
||||
- gravity_penalty_weight: 2.0
|
||||
- max_correspondence_distance_factor: 2.5
|
||||
- max_translation_m: 0.3
|
||||
- max_rotation_deg: 10.0
|
||||
- Verified with 36 passing tests and clean basedpyright (0 errors, though many warnings due to missing stubs).
|
||||
## Task 9: Final Verification
|
||||
- Added comprehensive tests for full-scene ICP pipeline:
|
||||
- `test_success_gate_single_camera`: verified relaxed success gate (>0).
|
||||
- `test_per_pair_logging_all_pairs`: verified all pairs logged regardless of convergence.
|
||||
- Restored missing regression tests for floor/hybrid modes and SOR preprocessing.
|
||||
- Updated README.md with new CLI flags and hybrid mode example.
|
||||
- Verified full regression suite (129 tests passed).
|
||||
- Confirmed `basedpyright` clean on modified files.
|
||||
- Note: `test_per_pair_logging_all_pairs` required mocking `unproject_depth_to_points` to return enough points (200) to pass the `len(pcd.points) < 100` check in `refine_with_icp`.
|
||||
|
||||
@@ -464,7 +464,7 @@ Parallel Speedup: ~40% faster than sequential
|
||||
|
||||
---
|
||||
|
||||
- [ ] 5. Integrate region selection into refine_with_icp
|
||||
- [x] 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`
|
||||
@@ -622,7 +622,7 @@ Parallel Speedup: ~40% faster than sequential
|
||||
|
||||
---
|
||||
|
||||
- [ ] 7. Wire CLI flags in refine_ground_plane.py
|
||||
- [x] 7. Wire CLI flags in refine_ground_plane.py
|
||||
|
||||
**What to do**:
|
||||
- Add CLI options to `refine_ground_plane.py`:
|
||||
@@ -759,7 +759,7 @@ Parallel Speedup: ~40% faster than sequential
|
||||
|
||||
---
|
||||
|
||||
- [ ] 9. Tests + README + regression verification
|
||||
- [x] 9. Tests + README + regression verification
|
||||
|
||||
**What to do**:
|
||||
- Add new tests to `tests/test_icp_registration.py`:
|
||||
@@ -811,11 +811,11 @@ Parallel Speedup: ~40% faster than sequential
|
||||
|
||||
**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
|
||||
- [x] ≥12 new test cases added
|
||||
- [x] `uv run pytest tests/test_icp_registration.py -v` → all pass (existing + new)
|
||||
- [x] `uv run pytest -x -vv` → all pass (full suite)
|
||||
- [x] `uv run basedpyright aruco/icp_registration.py refine_ground_plane.py` → 0 errors
|
||||
- [x] README.md documents all new CLI flags with examples
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
@@ -881,9 +881,9 @@ uv run basedpyright aruco/icp_registration.py refine_ground_plane.py # Expected
|
||||
```
|
||||
|
||||
### 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
|
||||
- [x] All "Must Have" present
|
||||
- [x] All "Must NOT Have" absent
|
||||
- [x] All tests pass
|
||||
- [x] `--icp-region floor` backward compatible
|
||||
- [x] `--icp-region hybrid` default in CLI
|
||||
- [x] README documents all new flags
|
||||
|
||||
Reference in New Issue
Block a user