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

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: