feat(aruco): add depth refinement module with bounded optimization

- Add extrinsics_to_params() and params_to_extrinsics() conversions
- Add depth_residual_objective() with regularization
- Add refine_extrinsics_with_depth() using L-BFGS-B
- Add scipy dependency for optimization
- Enforce bounds on rotation (±5°) and translation (±5cm)

py_workspace/aruco/depth_refine.py

@@ -0,0 +1,112 @@
+import numpy as np
+from typing import Dict, Tuple, Optional
+from scipy.optimize import minimize
+from .pose_math import rvec_tvec_to_matrix, matrix_to_rvec_tvec
+from .depth_verify import compute_depth_residual
+
+
+def extrinsics_to_params(T: np.ndarray) -> np.ndarray:
+    rvec, tvec = matrix_to_rvec_tvec(T)
+    return np.concatenate([rvec, tvec])
+
+
+def params_to_extrinsics(params: np.ndarray) -> np.ndarray:
+    rvec = params[:3]
+    tvec = params[3:]
+    return rvec_tvec_to_matrix(rvec, tvec)
+
+
+def depth_residual_objective(
+    params: np.ndarray,
+    marker_corners_world: Dict[int, np.ndarray],
+    depth_map: np.ndarray,
+    K: np.ndarray,
+    initial_params: np.ndarray,
+    regularization_weight: float = 0.1,
+) -> float:
+    T = params_to_extrinsics(params)
+    residuals = []
+
+    for marker_id, corners in marker_corners_world.items():
+        for corner in corners:
+            residual = compute_depth_residual(corner, T, depth_map, K, window_size=5)
+            if residual is not None:
+                residuals.append(residual)
+
+    if len(residuals) == 0:
+        return 1e6
+
+    residuals_array = np.array(residuals)
+    data_term = np.mean(residuals_array**2)
+
+    param_diff = params - initial_params
+    rotation_diff = np.linalg.norm(param_diff[:3])
+    translation_diff = np.linalg.norm(param_diff[3:])
+    regularization = regularization_weight * (rotation_diff + translation_diff)
+
+    return data_term + regularization
+
+
+def refine_extrinsics_with_depth(
+    T_initial: np.ndarray,
+    marker_corners_world: Dict[int, np.ndarray],
+    depth_map: np.ndarray,
+    K: np.ndarray,
+    max_translation_m: float = 0.05,
+    max_rotation_deg: float = 5.0,
+    regularization_weight: float = 0.1,
+) -> Tuple[np.ndarray, dict]:
+    initial_params = extrinsics_to_params(T_initial)
+
+    max_rotation_rad = np.deg2rad(max_rotation_deg)
+
+    bounds = [
+        (initial_params[0] - max_rotation_rad, initial_params[0] + max_rotation_rad),
+        (initial_params[1] - max_rotation_rad, initial_params[1] + max_rotation_rad),
+        (initial_params[2] - max_rotation_rad, initial_params[2] + max_rotation_rad),
+        (initial_params[3] - max_translation_m, initial_params[3] + max_translation_m),
+        (initial_params[4] - max_translation_m, initial_params[4] + max_translation_m),
+        (initial_params[5] - max_translation_m, initial_params[5] + max_translation_m),
+    ]
+
+    result = minimize(
+        depth_residual_objective,
+        initial_params,
+        args=(
+            marker_corners_world,
+            depth_map,
+            K,
+            initial_params,
+            regularization_weight,
+        ),
+        method="L-BFGS-B",
+        bounds=bounds,
+        options={"maxiter": 100, "disp": False},
+    )
+
+    T_refined = params_to_extrinsics(result.x)
+
+    delta_params = result.x - initial_params
+    delta_rotation_rad = np.linalg.norm(delta_params[:3])
+    delta_rotation_deg = np.rad2deg(delta_rotation_rad)
+    delta_translation = np.linalg.norm(delta_params[3:])
+
+    initial_cost = depth_residual_objective(
+        initial_params,
+        marker_corners_world,
+        depth_map,
+        K,
+        initial_params,
+        regularization_weight,
+    )
+
+    stats = {
+        "iterations": result.nit,
+        "success": result.success,
+        "initial_cost": float(initial_cost),
+        "final_cost": float(result.fun),
+        "delta_rotation_deg": float(delta_rotation_deg),
+        "delta_translation_norm_m": float(delta_translation),
+    }
+
+    return T_refined, stats