feat: wire --icp-depth-bias flag in refine_ground_plane.py

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

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,
                 },
             }