- 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 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 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).