feat(cli): add depth verify/refine outputs and tests

- Retrieve depth + confidence measures from SVOReader when depth enabled
- Compute depth residual metrics and attach to output JSON
- Optionally write per-corner residual CSV via --report-csv
- Post-process refinement: optimize final pose and report pre/post metrics
- Add unit tests for depth verification and refinement modules
- Add basedpyright dev dependency for diagnostics

py_workspace/aruco/depth_verify.py

@@ -1,12 +1,12 @@
 import numpy as np
 from dataclasses import dataclass
-from typing import Optional, Dict
+from typing import Optional, Dict, List, Tuple
 from .pose_math import invert_transform
 
 
 @dataclass
 class DepthVerificationResult:
-    residuals: list
+    residuals: List[Tuple[int, int, float]]  # (marker_id, corner_idx, residual)
     rmse: float
     mean_abs: float
     median: float
@@ -40,7 +40,7 @@ def compute_depth_residual(
         return None
 
     u, v = project_point_to_pixel(P_cam, K)
-    if u is None:
+    if u is None or v is None:
         return None
 
     h, w = depth_map.shape[:2]
@@ -67,6 +67,43 @@ def compute_depth_residual(
     return float(z_measured - z_predicted)
 
 
+def compute_marker_corner_residuals(
+    T_world_cam: np.ndarray,
+    marker_corners_world: Dict[int, np.ndarray],
+    depth_map: np.ndarray,
+    K: np.ndarray,
+    confidence_map: Optional[np.ndarray] = None,
+    confidence_thresh: float = 50,
+) -> List[Tuple[int, int, float]]:
+    detailed_residuals = []
+
+    for marker_id, corners in marker_corners_world.items():
+        for corner_idx, corner in enumerate(corners):
+            # Check confidence if map is provided
+            if confidence_map is not None:
+                T_cam_world = invert_transform(T_world_cam)
+                P_world_h = np.append(corner, 1.0)
+                P_cam = (T_cam_world @ P_world_h)[:3]
+                u_proj, v_proj = project_point_to_pixel(P_cam, K)
+
+                if u_proj is not None and v_proj is not None:
+                    h, w = confidence_map.shape[:2]
+                    if 0 <= u_proj < w and 0 <= v_proj < h:
+                        confidence = confidence_map[v_proj, u_proj]
+                        # Higher confidence value means LESS confident in ZED SDK
+                        # Range [1, 100], where 100 is typically occlusion/invalid
+                        if confidence > confidence_thresh:
+                            continue
+
+            residual = compute_depth_residual(
+                corner, T_world_cam, depth_map, K, window_size=5
+            )
+            if residual is not None:
+                detailed_residuals.append((int(marker_id), corner_idx, residual))
+
+    return detailed_residuals
+
+
 def verify_extrinsics_with_depth(
     T_world_cam: np.ndarray,
     marker_corners_world: Dict[int, np.ndarray],
@@ -75,28 +112,17 @@ def verify_extrinsics_with_depth(
     confidence_map: Optional[np.ndarray] = None,
     confidence_thresh: float = 50,
 ) -> DepthVerificationResult:
-    residuals = []
-    n_total = 0
-
-    for marker_id, corners in marker_corners_world.items():
-        for corner in corners:
-            n_total += 1
-
-            if confidence_map is not None:
-                u = int(round(corner[0]))
-                v = int(round(corner[1]))
-                h, w = confidence_map.shape[:2]
-                if 0 <= u < w and 0 <= v < h:
-                    confidence = confidence_map[v, u]
-                    if confidence > confidence_thresh:
-                        continue
-
-            residual = compute_depth_residual(
-                corner, T_world_cam, depth_map, K, window_size=5
-            )
-            if residual is not None:
-                residuals.append(residual)
+    detailed_residuals = compute_marker_corner_residuals(
+        T_world_cam,
+        marker_corners_world,
+        depth_map,
+        K,
+        confidence_map,
+        confidence_thresh,
+    )
 
+    residuals = [r[2] for r in detailed_residuals]
+    n_total = sum(len(corners) for corners in marker_corners_world.values())
     n_valid = len(residuals)
 
     if n_valid == 0:
@@ -128,10 +154,10 @@ def verify_extrinsics_with_depth(
         if depths:
             mean_depth = np.mean(depths)
             if mean_depth > 0:
-                depth_normalized_rmse = rmse / mean_depth
+                depth_normalized_rmse = float(rmse / mean_depth)
 
     return DepthVerificationResult(
-        residuals=residuals,
+        residuals=detailed_residuals,
         rmse=rmse,
         mean_abs=mean_abs,
         median=median,