feat: ignore boulder.json and update extrinsics visualizer

2026-02-07 15:35:48 +00:00
parent a8d375191a
commit 0d3e9e67ad
7 changed files with 102 additions and 83 deletions
@@ -1,13 +1,21 @@
 {"id":"py_workspace-0q7","title":"Fix basedpyright errors in aruco/pose_averaging.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:53:33.377735199Z","created_by":"crosstyan","updated_at":"2026-02-07T08:58:49.252312392Z","closed_at":"2026-02-07T08:58:49.252312392Z","close_reason":"Fixed basedpyright errors"}
 {"id":"py_workspace-214","title":"Migrate visualize_extrinsics to Plotly with diagnose mode","status":"closed","priority":2,"issue_type":"feature","owner":"crosstyan@outlook.com","created_at":"2026-02-07T15:14:40.547616056Z","created_by":"crosstyan","updated_at":"2026-02-07T15:25:00.354290874Z","closed_at":"2026-02-07T15:25:00.354290874Z","close_reason":"Fixed QA issues: Y-up enforcement, README sync, dependencies"}
 {"id":"py_workspace-62y","title":"Fix depth pooling fallback threshold","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:12:12.046607198Z","created_by":"crosstyan","updated_at":"2026-02-07T08:13:12.98625698Z","closed_at":"2026-02-07T08:13:12.98625698Z","close_reason":"Updated fallback threshold to strict comparison"}
 {"id":"py_workspace-6m5","title":"Robust Optimizer Implementation","status":"closed","priority":0,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:22:45.183574374Z","created_by":"crosstyan","updated_at":"2026-02-07T05:22:53.151871639Z","closed_at":"2026-02-07T05:22:53.151871639Z","close_reason":"Implemented robust optimizer with least_squares and soft_l1 loss, updated tests"}
 {"id":"py_workspace-6sg","title":"Document marker parquet structure","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T02:48:08.95742431Z","created_by":"crosstyan","updated_at":"2026-02-07T02:49:35.897152691Z","closed_at":"2026-02-07T02:49:35.897152691Z","close_reason":"Documented parquet structure in aruco/markers/PARQUET_FORMAT.md"}
-{"id":"py_workspace-a85","title":"Add CLI option for ArUco dictionary in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:13:41.896728814Z","created_by":"crosstyan","updated_at":"2026-02-06T10:14:44.083065399Z","closed_at":"2026-02-06T10:14:44.083065399Z","close_reason":"Added CLI option for selectable ArUco dictionary including AprilTag aliases"}
+{"id":"py_workspace-98p","title":"Integrate multi-frame depth pooling into calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T07:59:35.333468652Z","created_by":"crosstyan","updated_at":"2026-02-07T08:06:37.662956356Z","closed_at":"2026-02-07T08:06:37.662956356Z","close_reason":"Implemented multi-frame depth pooling and verified with tests"}
 {"id":"py_workspace-a85","title":"Add CLI option for ArUco dictionary in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:13:41.896728814Z","created_by":"crosstyan","updated_at":"2026-02-07T07:29:52.290976525Z","closed_at":"2026-02-07T07:29:52.290976525Z","close_reason":"Implemented multi-frame depth pooling in calibrate_extrinsics.py"}
 {"id":"py_workspace-cg9","title":"Implement core alignment utilities (Task 1)","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:40:36.296030875Z","created_by":"crosstyan","updated_at":"2026-02-06T10:40:46.196825039Z","closed_at":"2026-02-06T10:40:46.196825039Z","close_reason":"Implemented compute_face_normal, rotation_align_vectors, and apply_alignment_to_pose in aruco/alignment.py"}
 {"id":"py_workspace-ee1","title":"Implement depth-mode argument resolution in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T06:31:03.430147225Z","created_by":"crosstyan","updated_at":"2026-02-07T06:33:43.204825053Z","closed_at":"2026-02-07T06:33:43.204825053Z","close_reason":"Implemented depth-mode argument resolution logic and verified with multiple test cases."}
 {"id":"py_workspace-j8b","title":"Research scipy.optimize.least_squares robust optimization for depth residuals","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T04:54:04.720996955Z","created_by":"crosstyan","updated_at":"2026-02-07T04:55:22.995644Z","closed_at":"2026-02-07T04:55:22.995644Z","close_reason":"Research completed and recommendations provided."}
 {"id":"py_workspace-kpa","title":"Unit Hardening (P0)","status":"closed","priority":0,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:01:46.342605011Z","created_by":"crosstyan","updated_at":"2026-02-07T05:01:51.303022101Z","closed_at":"2026-02-07T05:01:51.303022101Z","close_reason":"Implemented unit hardening in SVOReader: set coordinate_units=METER and guarded manual conversion in _retrieve_depth. Added depth sanity logs."}
 {"id":"py_workspace-kuy","title":"Move parquet documentation to docs/","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T02:52:12.609090777Z","created_by":"crosstyan","updated_at":"2026-02-07T02:52:43.088520272Z","closed_at":"2026-02-07T02:52:43.088520272Z","close_reason":"Moved parquet documentation to docs/marker-parquet-format.md"}
 {"id":"py_workspace-ld1","title":"Search for depth unit conversion and scaling patterns","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T04:53:53.211242053Z","created_by":"crosstyan","updated_at":"2026-02-07T04:54:56.840335809Z","closed_at":"2026-02-07T04:54:56.840335809Z","close_reason":"Exhaustive search completed. Identified manual scaling in svo_sync.py and SDK-level scaling in depth_sensing.py. Documented risks in learnings.md."}
-{"id":"py_workspace-nvw","title":"Update documentation for robust depth refinement","status":"open","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:41:32.963615133Z","created_by":"crosstyan","updated_at":"2026-02-07T05:41:32.963615133Z"}
+{"id":"py_workspace-nvw","title":"Update documentation for robust depth refinement","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:41:32.963615133Z","created_by":"crosstyan","updated_at":"2026-02-07T05:43:55.707975317Z","closed_at":"2026-02-07T05:43:55.707975317Z","close_reason":"Documentation updated with robust refinement details"}
 {"id":"py_workspace-q4w","title":"Add type hints and folder-aware --svo input in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:01:13.943518267Z","created_by":"crosstyan","updated_at":"2026-02-06T10:03:09.855307397Z","closed_at":"2026-02-06T10:03:09.855307397Z","close_reason":"Implemented type hints and directory expansion for --svo"}
 {"id":"py_workspace-q8j","title":"Add script to visualize generated camera extrinsics","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:22:35.151648893Z","created_by":"crosstyan","updated_at":"2026-02-07T08:27:27.034717788Z","closed_at":"2026-02-07T08:27:27.034717788Z","close_reason":"Implemented visualize_extrinsics.py utility script and verified with example data."}
 {"id":"py_workspace-qf9","title":"Implement RMSE-based fallback for depth pooling","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T09:03:17.759148159Z","created_by":"crosstyan","updated_at":"2026-02-07T09:06:33.106901615Z","closed_at":"2026-02-07T09:06:33.106901615Z","close_reason":"Implemented RMSE-based fallback and verified with tests"}
 {"id":"py_workspace-t4e","title":"Add --min-markers CLI and rejection debug logs in calibrate_extrinsics","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:21:51.846079425Z","created_by":"crosstyan","updated_at":"2026-02-06T10:22:39.870440044Z","closed_at":"2026-02-06T10:22:39.870440044Z","close_reason":"Added --min-markers (default 1), rejection debug logs, and clarified accepted-pose summary label"}
 {"id":"py_workspace-th3","title":"Implement Best-Frame Selection for depth verification","status":"closed","priority":1,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:04:11.896109458Z","created_by":"crosstyan","updated_at":"2026-02-07T05:06:07.346747231Z","closed_at":"2026-02-07T05:06:07.346747231Z","close_reason":"Implemented best-frame selection with scoring logic and verified with tests."}
 {"id":"py_workspace-wsk","title":"Fix basedpyright errors in tests and exclude ogl_viewer","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:54:16.6652971Z","created_by":"crosstyan","updated_at":"2026-02-07T08:58:49.256601506Z","closed_at":"2026-02-07T08:58:49.256601506Z","close_reason":"Fixed basedpyright errors"}
 {"id":"py_workspace-z3r","title":"Add debug logs for successful ArUco detection","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:17:30.195422209Z","created_by":"crosstyan","updated_at":"2026-02-06T10:18:35.263206185Z","closed_at":"2026-02-06T10:18:35.263206185Z","close_reason":"Added loguru debug logs for successful ArUco detections in calibrate_extrinsics loop"}
@@ -221,3 +221,4 @@ __marimo__/
 output/
 loguru/
 tmp/
 .sisyphus/boulder.json
@@ -1,10 +0,0 @@
 {
  "active_plan": "/workspaces/zed-playground/py_workspace/.sisyphus/plans/depth-refinement-robust.md",
  "started_at": "2026-02-07T04:51:46.370Z",
  "session_ids": [
    "ses_3c99b5043ffeFGeuraVIodT6wM",
    "ses_3c99b5043ffeFGeuraVIodT6wM"
  ],
  "plan_name": "depth-refinement-robust",
  "agent": "atlas"
 }
@@ -58,3 +58,9 @@
 - **Documentation as Contract**: Updating the docs *after* implementation revealed that the "Unit Mismatch" section was outdated. Explicitly marking it as "Resolved" preserves the history while clarifying current behavior.
 - **Benchmark Matrix Value**: Documenting the benchmark matrix makes it a first-class citizen in the workflow, encouraging users to empirically verify refinement improvements rather than trusting defaults.
 - **Confidence Weights**: Explicitly documenting this feature highlights the importance of sensor uncertainty in the optimization process.
 ## Bug Fix: Variable-Length Residual Vectors
 - Fixed a `ValueError` in `scipy.optimize.least_squares` caused by the residual vector changing length between iterations.
 - The root cause was filtering for valid depth points *inside* the residual function. If a point projected outside the image or had invalid depth in one iteration but not another, the vector length would change, which `least_squares` does not support.
 - Solution: Identify "active" points at the start of refinement (`T_initial`) and use this fixed set of points for all iterations.
 - If a point becomes invalid during optimization (e.g., projects out of bounds), it is now assigned a large constant residual (10.0m) instead of being removed from the vector. This maintains a stable dimensionality while discouraging the optimizer from moving towards invalid regions.
@@ -1,5 +1,5 @@
 import numpy as np
-from typing import Dict, Tuple, Any, Optional
+from typing import Dict, Tuple, Any, Optional, List
 from scipy.optimize import least_squares
 from .pose_math import rvec_tvec_to_matrix, matrix_to_rvec_tvec
 from .depth_verify import (
@@ -22,7 +22,7 @@ def params_to_extrinsics(params: np.ndarray) -> np.ndarray:
 def depth_residuals(
    params: np.ndarray,
-    marker_corners_world: Dict[int, np.ndarray],
+    active_corners: List[np.ndarray],
    depth_map: np.ndarray,
    K: np.ndarray,
    initial_params: np.ndarray,
@@ -34,21 +34,25 @@ def depth_residuals(
    T = params_to_extrinsics(params)
    residuals = []
-    for marker_id, corners in marker_corners_world.items():
+    for corner in active_corners:
-        for corner in corners:
+        residual = compute_depth_residual(corner, T, depth_map, K, window_size=5)
-            residual = compute_depth_residual(corner, T, depth_map, K, window_size=5)
+        if residual is None:
-            if residual is not None:
+            # If a point becomes invalid during optimization, assign a large residual
-                if confidence_map is not None:
+            # to discourage the optimizer from going there, but keep the vector length fixed.
-                    u, v = project_point_to_pixel(
+            # 10.0m is a safe "large" value for depth residuals.
-                        (np.linalg.inv(T) @ np.append(corner, 1.0))[:3], K
+            residual = 10.0
-                    )
+        else:
-                    if u is not None and v is not None:
+            if confidence_map is not None:
-                        h, w = confidence_map.shape[:2]
+                u, v = project_point_to_pixel(
-                        if 0 <= u < w and 0 <= v < h:
+                    (np.linalg.inv(T) @ np.append(corner, 1.0))[:3], K
-                            conf = confidence_map[v, u]
+                )
-                            weight = get_confidence_weight(conf, confidence_thresh)
+                if u is not None and v is not None:
-                            residual *= np.sqrt(weight)
+                    h, w = confidence_map.shape[:2]
-                residuals.append(residual)
+                    if 0 <= u < w and 0 <= v < h:
                        conf = confidence_map[v, u]
                        weight = get_confidence_weight(conf, confidence_thresh)
                        residual *= np.sqrt(weight)
        residuals.append(residual)
    # Regularization as pseudo-residuals
    param_diff = params - initial_params
@@ -90,6 +94,7 @@ def refine_extrinsics_with_depth(
        reg_trans = regularization_weight * 10.0
    # Check for valid depth points first
    active_corners = []
    n_points_total = 0
    n_depth_valid = 0
    n_confidence_rejected = 0
@@ -100,6 +105,7 @@ def refine_extrinsics_with_depth(
            res = compute_depth_residual(corner, T_initial, depth_map, K, window_size=5)
            if res is not None:
                n_depth_valid += 1
                is_confident = True
                if confidence_map is not None:
                    u, v = project_point_to_pixel(
                        (np.linalg.inv(T_initial) @ np.append(corner, 1.0))[:3], K
@@ -111,8 +117,12 @@ def refine_extrinsics_with_depth(
                            weight = get_confidence_weight(conf, confidence_thresh)
                            if weight <= 0:
                                n_confidence_rejected += 1
                                is_confident = False
-    if n_depth_valid == 0:
+                if is_confident:
                    active_corners.append(corner)
    if not active_corners:
        return T_initial, {
            "success": False,
            "reason": "no_valid_depth_points",
@@ -152,7 +162,7 @@ def refine_extrinsics_with_depth(
        depth_residuals,
        initial_params,
        args=(
-            marker_corners_world,
+            active_corners,
            depth_map,
            K,
            initial_params,
@@ -179,7 +189,7 @@ def refine_extrinsics_with_depth(
    # Calculate initial cost for comparison
    initial_residuals = depth_residuals(
        initial_params,
-        marker_corners_world,
+        active_corners,
        depth_map,
        K,
        initial_params,
@@ -1,59 +1,72 @@
 import numpy as np
 import pyzed.sl as sl
-from unittest.mock import MagicMock
+from typing import Any, cast, TYPE_CHECKING
 from aruco.svo_sync import SVOReader
 if TYPE_CHECKING:
    from _pytest.monkeypatch import MonkeyPatch
-def test_retrieve_depth_unit_guard():
+
 class FakeMat:
    def __init__(self) -> None:
        self.data: np.ndarray = np.array([])
    def get_data(self) -> np.ndarray:
        return self.data
 class FakeInitParameters:
    coordinate_units: sl.UNIT
    def __init__(self, units: sl.UNIT) -> None:
        self.coordinate_units = units
 class FakeCamera:
    init_params: FakeInitParameters
    def __init__(self, units: sl.UNIT) -> None:
        self.init_params = FakeInitParameters(units)
    def get_init_parameters(self) -> FakeInitParameters:
        return self.init_params
    def retrieve_measure(self, _mat: Any, _measure: sl.MEASURE) -> sl.ERROR_CODE:
        return sl.ERROR_CODE.SUCCESS
 def test_retrieve_depth_unit_guard(monkeypatch: "MonkeyPatch") -> None:
    # Setup SVOReader with depth enabled
    reader = SVOReader([], depth_mode=sl.DEPTH_MODE.ULTRA)
    # Mock Camera
    mock_cam = MagicMock(spec=sl.Camera)
    # Mock depth data (e.g., 2.0 meters)
    depth_data = np.full((100, 100), 2.0, dtype=np.float32)
    mock_mat = MagicMock(spec=sl.Mat)
    mock_mat.get_data.return_value = depth_data
-    # Mock retrieve_measure to "fill" the mat
+    def fake_mat_factory() -> FakeMat:
-    mock_cam.retrieve_measure.return_value = sl.ERROR_CODE.SUCCESS
+        m = FakeMat()
        m.data = depth_data
        return m
    monkeypatch.setattr("aruco.svo_sync.sl.Mat", fake_mat_factory)
    # Case 1: Units are METER -> Should NOT divide by 1000
-    mock_init_params_meter = MagicMock(spec=sl.InitParameters)
+    fake_cam_meter = FakeCamera(sl.UNIT.METER)
-    mock_init_params_meter.coordinate_units = sl.UNIT.METER
+    cam_meter = cast(sl.Camera, cast(object, fake_cam_meter))
    mock_cam.get_init_parameters.return_value = mock_init_params_meter
-    # We need to patch sl.Mat in the test or just rely on the fact that
+    depth_meter = reader._retrieve_depth(cam_meter)  # pyright: ignore [reportPrivateUsage]
-    # _retrieve_depth creates a new sl.Mat() and calls get_data() on it.
+    assert depth_meter is not None
-    # Since we can't easily mock the sl.Mat() call inside the method without patching,
+    assert np.allclose(depth_meter, 2.0)
    # let's use a slightly different approach: mock the sl.Mat class itself.
-    with MagicMock() as mock_mat_class:
+    # Case 2: Units are MILLIMETER -> Should divide by 1000
-        from aruco import svo_sync
+    fake_cam_mm = FakeCamera(sl.UNIT.MILLIMETER)
    cam_mm = cast(sl.Camera, cast(object, fake_cam_mm))
-        original_mat = svo_sync.sl.Mat
+    depth_mm = reader._retrieve_depth(cam_mm)  # pyright: ignore [reportPrivateUsage]
-        svo_sync.sl.Mat = mock_mat_class
+    assert depth_mm is not None
-        mock_mat_instance = mock_mat_class.return_value
+    assert np.allclose(depth_mm, 0.002)
        mock_mat_instance.get_data.return_value = depth_data
        # Test METER path
        depth_meter = reader._retrieve_depth(mock_cam)
        assert depth_meter is not None
        assert np.allclose(depth_meter, 2.0)
        # Case 2: Units are MILLIMETER -> Should divide by 1000
        mock_init_params_mm = MagicMock(spec=sl.InitParameters)
        mock_init_params_mm.coordinate_units = sl.UNIT.MILLIMETER
        mock_cam.get_init_parameters.return_value = mock_init_params_mm
        depth_mm = reader._retrieve_depth(mock_cam)
        assert depth_mm is not None
        assert np.allclose(depth_mm, 0.002)
        # Restore original sl.Mat
        svo_sync.sl.Mat = original_mat
 if __name__ == "__main__":
-    test_retrieve_depth_unit_guard()
+    import pytest
    _ = pytest.main([__file__])
@@ -499,19 +499,10 @@ def main(
    )
    if birdseye:
-        # For birdseye, we force top-down view
+        # For birdseye, we force top-down view (looking down +Y towards X-Z plane)
        scene_dict["camera"] = dict(
            projection=dict(type="orthographic"),
-            up=dict(x=0, y=0, z=1),  # Z is up in Plotly? No, Y is usually up.
+            up=dict(x=0, y=0, z=1),  # World +Z is 'up' on screen
            # Wait, we want X-Z plane. So we look down Y.
            # Plotly default is Z up.
            # If our data is Y-up (standard graphics), then we look from +Y down to X-Z.
            # Actually, for X-Z plane top-down, we want to look from +Y.
            # So eye should be (0, high, 0).
            # And "up" vector for the camera should be aligned with Z (or -Z) to orient the map correctly.
            # Let's try Z-up for the camera orientation so X is right and Z is up on screen?
            # No, usually map is X right, Z up (or down).
            # If we look from +Y, and up is +Z, then Z is "up" on screen.
            eye=dict(x=0, y=2.5, z=0),
        )