feat(refine): replace L-BFGS-B MSE with least_squares soft-L1 robust optimizer

py_workspace/aruco/depth_refine.py

@@ -1,6 +1,6 @@
 import numpy as np
-from typing import Dict, Tuple
-from scipy.optimize import minimize
+from typing import Dict, Tuple, Any
+from scipy.optimize import least_squares
 from .pose_math import rvec_tvec_to_matrix, matrix_to_rvec_tvec
 from .depth_verify import compute_depth_residual
 
@@ -16,14 +16,15 @@ def params_to_extrinsics(params: np.ndarray) -> np.ndarray:
     return rvec_tvec_to_matrix(rvec, tvec)
 
 
-def depth_residual_objective(
+def depth_residuals(
     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:
+    reg_rot: float = 0.1,
+    reg_trans: float = 1.0,
+) -> np.ndarray:
     T = params_to_extrinsics(params)
     residuals = []
 
@@ -33,18 +34,18 @@ def depth_residual_objective(
             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)
-
+    # Regularization as pseudo-residuals
     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)
+    
+    # Rotation regularization (first 3 params)
+    if reg_rot > 0:
+        residuals.extend(param_diff[:3] * reg_rot)
+        
+    # Translation regularization (last 3 params)
+    if reg_trans > 0:
+        residuals.extend(param_diff[3:] * reg_trans)
 
-    return float(np.real(data_term + regularization))
+    return np.array(residuals)
 
 
 def refine_extrinsics_with_depth(
@@ -55,33 +56,74 @@ def refine_extrinsics_with_depth(
     max_translation_m: float = 0.05,
     max_rotation_deg: float = 5.0,
     regularization_weight: float = 0.1,
-) -> Tuple[np.ndarray, dict[str, float]]:
+    loss: str = "soft_l1",
+    f_scale: float = 0.1,
+    reg_rot: float | None = None,
+    reg_trans: float | None = None,
+) -> Tuple[np.ndarray, dict[str, Any]]:
     initial_params = extrinsics_to_params(T_initial)
 
+    # Handle legacy regularization_weight if specific weights aren't provided
+    # Default behavior: reg_rot = weight, reg_trans = weight * 10
+    # This matches the plan's default of (0.1, 1.0) when weight is 0.1
+    if reg_rot is None:
+        reg_rot = regularization_weight
+    if reg_trans is None:
+        reg_trans = regularization_weight * 10.0
+
+    # Check for valid depth points first
+    data_residual_count = 0
+    for marker_id, corners in marker_corners_world.items():
+        for corner in corners:
+            res = compute_depth_residual(corner, T_initial, depth_map, K, window_size=5)
+            if res is not None:
+                data_residual_count += 1
+    
+    if data_residual_count == 0:
+        return T_initial, {
+            "success": False,
+            "reason": "no_valid_depth_points",
+            "initial_cost": 0.0,
+            "final_cost": 0.0,
+            "iterations": 0,
+            "delta_rotation_deg": 0.0,
+            "delta_translation_norm_m": 0.0,
+            "termination_message": "No valid depth points found at marker corners",
+            "nfev": 0,
+            "optimality": 0.0,
+            "active_mask": np.zeros(6, dtype=int),
+            "cost": 0.0
+        }
+
     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),
-    ]
+    lower_bounds = initial_params.copy()
+    upper_bounds = initial_params.copy()
+    
+    lower_bounds[:3] -= max_rotation_rad
+    upper_bounds[:3] += max_rotation_rad
+    lower_bounds[3:] -= max_translation_m
+    upper_bounds[3:] += max_translation_m
+    
+    bounds = (lower_bounds, upper_bounds)
 
-    result = minimize(
-        depth_residual_objective,
+    result = least_squares(
+        depth_residuals,
         initial_params,
         args=(
             marker_corners_world,
             depth_map,
             K,
             initial_params,
-            regularization_weight,
+            reg_rot,
+            reg_trans,
         ),
-        method="L-BFGS-B",
+        method="trf",
+        loss=loss,
+        f_scale=f_scale,
         bounds=bounds,
-        options={"maxiter": 100, "disp": False},
+        x_scale="jac",
+        max_nfev=200,
     )
 
     T_refined = params_to_extrinsics(result.x)
@@ -91,22 +133,30 @@ def refine_extrinsics_with_depth(
     delta_rotation_deg = np.rad2deg(delta_rotation_rad)
     delta_translation = np.linalg.norm(delta_params[3:])
 
-    initial_cost = depth_residual_objective(
+    # Calculate initial cost for comparison
+    initial_residuals = depth_residuals(
         initial_params,
         marker_corners_world,
         depth_map,
         K,
         initial_params,
-        regularization_weight,
+        reg_rot,
+        reg_trans,
     )
+    initial_cost = 0.5 * np.sum(initial_residuals**2)
 
     stats = {
-        "iterations": result.nit,
+        "iterations": result.nfev,
         "success": result.success,
         "initial_cost": float(initial_cost),
-        "final_cost": float(result.fun),
+        "final_cost": float(result.cost),
         "delta_rotation_deg": float(delta_rotation_deg),
         "delta_translation_norm_m": float(delta_translation),
+        "termination_message": result.message,
+        "nfev": result.nfev,
+        "optimality": float(result.optimality),
+        "active_mask": result.active_mask,
+        "cost": float(result.cost),
     }
 
     return T_refined, stats

py_workspace/aruco/svo_sync.py

@@ -3,6 +3,7 @@ import numpy as np
 from dataclasses import dataclass
 from typing import Any
 import os
+from loguru import logger
 
 
 @dataclass
@@ -42,6 +43,7 @@ class SVOReader:
             init_params.set_from_svo_file(path)
             init_params.svo_real_time_mode = False
             init_params.depth_mode = depth_mode
+            init_params.coordinate_units = sl.UNIT.METER
 
             cam = sl.Camera()
             status = cam.open(init_params)
@@ -184,9 +186,29 @@ class SVOReader:
         cam.retrieve_measure(depth_mat, sl.MEASURE.DEPTH)
         depth_data = depth_mat.get_data().copy()
 
-        # ZED SDK defaults to MILLIMETER units if not specified in InitParameters.
-        # We convert to meters to match the extrinsics coordinate system.
-        return depth_data / 1000.0
+        # Check if units are already in meters to avoid double scaling.
+        # SDK coordinate_units is set to METER in __init__.
+        units = cam.get_init_parameters().coordinate_units
+        if units == sl.UNIT.METER:
+            depth = depth_data
+        else:
+            # Fallback for safety, though coordinate_units should be METER.
+            depth = depth_data / 1000.0
+
+        # Sanity check and debug logging
+        valid_mask = np.isfinite(depth) & (depth > 0)
+        if np.any(valid_mask):
+            valid_depths = depth[valid_mask]
+            logger.debug(
+                f"Depth stats (m) - Min: {np.min(valid_depths):.3f}, "
+                f"Median: {np.median(valid_depths):.3f}, "
+                f"Max: {np.max(valid_depths):.3f}, "
+                f"P95: {np.percentile(valid_depths, 95):.3f}"
+            )
+        else:
+            logger.warning("No valid depth values retrieved")
+
+        return depth
 
     def _retrieve_confidence(self, cam: sl.Camera) -> np.ndarray | None:
         if not self.enable_depth: