chore: checkpoint ground-plane calibration refinement work

py_workspace/calibrate_extrinsics.py

@@ -25,6 +25,7 @@ from aruco.preview import draw_detected_markers, draw_pose_axes, show_preview
 from aruco.depth_verify import verify_extrinsics_with_depth
 from aruco.depth_refine import refine_extrinsics_with_depth
 from aruco.depth_pool import pool_depth_maps
+from aruco.depth_save import save_depth_data
 from aruco.alignment import (
     get_face_normal_from_geometry,
     detect_ground_face,
@@ -128,14 +129,21 @@ def apply_depth_verify_refine_postprocess(
     depth_confidence_threshold: int,
     depth_pool_size: int = 1,
     report_csv_path: Optional[str] = None,
+    save_depth_path: Optional[str] = None,
 ) -> Tuple[Dict[str, Any], List[List[Any]]]:
     """
     Apply depth verification and refinement to computed extrinsics.
     Returns updated results and list of CSV rows.
     """
     csv_rows: List[List[Any]] = []
+    camera_depth_data: Dict[str, Any] = {}
 
     if not (verify_depth or refine_depth):
+        if save_depth_path:
+            click.echo(
+                "Warning: --save-depth ignored because depth verification/refinement is not enabled.",
+                err=True,
+            )
         return results, csv_rows
 
     click.echo("\nRunning depth verification/refinement on computed extrinsics...")
@@ -169,6 +177,19 @@ def apply_depth_verify_refine_postprocess(
         best_vf = valid_frames[0]
         ids = best_vf["ids"]
 
+        # Prepare raw frames data for saving if requested
+        raw_frames_data = []
+        if save_depth_path:
+            for vf in valid_frames:
+                raw_frames_data.append(
+                    {
+                        "frame_index": vf["frame_index"],
+                        "score": vf["score"],
+                        "depth_map": vf["frame"].depth_map,
+                        "confidence_map": vf["frame"].confidence_map,
+                    }
+                )
+
         # Determine if we should pool or use single frame
         use_pooling = depth_pool_size > 1 and len(depth_maps) > 1
 
@@ -304,6 +325,18 @@ def apply_depth_verify_refine_postprocess(
             else:
                 pool_metadata = None
 
+        # Collect data for saving
+        if save_depth_path:
+            h, w = final_depth.shape[:2]
+            camera_depth_data[str(serial)] = {
+                "intrinsics": camera_matrices[serial],
+                "resolution": (w, h),
+                "pooled_depth": final_depth,
+                "pooled_confidence": final_conf,
+                "pool_metadata": pool_metadata,
+                "raw_frames": raw_frames_data,
+            }
+
         # Use the FINAL COMPUTED POSE for verification
         pose_str = results[str(serial)]["pose"]
         T_mean = np.fromstring(pose_str, sep=" ").reshape(4, 4)
@@ -419,6 +452,13 @@ def apply_depth_verify_refine_postprocess(
             writer.writerows(csv_rows)
         click.echo(f"Saved depth verification report to {report_csv_path}")
 
+    if save_depth_path and camera_depth_data:
+        try:
+            save_depth_data(save_depth_path, camera_depth_data)
+            click.echo(f"Saved depth data to {save_depth_path}")
+        except Exception as e:
+            click.echo(f"Error saving depth data: {e}", err=True)
+
     return results, csv_rows
 
 
@@ -612,6 +652,11 @@ def run_benchmark_matrix(
 @click.option(
     "--report-csv", type=click.Path(), help="Optional path for per-frame CSV report."
 )
+@click.option(
+    "--save-depth",
+    type=click.Path(),
+    help="Optional path to save depth data (HDF5) used for verification/refinement.",
+)
 @click.option(
     "--auto-align/--no-auto-align",
     default=False,
@@ -667,6 +712,7 @@ def main(
     depth_confidence_threshold: int,
     depth_pool_size: int,
     report_csv: str | None,
+    save_depth: str | None,
     auto_align: bool,
     ground_face: str | None,
     ground_marker_id: int | None,
@@ -978,6 +1024,7 @@ def main(
         depth_confidence_threshold,
         depth_pool_size,
         report_csv,
+        save_depth,
     )
 
     # 5. Run Benchmark Matrix if requested