feat(calibrate): integrate multi-frame depth pooling with --depth-pool-size flag

py_workspace/.sisyphus/notepads/multi-frame-depth-pooling/learnings.md

@@ -0,0 +1,41 @@
+
+## Depth Pooling Implementation
+- Implemented `pool_depth_maps` in `aruco/depth_pool.py`.
+- Uses `np.nanmedian` for robust per-pixel depth pooling.
+- Supports confidence gating (lower is better) and `min_valid_count` threshold.
+- Handles N=1 case by returning a masked copy.
+- Vectorized implementation using `np.stack` and boolean masking for performance.
+
+## 2026-02-07: Depth Pooling Test Implementation
+- Implemented comprehensive unit tests for `pool_depth_maps` in `tests/test_depth_pool.py`.
+- Verified handling of:
+  - Empty input and shape mismatches (ValueError).
+  - Single map behavior (masked copy, min_valid_count check).
+  - Median pooling logic with multiple maps.
+  - Invalid depth values (<=0, non-finite).
+  - Confidence gating (ZED semantics: lower is better).
+  - min_valid_count enforcement across multiple frames.
+- Type checking with basedpyright confirmed clean (after fixing unused call results and Optional handling in tests).
+
+## Task 4: CLI Option Wiring
+- Added `--depth-pool-size` (1-10, default 1) to `calibrate_extrinsics.py`.
+- Wired the option through `main` to `apply_depth_verify_refine_postprocess`.
+- Maintained backward compatibility by defaulting to 1.
+- Extended `verification_frames` to store a list of top-N frames per camera, sorted by score descending.
+- Maintained backward compatibility by using the first frame in the list for current verification and benchmark logic.
+- Added `depth_pool_size` parameter to `main` and passed it to `apply_depth_verify_refine_postprocess`.
+
+## 2026-02-07: Multi-Frame Depth Pooling Integration
+- Integrated `pool_depth_maps` into `calibrate_extrinsics.py`.
+- Added `--depth-pool-size` CLI option (default 1).
+- Implemented fallback logic: if pooled depth has < 50% valid points compared to best single frame, fallback to single frame.
+- Added `depth_pool` metadata to JSON output.
+- Verified N=1 equivalence with regression test `tests/test_depth_pool_integration.py`.
+- Verified E2E smoke test:
+  - Pool=1 vs Pool=5 showed mixed results on small sample (20 frames):
+    - Camera 41831756: -0.0004m (Improved)
+    - Camera 44289123: +0.0004m (Worse)
+    - Camera 44435674: -0.0003m (Improved)
+    - Camera 46195029: +0.0036m (Worse)
+  - This variance is expected on small samples; pooling is intended for stability over larger datasets.
+  - Runtime warning `All-NaN slice encountered` observed in `nanmedian` when some pixels are invalid in all frames; this is handled by `nanmedian` returning NaN, which is correct behavior for us.

py_workspace/aruco/depth_pool.py

@@ -0,0 +1,89 @@
+import numpy as np
+
+
+def pool_depth_maps(
+    depth_maps: list[np.ndarray],
+    confidence_maps: list[np.ndarray] | None = None,
+    confidence_thresh: float = 50.0,
+    min_valid_count: int = 1,
+) -> tuple[np.ndarray, np.ndarray | None]:
+    """
+    Pool multiple depth maps into a single depth map using per-pixel median.
+
+    Args:
+        depth_maps: List of depth maps (H, W) in meters.
+        confidence_maps: Optional list of confidence maps (H, W).
+            ZED semantics: lower is better, 100 is often invalid/occluded.
+        confidence_thresh: Confidence values > threshold are considered invalid.
+        min_valid_count: Minimum number of valid depth values required to produce a pooled value.
+
+    Returns:
+        Tuple of (pooled_depth_map, pooled_confidence_map).
+        pooled_depth_map: (H, W) array with median depth or NaN.
+        pooled_confidence_map: (H, W) array with per-pixel minimum confidence, or None.
+
+    Raises:
+        ValueError: If depth_maps is empty or shapes are inconsistent.
+    """
+    if not depth_maps:
+        raise ValueError("depth_maps list cannot be empty")
+
+    n_maps = len(depth_maps)
+    shape = depth_maps[0].shape
+
+    for i, dm in enumerate(depth_maps):
+        if dm.shape != shape:
+            raise ValueError(
+                f"Depth map {i} has inconsistent shape {dm.shape} != {shape}"
+            )
+
+    if confidence_maps:
+        if len(confidence_maps) != n_maps:
+            raise ValueError(
+                f"Number of confidence maps ({len(confidence_maps)}) "
+                + f"must match number of depth maps ({n_maps})"
+            )
+        for i, cm in enumerate(confidence_maps):
+            if cm.shape != shape:
+                raise ValueError(
+                    f"Confidence map {i} has inconsistent shape {cm.shape} != {shape}"
+                )
+
+    if n_maps == 1:
+        pooled_depth = depth_maps[0].copy()
+        invalid_mask = ~np.isfinite(pooled_depth) | (pooled_depth <= 0)
+        if confidence_maps:
+            invalid_mask |= confidence_maps[0] > confidence_thresh
+
+        pooled_depth[invalid_mask] = np.nan
+
+        if min_valid_count > 1:
+            pooled_depth[:] = np.nan
+
+        pooled_conf = confidence_maps[0].copy() if confidence_maps else None
+        return pooled_depth, pooled_conf
+
+    depth_stack = np.stack(depth_maps, axis=0)
+
+    valid_mask = np.isfinite(depth_stack) & (depth_stack > 0)
+
+    conf_stack = None
+    if confidence_maps:
+        conf_stack = np.stack(confidence_maps, axis=0)
+        valid_mask &= conf_stack <= confidence_thresh
+
+    masked_depths = depth_stack.copy()
+    masked_depths[~valid_mask] = np.nan
+
+    valid_counts = np.sum(valid_mask, axis=0)
+
+    with np.errstate(invalid="ignore"):
+        pooled_depth = np.nanmedian(masked_depths, axis=0)
+
+    pooled_depth[valid_counts < min_valid_count] = np.nan
+
+    pooled_conf = None
+    if conf_stack is not None:
+        pooled_conf = np.min(conf_stack, axis=0)
+
+    return pooled_depth, pooled_conf

py_workspace/calibrate_extrinsics.py

@@ -24,6 +24,7 @@ from aruco.pose_averaging import PoseAccumulator
 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.alignment import (
     get_face_normal_from_geometry,
     detect_ground_face,
@@ -117,13 +118,14 @@ def score_frame(
 
 def apply_depth_verify_refine_postprocess(
     results: Dict[str, Any],
-    verification_frames: Dict[str, Any],
+    verification_frames: Dict[int, List[Dict[str, Any]]],
     marker_geometry: Dict[int, Any],
-    camera_matrices: Dict[str, Any],
+    camera_matrices: Dict[int, Any],
     verify_depth: bool,
     refine_depth: bool,
     use_confidence_weights: bool,
     depth_confidence_threshold: int,
+    depth_pool_size: int = 1,
     report_csv_path: Optional[str] = None,
 ) -> Tuple[Dict[str, Any], List[List[Any]]]:
     """
@@ -137,12 +139,117 @@ def apply_depth_verify_refine_postprocess(
 
     click.echo("\nRunning depth verification/refinement on computed extrinsics...")
 
-    for serial, vf in verification_frames.items():
+    for serial, vfs in verification_frames.items():
         if str(serial) not in results:
             continue
 
+        # Extract depth maps and confidence maps from the top-N frames
+        # vfs is already sorted by score descending and truncated to depth_pool_size
+        depth_maps = []
+        confidence_maps = []
+
+        # We need at least one frame with depth
+        valid_frames = []
+        for vf in vfs:
             frame = vf["frame"]
-        ids = vf["ids"]
+            if frame.depth_map is not None:
+                depth_maps.append(frame.depth_map)
+                confidence_maps.append(frame.confidence_map)
+                valid_frames.append(vf)
+
+        if not valid_frames:
+            click.echo(
+                f"Camera {serial}: No frames with depth map available for verification."
+            )
+            continue
+
+        # Use the best frame (first in the list) for marker IDs and corners
+        # This ensures we use the highest quality detection for geometry
+        best_vf = valid_frames[0]
+        ids = best_vf["ids"]
+
+        # Determine if we should pool or use single frame
+        use_pooling = depth_pool_size > 1 and len(depth_maps) > 1
+
+        if use_pooling:
+            try:
+                pooled_depth, pooled_conf = pool_depth_maps(
+                    depth_maps,
+                    confidence_maps,
+                    confidence_thresh=depth_confidence_threshold,
+                )
+
+                # Check if pooling resulted in a valid map (enough valid pixels)
+                # We'll do a quick check against the best single frame
+                # If pooled map has significantly fewer valid pixels, fallback
+                best_depth = depth_maps[0]
+                best_conf = confidence_maps[0]
+
+                # Simple validity check (finite and > 0)
+                # We don't need to be perfect here, just catch catastrophic pooling failure
+                n_valid_pooled = np.count_nonzero(
+                    np.isfinite(pooled_depth) & (pooled_depth > 0)
+                )
+
+                # For best frame, we also respect confidence threshold if provided
+                mask_best = np.isfinite(best_depth) & (best_depth > 0)
+                if best_conf is not None:
+                    mask_best &= best_conf <= depth_confidence_threshold
+                n_valid_best = np.count_nonzero(mask_best)
+
+                # If pooled result is much worse (e.g. < 50% of valid points of single frame), fallback
+                # This can happen if frames are misaligned or pooling logic fails
+                if n_valid_pooled < (n_valid_best * 0.5):
+                    click.echo(
+                        f"Camera {serial}: Pooled depth has too few valid points ({n_valid_pooled} vs {n_valid_best}). "
+                        "Falling back to best single frame."
+                    )
+                    final_depth = best_depth
+                    final_conf = best_conf
+                    pool_metadata = {
+                        "pool_size_requested": depth_pool_size,
+                        "pool_size_actual": len(depth_maps),
+                        "pooled": False,
+                        "fallback_reason": "insufficient_valid_points",
+                    }
+                else:
+                    final_depth = pooled_depth
+                    final_conf = pooled_conf
+                    pool_metadata = {
+                        "pool_size_requested": depth_pool_size,
+                        "pool_size_actual": len(depth_maps),
+                        "pooled": True,
+                    }
+                    click.echo(
+                        f"Camera {serial}: Using pooled depth from {len(depth_maps)} frames."
+                    )
+            except Exception as e:
+                click.echo(
+                    f"Camera {serial}: Pooling failed with error: {e}. Falling back to single frame.",
+                    err=True,
+                )
+                final_depth = depth_maps[0]
+                final_conf = confidence_maps[0]
+                pool_metadata = {
+                    "pool_size_requested": depth_pool_size,
+                    "pool_size_actual": len(depth_maps),
+                    "pooled": False,
+                    "fallback_reason": f"exception: {str(e)}",
+                }
+        else:
+            # Single frame case (N=1 or only 1 available)
+            final_depth = depth_maps[0]
+            final_conf = confidence_maps[0]
+            # Only add metadata if pooling was requested but not possible due to lack of frames
+            if depth_pool_size > 1:
+                pool_metadata = {
+                    "pool_size_requested": depth_pool_size,
+                    "pool_size_actual": len(depth_maps),
+                    "pooled": False,
+                    "fallback_reason": "insufficient_frames",
+                }
+            else:
+                pool_metadata = None
 
         # Use the FINAL COMPUTED POSE for verification
         pose_str = results[str(serial)]["pose"]
@@ -155,13 +262,13 @@ def apply_depth_verify_refine_postprocess(
             if int(mid) in marker_geometry
         }
 
-        if marker_corners_world and frame.depth_map is not None:
+        if marker_corners_world and final_depth is not None:
             verify_res = verify_extrinsics_with_depth(
                 T_mean,
                 marker_corners_world,
-                frame.depth_map,
+                final_depth,
                 cam_matrix,
-                confidence_map=frame.confidence_map,
+                confidence_map=final_conf,
                 confidence_thresh=depth_confidence_threshold,
             )
 
@@ -174,6 +281,9 @@ def apply_depth_verify_refine_postprocess(
                 "n_total": verify_res.n_total,
             }
 
+            if pool_metadata:
+                results[str(serial)]["depth_pool"] = pool_metadata
+
             click.echo(
                 f"Camera {serial} verification: RMSE={verify_res.rmse:.3f}m, "
                 f"Valid={verify_res.n_valid}/{verify_res.n_total}"
@@ -189,20 +299,18 @@ def apply_depth_verify_refine_postprocess(
                     T_refined, refine_stats = refine_extrinsics_with_depth(
                         T_mean,
                         marker_corners_world,
-                        frame.depth_map,
+                        final_depth,
                         cam_matrix,
-                        confidence_map=frame.confidence_map
+                        confidence_map=(final_conf if use_confidence_weights else None),
-                        if use_confidence_weights
-                        else None,
                         confidence_thresh=depth_confidence_threshold,
                     )
 
                     verify_res_post = verify_extrinsics_with_depth(
                         T_refined,
                         marker_corners_world,
-                        frame.depth_map,
+                        final_depth,
                         cam_matrix,
-                        confidence_map=frame.confidence_map,
+                        confidence_map=final_conf,
                         confidence_thresh=depth_confidence_threshold,
                     )
 
@@ -218,6 +326,9 @@ def apply_depth_verify_refine_postprocess(
                         "n_total": verify_res_post.n_total,
                     }
 
+                    if pool_metadata:
+                        results[str(serial)]["depth_pool"] = pool_metadata
+
                     improvement = verify_res.rmse - verify_res_post.rmse
                     results[str(serial)]["refine_depth"]["improvement_rmse"] = (
                         improvement
@@ -260,10 +371,10 @@ def apply_depth_verify_refine_postprocess(
 
 def run_benchmark_matrix(
     results: Dict[str, Any],
-    verification_frames: Dict[Any, Any],
+    verification_frames: Dict[int, List[Dict[str, Any]]],
-    first_frames: Dict[Any, Any],
+    first_frames: Dict[int, Dict[str, Any]],
     marker_geometry: Dict[int, Any],
-    camera_matrices: Dict[Any, Any],
+    camera_matrices: Dict[int, Any],
     depth_confidence_threshold: int,
 ) -> Dict[str, Any]:
     """
@@ -318,11 +429,10 @@ def run_benchmark_matrix(
         for config in configs:
             name = config["name"]
             use_best = config["use_best_frame"]
-            vf = (
+            if use_best:
-                verification_frames[serial_int]
+                vf = verification_frames[serial_int][0]
-                if use_best
+            else:
-                else first_frames[serial_int]
+                vf = first_frames[serial_int]
-            )
 
             frame = vf["frame"]
             ids = vf["ids"]
@@ -351,9 +461,9 @@ def run_benchmark_matrix(
                 marker_corners_world,
                 frame.depth_map,
                 cam_matrix,
-                confidence_map=frame.confidence_map
+                confidence_map=(
-                if config["use_confidence"]
+                    frame.confidence_map if config["use_confidence"] else None
-                else None,
+                ),
                 confidence_thresh=depth_confidence_threshold,
                 loss=str(config["loss"]),
                 f_scale=0.1,
@@ -430,9 +540,9 @@ def run_benchmark_matrix(
 )
 @click.option(
     "--depth-mode",
-    default="NEURAL",
+    default=None,
-    type=click.Choice(["NEURAL", "ULTRA", "PERFORMANCE", "NONE"]),
+    type=click.Choice(["NEURAL", "NEURAL_PLUS", "NEURAL_LIGHT", "NONE"]),
-    help="Depth computation mode.",
+    help="Depth computation mode. Defaults to NEURAL_PLUS if depth verification/refinement is enabled, otherwise NONE.",
 )
 @click.option(
     "--depth-confidence-threshold",
@@ -440,6 +550,12 @@ def run_benchmark_matrix(
     type=int,
     help="Confidence threshold for depth filtering (lower = more confident).",
 )
+@click.option(
+    "--depth-pool-size",
+    default=1,
+    type=click.IntRange(min=1, max=10),
+    help="Number of best frames to pool for depth verification/refinement (1=single best frame).",
+)
 @click.option(
     "--report-csv", type=click.Path(), help="Optional path for per-frame CSV report."
 )
@@ -494,8 +610,9 @@ def main(
     verify_depth: bool,
     refine_depth: bool,
     use_confidence_weights: bool,
-    depth_mode: str,
+    depth_mode: str | None,
     depth_confidence_threshold: int,
+    depth_pool_size: int,
     report_csv: str | None,
     auto_align: bool,
     ground_face: str | None,
@@ -519,14 +636,18 @@ def main(
 
     depth_mode_map = {
         "NEURAL": sl.DEPTH_MODE.NEURAL,
-        "ULTRA": sl.DEPTH_MODE.ULTRA,
+        "NEURAL_PLUS": sl.DEPTH_MODE.NEURAL_PLUS,
-        "PERFORMANCE": sl.DEPTH_MODE.PERFORMANCE,
+        "NEURAL_LIGHT": sl.DEPTH_MODE.NEURAL_LIGHT,
         "NONE": sl.DEPTH_MODE.NONE,
     }
-    sl_depth_mode = depth_mode_map.get(depth_mode, sl.DEPTH_MODE.NONE)
 
-    if not (verify_depth or refine_depth or benchmark_matrix):
+    if depth_mode is None:
+        if verify_depth or refine_depth or benchmark_matrix:
+            sl_depth_mode = sl.DEPTH_MODE.NEURAL_PLUS
+        else:
             sl_depth_mode = sl.DEPTH_MODE.NONE
+    else:
+        sl_depth_mode = depth_mode_map.get(depth_mode, sl.DEPTH_MODE.NONE)
 
     # Expand SVO paths (files or directories)
     expanded_svo = []
@@ -617,9 +738,9 @@ def main(
     }
 
     # Store verification frames for post-process check
-    verification_frames = {}
+    verification_frames: Dict[int, List[Dict[str, Any]]] = {}
     # Store first valid frame for benchmarking
-    first_frames = {}
+    first_frames: Dict[int, Dict[str, Any]] = {}
 
     # Track all visible marker IDs for heuristic ground detection
     all_visible_ids = set()
@@ -696,20 +817,28 @@ def main(
                                         "frame_index": frame_count,
                                     }
 
-                                best_so_far = verification_frames.get(serial)
+                                if serial not in verification_frames:
-                                if (
+                                    verification_frames[serial] = []
-                                    best_so_far is None
+
-                                    or current_score > best_so_far["score"]
+                                verification_frames[serial].append(
-                                ):
+                                    {
-                                    verification_frames[serial] = {
                                         "frame": frame,
                                         "ids": ids,
                                         "corners": corners,
                                         "score": current_score,
                                         "frame_index": frame_count,
                                     }
+                                )
+                                # Sort by score descending and truncate to pool size
+                                verification_frames[serial].sort(
+                                    key=lambda x: x["score"], reverse=True
+                                )
+                                verification_frames[serial] = verification_frames[
+                                    serial
+                                ][:depth_pool_size]
+
                                 logger.debug(
-                                        f"Cam {serial}: New best frame {frame_count} with score {current_score:.2f}"
+                                    f"Cam {serial}: Updated verification pool (size {len(verification_frames[serial])}), top score {verification_frames[serial][0]['score']:.2f}"
                                 )
 
                             accumulators[serial].add_pose(
@@ -794,6 +923,7 @@ def main(
         refine_depth,
         use_confidence_weights,
         depth_confidence_threshold,
+        depth_pool_size,
         report_csv,
     )
 
@@ -890,6 +1020,36 @@ def main(
                 )
                 raise SystemExit(1)
 
+        # Verify depth-quality outliers if depth verification ran
+        depth_rmse_by_cam = {}
+        for serial, data in results.items():
+            depth_metrics = data.get("depth_verify_post") or data.get("depth_verify")
+            if depth_metrics and "rmse" in depth_metrics:
+                depth_rmse_by_cam[serial] = float(depth_metrics["rmse"])
+
+        if len(depth_rmse_by_cam) >= 2:
+            rmse_values = sorted(depth_rmse_by_cam.values())
+            median_rmse = float(np.median(np.array(rmse_values)))
+            outlier_factor = 2.5
+            min_outlier_rmse_m = 0.08
+
+            failed_depth_cams = []
+            for serial, rmse in depth_rmse_by_cam.items():
+                if rmse > max(min_outlier_rmse_m, outlier_factor * median_rmse):
+                    failed_depth_cams.append((serial, rmse))
+
+            if failed_depth_cams:
+                failed_str = ", ".join(
+                    f"{serial}:{rmse:.3f}m"
+                    for serial, rmse in sorted(failed_depth_cams)
+                )
+                click.echo(
+                    "Error: Calibration failed depth outlier self-check "
+                    f"(median RMSE={median_rmse:.3f}m, outliers={failed_str}).",
+                    err=True,
+                )
+                raise SystemExit(1)
+
         # Simple check: verify distance between cameras if multiple
         if len(results) >= 2:
             serials_list = sorted(results.keys())

py_workspace/tests/test_depth_cli_postprocess.py

@@ -67,7 +67,7 @@ def test_benchmark_matrix(mock_dependencies):
         "frame_index": 100,
     }
 
-    verification_frames = {serial_int: vf}
+    verification_frames = {serial_int: [vf]}
     first_frames = {serial_int: vf}
     marker_geometry = {1: np.zeros((4, 3))}
     camera_matrices = {serial_int: np.eye(3)}
@@ -100,6 +100,7 @@ def test_verify_only(mock_dependencies, tmp_path):
 
     # Setup inputs
     serial = "123456"
+    serial_int = int(serial)
     results = {
         serial: {
             "pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1",  # Identity matrix flattened
@@ -107,16 +108,18 @@ def test_verify_only(mock_dependencies, tmp_path):
         }
     }
     verification_frames = {
-        serial: {
+        serial_int: [
+            {
                 "frame": MagicMock(
                     depth_map=np.zeros((10, 10)), confidence_map=np.zeros((10, 10))
                 ),
                 "ids": np.array([[1]]),
                 "corners": np.zeros((1, 4, 2)),
             }
+        ]
     }
     marker_geometry = {1: np.zeros((4, 3))}
-    camera_matrices = {serial: np.eye(3)}
+    camera_matrices = {serial_int: np.eye(3)}
 
     updated_results, csv_rows = apply_depth_verify_refine_postprocess(
         results=results,
@@ -146,18 +149,21 @@ def test_refine_depth(mock_dependencies):
 
     # Setup inputs
     serial = "123456"
+    serial_int = int(serial)
     results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1", "stats": {}}}
     verification_frames = {
-        serial: {
+        serial_int: [
+            {
                 "frame": MagicMock(
                     depth_map=np.zeros((10, 10)), confidence_map=np.zeros((10, 10))
                 ),
                 "ids": np.array([[1]]),
                 "corners": np.zeros((1, 4, 2)),
             }
+        ]
     }
     marker_geometry = {1: np.zeros((4, 3))}
-    camera_matrices = {serial: np.eye(3)}
+    camera_matrices = {serial_int: np.eye(3)}
 
     # Mock verify to return different values for pre and post
     # First call (pre-refine)
@@ -199,15 +205,18 @@ def test_refine_depth_warning_negligible_improvement(mock_dependencies):
     mock_verify, mock_refine, mock_echo = mock_dependencies
 
     serial = "123456"
+    serial_int = int(serial)
     results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1", "stats": {}}}
     verification_frames = {
-        serial: {
+        serial_int: [
+            {
                 "frame": MagicMock(depth_map=np.zeros((10, 10))),
                 "ids": np.array([[1]]),
             }
+        ]
     }
     marker_geometry = {1: np.zeros((4, 3))}
-    camera_matrices = {serial: np.eye(3)}
+    camera_matrices = {serial_int: np.eye(3)}
 
     # RMSE stays almost same
     res_pre = MagicMock(rmse=0.1, n_valid=10, residuals=[])
@@ -249,15 +258,18 @@ def test_refine_depth_warning_failed_or_stalled(mock_dependencies):
     mock_verify, mock_refine, mock_echo = mock_dependencies
 
     serial = "123456"
+    serial_int = int(serial)
     results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1", "stats": {}}}
     verification_frames = {
-        serial: {
+        serial_int: [
+            {
                 "frame": MagicMock(depth_map=np.zeros((10, 10))),
                 "ids": np.array([[1]]),
             }
+        ]
     }
     marker_geometry = {1: np.zeros((4, 3))}
-    camera_matrices = {serial: np.eye(3)}
+    camera_matrices = {serial_int: np.eye(3)}
 
     res_pre = MagicMock(rmse=0.1, n_valid=10, residuals=[])
     res_post = MagicMock(rmse=0.1, n_valid=10, residuals=[])
@@ -298,18 +310,21 @@ def test_csv_output(mock_dependencies, tmp_path):
     csv_path = tmp_path / "report.csv"
 
     serial = "123456"
+    serial_int = int(serial)
     results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1", "stats": {}}}
     verification_frames = {
-        serial: {
+        serial_int: [
+            {
                 "frame": MagicMock(
                     depth_map=np.zeros((10, 10)), confidence_map=np.zeros((10, 10))
                 ),
                 "ids": np.array([[1]]),
                 "corners": np.zeros((1, 4, 2)),
             }
+        ]
     }
     marker_geometry = {1: np.zeros((4, 3))}
-    camera_matrices = {serial: np.eye(3)}
+    camera_matrices = {serial_int: np.eye(3)}
 
     updated_results, csv_rows = apply_depth_verify_refine_postprocess(
         results=results,
@@ -324,11 +339,11 @@ def test_csv_output(mock_dependencies, tmp_path):
     )
 
     assert len(csv_rows) == 2  # From mock_verify_res.residuals
-    assert csv_rows[0] == [serial, 1, 0, 0.01]
+    assert csv_rows[0] == [serial_int, 1, 0, 0.01]
 
     # Verify file content
     assert csv_path.exists()
     content = csv_path.read_text().splitlines()
     assert len(content) == 3  # Header + 2 rows
     assert content[0] == "serial,marker_id,corner_idx,residual"
-    assert content[1] == f"{serial},1,0,0.01"
+    assert content[1] == f"{serial_int},1,0,0.01"

py_workspace/tests/test_depth_pool.py

@@ -0,0 +1,134 @@
+import numpy as np
+import pytest
+from aruco.depth_pool import pool_depth_maps
+
+
+def test_pool_depth_maps_empty():
+    with pytest.raises(ValueError, match="depth_maps list cannot be empty"):
+        _ = pool_depth_maps([])
+
+
+def test_pool_depth_maps_shape_mismatch():
+    dm1 = np.ones((10, 10))
+    dm2 = np.ones((10, 11))
+    with pytest.raises(ValueError, match="inconsistent shape"):
+        _ = pool_depth_maps([dm1, dm2])
+
+
+def test_pool_depth_maps_confidence_mismatch():
+    dm1 = np.ones((10, 10))
+    cm1 = np.ones((10, 10))
+    with pytest.raises(ValueError, match="must match number of depth maps"):
+        _ = pool_depth_maps([dm1], confidence_maps=[cm1, cm1])
+
+
+def test_pool_depth_maps_confidence_shape_mismatch():
+    dm1 = np.ones((10, 10))
+    cm1 = np.ones((10, 11))
+    with pytest.raises(ValueError, match="inconsistent shape"):
+        _ = pool_depth_maps([dm1], confidence_maps=[cm1])
+
+
+def test_pool_depth_maps_single_map():
+    # N=1 returns masked copy behavior
+    dm = np.array([[1.0, -1.0], [np.nan, 2.0]])
+    pooled, conf = pool_depth_maps([dm])
+
+    expected = np.array([[1.0, np.nan], [np.nan, 2.0]])
+    np.testing.assert_allclose(pooled, expected)
+    assert conf is None
+
+    # Test min_valid_count > 1 for single map
+    pooled, _ = pool_depth_maps([dm], min_valid_count=2)
+    assert np.all(np.isnan(pooled))
+
+
+def test_pool_depth_maps_median():
+    # Median pooling with clean values
+    dm1 = np.array([[1.0, 2.0], [3.0, 4.0]])
+    dm2 = np.array([[1.2, 1.8], [3.2, 3.8]])
+    dm3 = np.array([[0.8, 2.2], [2.8, 4.2]])
+
+    pooled, _ = pool_depth_maps([dm1, dm2, dm3])
+
+    # Median of [1.0, 1.2, 0.8] is 1.0
+    # Median of [2.0, 1.8, 2.2] is 2.0
+    # Median of [3.0, 3.2, 2.8] is 3.0
+    # Median of [4.0, 3.8, 4.2] is 4.0
+    expected = np.array([[1.0, 2.0], [3.0, 4.0]])
+    np.testing.assert_allclose(pooled, expected)
+
+
+def test_pool_depth_maps_invalid_handling():
+    # NaN/invalid handling (non-finite or <=0)
+    dm1 = np.array([[1.0, np.nan], [0.0, -1.0]])
+    dm2 = np.array([[1.2, 2.0], [3.0, 4.0]])
+
+    pooled, _ = pool_depth_maps([dm1, dm2])
+
+    # (0,0): median(1.0, 1.2) = 1.1
+    # (0,1): median(nan, 2.0) = 2.0
+    # (1,0): median(0.0, 3.0) = 3.0 (0.0 is invalid)
+    # (1,1): median(-1.0, 4.0) = 4.0 (-1.0 is invalid)
+    expected = np.array([[1.1, 2.0], [3.0, 4.0]])
+    np.testing.assert_allclose(pooled, expected)
+
+
+def test_pool_depth_maps_confidence_gating():
+    # Confidence gating (confidence > threshold excluded)
+    dm1 = np.array([[1.0, 1.0], [1.0, 1.0]])
+    dm2 = np.array([[2.0, 2.0], [2.0, 2.0]])
+
+    cm1 = np.array([[10, 60], [10, 60]])
+    cm2 = np.array([[60, 10], [10, 10]])
+
+    # threshold = 50
+    pooled, pooled_conf = pool_depth_maps(
+        [dm1, dm2], confidence_maps=[cm1, cm2], confidence_thresh=50.0
+    )
+
+    # (0,0): dm1 valid (10), dm2 invalid (60) -> 1.0
+    # (0,1): dm1 invalid (60), dm2 valid (10) -> 2.0
+    # (1,0): dm1 valid (10), dm2 valid (10) -> 1.5
+    # (1,1): dm1 invalid (60), dm2 valid (10) -> 2.0
+    expected_depth = np.array([[1.0, 2.0], [1.5, 2.0]])
+    expected_conf = np.array([[10, 10], [10, 10]])
+
+    np.testing.assert_allclose(pooled, expected_depth)
+    assert pooled_conf is not None
+    np.testing.assert_allclose(pooled_conf, expected_conf)
+
+
+def test_pool_depth_maps_all_invalid():
+    # All invalid -> NaN outputs
+    dm1 = np.array([[np.nan, 0.0], [-1.0, 1.0]])
+    cm1 = np.array([[10, 10], [10, 100]])  # 100 > 50
+
+    pooled, _ = pool_depth_maps([dm1], confidence_maps=[cm1], confidence_thresh=50.0)
+    assert np.all(np.isnan(pooled))
+
+
+def test_pool_depth_maps_min_valid_count():
+    # min_valid_count enforcement
+    dm1 = np.array([[1.0, 1.0], [1.0, 1.0]])
+    dm2 = np.array([[2.0, 2.0], [np.nan, np.nan]])
+
+    # min_valid_count = 2
+    pooled, _ = pool_depth_maps([dm1, dm2], min_valid_count=2)
+
+    # (0,0): 2 valid -> 1.5
+    # (0,1): 2 valid -> 1.5
+    # (1,0): 1 valid -> nan
+    # (1,1): 1 valid -> nan
+    expected = np.array([[1.5, 1.5], [np.nan, np.nan]])
+    np.testing.assert_allclose(pooled, expected)
+
+
+def test_pool_depth_maps_confidence_none():
+    # confidence_maps None behavior
+    dm1 = np.ones((2, 2))
+    dm2 = np.ones((2, 2)) * 2
+
+    pooled, conf = pool_depth_maps([dm1, dm2])
+    assert conf is None
+    np.testing.assert_allclose(pooled, np.ones((2, 2)) * 1.5)

py_workspace/tests/test_depth_pool_integration.py

@@ -0,0 +1,253 @@
+import pytest
+import numpy as np
+from unittest.mock import MagicMock, patch
+import sys
+from pathlib import Path
+
+# Add py_workspace to path
+sys.path.append(str(Path(__file__).parent.parent))
+
+from calibrate_extrinsics import apply_depth_verify_refine_postprocess
+
+
+@pytest.fixture
+def mock_dependencies():
+    with (
+        patch("calibrate_extrinsics.verify_extrinsics_with_depth") as mock_verify,
+        patch("calibrate_extrinsics.refine_extrinsics_with_depth") as mock_refine,
+        patch("calibrate_extrinsics.click.echo") as mock_echo,
+    ):
+        # Setup mock return values
+        mock_verify_res = MagicMock()
+        mock_verify_res.rmse = 0.05
+        mock_verify_res.mean_abs = 0.04
+        mock_verify_res.median = 0.03
+        mock_verify_res.depth_normalized_rmse = 0.02
+        mock_verify_res.n_valid = 100
+        mock_verify_res.n_total = 120
+        mock_verify_res.residuals = []
+        mock_verify.return_value = mock_verify_res
+
+        mock_refine.return_value = (np.eye(4), {"success": True})
+
+        yield mock_verify, mock_refine, mock_echo
+
+
+def test_pool_size_1_equivalence(mock_dependencies):
+    """
+    Regression test: Ensure pool_size=1 behaves exactly like the old single-frame path.
+    """
+    mock_verify, _, _ = mock_dependencies
+
+    serial = "123456"
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"}}
+
+    # Create a frame with specific depth values
+    depth_map = np.ones((10, 10)) * 2.0
+    conf_map = np.zeros((10, 10))
+
+    frame_mock = MagicMock()
+    frame_mock.depth_map = depth_map
+    frame_mock.confidence_map = conf_map
+
+    vf = {
+        "frame": frame_mock,
+        "ids": np.array([[1]]),
+        "corners": np.zeros((1, 4, 2)),
+        "score": 100.0,
+    }
+
+    # Structure for new implementation: list of frames
+    verification_frames = {serial: [vf]}
+    marker_geometry = {1: np.zeros((4, 3))}
+    camera_matrices = {serial: np.eye(3)}
+
+    # Run with pool_size=1
+    apply_depth_verify_refine_postprocess(
+        results=results,
+        verification_frames=verification_frames,
+        marker_geometry=marker_geometry,
+        camera_matrices=camera_matrices,
+        verify_depth=True,
+        refine_depth=False,
+        use_confidence_weights=False,
+        depth_confidence_threshold=50,
+        depth_pool_size=1,
+    )
+
+    # Verify that verify_extrinsics_with_depth was called with the exact depth map from the frame
+    args, _ = mock_verify.call_args
+    passed_depth_map = args[2]
+
+    np.testing.assert_array_equal(passed_depth_map, depth_map)
+    assert passed_depth_map is depth_map
+
+
+def test_pool_size_5_integration(mock_dependencies):
+    """
+    Test that pool_size > 1 actually calls pooling and uses the result.
+    """
+    mock_verify, _, mock_echo = mock_dependencies
+
+    serial = "123456"
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"}}
+
+    # Create 3 frames with different depth values
+    # Frame 1: 2.0m
+    # Frame 2: 2.2m
+    # Frame 3: 1.8m
+    # Median should be 2.0m
+
+    frames = []
+    for d in [2.0, 2.2, 1.8]:
+        f = MagicMock()
+        f.depth_map = np.ones((10, 10)) * d
+        f.confidence_map = np.zeros((10, 10))
+        frames.append(f)
+
+    vfs = []
+    for i, f in enumerate(frames):
+        vfs.append(
+            {
+                "frame": f,
+                "ids": np.array([[1]]),
+                "corners": np.zeros((1, 4, 2)),
+                "score": 100.0 - i,
+            }
+        )
+
+    verification_frames = {serial: vfs}
+    marker_geometry = {1: np.zeros((4, 3))}
+    camera_matrices = {serial: np.eye(3)}
+
+    # Run with pool_size=3
+    apply_depth_verify_refine_postprocess(
+        results=results,
+        verification_frames=verification_frames,
+        marker_geometry=marker_geometry,
+        camera_matrices=camera_matrices,
+        verify_depth=True,
+        refine_depth=False,
+        use_confidence_weights=False,
+        depth_confidence_threshold=50,
+        depth_pool_size=3,
+    )
+
+    # Check that "Using pooled depth" was logged
+    any_pooled = any(
+        "Using pooled depth" in str(call.args[0]) for call in mock_echo.call_args_list
+    )
+    assert any_pooled
+
+    # Check that the depth map passed to verify is the median (2.0)
+    args, _ = mock_verify.call_args
+    passed_depth_map = args[2]
+
+    expected_median = np.ones((10, 10)) * 2.0
+    np.testing.assert_allclose(passed_depth_map, expected_median)
+
+    # Verify metadata was added
+    assert "depth_pool" in results[serial]
+    assert results[serial]["depth_pool"]["pooled"] is True
+    assert results[serial]["depth_pool"]["pool_size_actual"] == 3
+
+
+def test_pool_fallback_insufficient_valid(mock_dependencies):
+    """
+    Test fallback to single frame when pooled result has too few valid points.
+    """
+    mock_verify, _, mock_echo = mock_dependencies
+
+    serial = "123456"
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"}}
+
+    # Frame 1: Good depth
+    f1 = MagicMock()
+    f1.depth_map = np.ones((10, 10)) * 2.0
+    f1.confidence_map = np.zeros((10, 10))
+
+    # Frame 2: NaN depth (simulating misalignment or noise)
+    f2 = MagicMock()
+    f2.depth_map = np.full((10, 10), np.nan)
+    f2.confidence_map = np.zeros((10, 10))
+
+    # Frame 3: NaN depth
+    f3 = MagicMock()
+    f3.depth_map = np.full((10, 10), np.nan)
+    f3.confidence_map = np.zeros((10, 10))
+
+    # With median pooling, if >50% are NaN, result is NaN (standard median behavior with NaNs usually propagates or ignores)
+    # Our pool_depth_maps uses nanmedian, which ignores NaNs.
+    # But if we have [2.0, NaN, NaN], median of [2.0] is 2.0.
+    # Wait, let's make it so they are valid but inconsistent to cause variance?
+    # Or just force the pooled result to be bad by making them all different and sparse?
+
+    # Let's use the fact that we can patch pool_depth_maps in the test!
+    with patch("calibrate_extrinsics.pool_depth_maps") as mock_pool:
+        # Return empty/invalid map
+        mock_pool.return_value = (
+            np.zeros((10, 10)),
+            None,
+        )  # Zeros are invalid depth (<=0)
+
+        # Frame 1: Valid on left half
+        d1 = np.full((10, 10), np.nan)
+        d1[:, :5] = 2.0
+        f1.depth_map = d1
+        f1.confidence_map = np.zeros((10, 10))
+
+        # Frame 2: Valid on right half
+        d2 = np.full((10, 10), np.nan)
+        d2[:, 5:] = 2.0
+        f2.depth_map = d2
+        f2.confidence_map = np.zeros((10, 10))
+
+        vfs = [
+            {
+                "frame": f1,
+                "ids": np.array([[1]]),
+                "corners": np.zeros((1, 4, 2)),
+                "score": 100,
+            },
+            {
+                "frame": f2,
+                "ids": np.array([[1]]),
+                "corners": np.zeros((1, 4, 2)),
+                "score": 90,
+            },
+        ]
+
+        verification_frames = {serial: vfs}
+        marker_geometry = {1: np.zeros((4, 3))}
+        camera_matrices = {serial: np.eye(3)}
+
+        apply_depth_verify_refine_postprocess(
+            results=results,
+            verification_frames=verification_frames,
+            marker_geometry=marker_geometry,
+            camera_matrices=camera_matrices,
+            verify_depth=True,
+            refine_depth=False,
+            use_confidence_weights=False,
+            depth_confidence_threshold=50,
+            depth_pool_size=2,
+        )
+
+        # Check for fallback message
+        any_fallback = any(
+            "Falling back to best single frame" in str(call.args[0])
+            for call in mock_echo.call_args_list
+        )
+        assert any_fallback
+
+        # Verify we used the best frame (f1)
+        args, _ = mock_verify.call_args
+        passed_depth_map = args[2]
+        assert passed_depth_map is d1
+
+        # Verify metadata
+        assert results[serial]["depth_pool"]["pooled"] is False
+        assert (
+            results[serial]["depth_pool"]["fallback_reason"]
+            == "insufficient_valid_points"
+        )