diff --git a/py_workspace/.sisyphus/notepads/depth-bias-correction/learnings.md b/py_workspace/.sisyphus/notepads/depth-bias-correction/learnings.md new file mode 100644 index 0000000..c316bfa --- /dev/null +++ b/py_workspace/.sisyphus/notepads/depth-bias-correction/learnings.md @@ -0,0 +1,10 @@ +- Implemented `estimate_depth_biases(...)` in `aruco/icp_registration.py` using the same scene extraction + overlap gating pattern as ICP (`overlap_mode` auto-derived from `config.region`, then `compute_overlap_xz`/`compute_overlap_3d` + `min_overlap_area`). +- Pairwise bias observation uses robust medians of scalar residuals projected along source camera rays in world frame: `r = (p_target - p_source) dot ray_source_world`. +- Global solve is constrained least squares over pair equations `beta_j - beta_i ~= b_ij` with gauge fixed by forcing reference camera bias to exactly `0.0`; disconnected cameras remain at fallback `0.0`. +- Safety cap implemented via `max_abs_bias` defaulting to `0.3 m` (read from config via `getattr`), with clipping logs for implausible estimates. +- Task 2 integration in `refine_with_icp`: added config gate `depth_bias` (default `True`) and metrics field `depth_biases` to preserve visibility of applied prepass offsets. +- Refinement loop now applies copy-based depth correction per camera (`depth_corrected = data["depth"].copy()`), adds `beta`, masks non-positive depths to `NaN`, and unprojects from corrected depth without mutating the original input map. +## Patterns and Conventions +- Wired new CLI flags in `refine_ground_plane.py` using click options. +- Extended `_meta.icp_refined` JSON structure to include `depth_bias` config and `depth_biases` metrics. +- Logged estimated biases if available in `ICPMetrics`. diff --git a/py_workspace/refine_ground_plane.py b/py_workspace/refine_ground_plane.py index a56764a..defb171 100644 --- a/py_workspace/refine_ground_plane.py +++ b/py_workspace/refine_ground_plane.py @@ -103,6 +103,56 @@ from aruco.icp_registration import refine_with_icp, ICPConfig default=0.02, help="Voxel size for ICP downsampling (meters).", ) +@click.option( + "--icp-region", + type=click.Choice(["floor", "hybrid", "full"]), + default="hybrid", + help="Region to use for ICP registration.", +) +@click.option( + "--icp-global-init/--no-icp-global-init", + default=False, + help="Enable FPFH+RANSAC global pre-alignment.", +) +@click.option( + "--icp-min-overlap", + type=float, + default=0.5, + help="Minimum overlap area/volume to accept a pair.", +) +@click.option( + "--icp-band-height", + type=float, + default=0.3, + help="Height of the floor band (meters).", +) +@click.option( + "--icp-robust-kernel", + type=click.Choice(["none", "tukey"]), + default="none", + help="Robust kernel for ICP optimization.", +) +@click.option( + "--icp-robust-k", + type=float, + default=0.1, + help="Parameter k for robust kernel.", +) +@click.option( + "--icp-depth-bias/--no-icp-depth-bias", + default=True, + help="Enable per-camera depth bias pre-correction.", +) +@click.option( + "--icp-max-rotation-deg", + default=10.0, + help="Maximum allowed rotation correction for ICP in degrees.", +) +@click.option( + "--icp-max-translation-m", + default=0.3, + help="Maximum allowed translation correction for ICP in meters.", +) @click.option( "--debug/--no-debug", default=False, @@ -124,6 +174,15 @@ def main( icp: bool, icp_method: str, icp_voxel_size: float, + icp_region: str, + icp_global_init: bool, + icp_min_overlap: float, + icp_band_height: float, + icp_robust_kernel: str, + icp_robust_k: float, + icp_depth_bias: bool, + icp_max_rotation_deg: float, + icp_max_translation_m: float, debug: bool, ): """ @@ -211,10 +270,21 @@ def main( # 4.5 Optional ICP Refinement icp_metrics = None if icp: - logger.info(f"Running ICP refinement ({icp_method})...") + logger.info( + f"Running ICP refinement ({icp_method}, region={icp_region})..." + ) icp_config = ICPConfig( method=icp_method, voxel_size=icp_voxel_size, + region=icp_region, + global_init=icp_global_init, + min_overlap_area=icp_min_overlap, + band_height=icp_band_height, + robust_kernel=icp_robust_kernel, + robust_kernel_k=icp_robust_k, + depth_bias=icp_depth_bias, + max_rotation_deg=icp_max_rotation_deg, + max_translation_m=icp_max_translation_m, ) icp_extrinsics, icp_metrics = refine_with_icp( @@ -226,6 +296,10 @@ def main( if icp_metrics.success: logger.info(f"ICP refinement successful: {icp_metrics.message}") + if icp_metrics.depth_biases: + logger.info( + f"Estimated depth biases (m): {icp_metrics.depth_biases}" + ) new_extrinsics = icp_extrinsics else: logger.warning( @@ -288,6 +362,13 @@ def main( "config": { "method": icp_method, "voxel_size": icp_voxel_size, + "region": icp_region, + "global_init": icp_global_init, + "min_overlap": icp_min_overlap, + "band_height": icp_band_height, + "robust_kernel": icp_robust_kernel, + "robust_kernel_k": icp_robust_k, + "depth_bias": icp_depth_bias, }, "metrics": { "success": icp_metrics.success, @@ -295,6 +376,7 @@ def main( "num_pairs_converged": icp_metrics.num_pairs_converged, "num_cameras_optimized": icp_metrics.num_cameras_optimized, "num_disconnected": icp_metrics.num_disconnected, + "depth_biases": icp_metrics.depth_biases, "message": icp_metrics.message, }, }