From 0d3e9e67ad73a42ef804c8e9728f615d194e85d2 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Sat, 7 Feb 2026 15:35:48 +0000 Subject: [PATCH] feat: ignore boulder.json and update extrinsics visualizer --- py_workspace/.beads/issues.jsonl | 12 ++- py_workspace/.gitignore | 1 + py_workspace/.sisyphus/boulder.json | 10 -- .../depth-refinement-robust/learnings.md | 6 ++ py_workspace/aruco/depth_refine.py | 50 ++++++---- py_workspace/tests/test_depth_units.py | 93 +++++++++++-------- py_workspace/visualize_extrinsics.py | 13 +-- 7 files changed, 102 insertions(+), 83 deletions(-) delete mode 100644 py_workspace/.sisyphus/boulder.json diff --git a/py_workspace/.beads/issues.jsonl b/py_workspace/.beads/issues.jsonl index b3a1a6d..fabe9f7 100644 --- a/py_workspace/.beads/issues.jsonl +++ b/py_workspace/.beads/issues.jsonl @@ -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"} diff --git a/py_workspace/.gitignore b/py_workspace/.gitignore index 501accd..05cce63 100644 --- a/py_workspace/.gitignore +++ b/py_workspace/.gitignore @@ -221,3 +221,4 @@ __marimo__/ output/ loguru/ tmp/ +.sisyphus/boulder.json diff --git a/py_workspace/.sisyphus/boulder.json b/py_workspace/.sisyphus/boulder.json deleted file mode 100644 index c32f8d0..0000000 --- a/py_workspace/.sisyphus/boulder.json +++ /dev/null @@ -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" -} diff --git a/py_workspace/.sisyphus/notepads/depth-refinement-robust/learnings.md b/py_workspace/.sisyphus/notepads/depth-refinement-robust/learnings.md index 3587fbe..4717a60 100644 --- a/py_workspace/.sisyphus/notepads/depth-refinement-robust/learnings.md +++ b/py_workspace/.sisyphus/notepads/depth-refinement-robust/learnings.md @@ -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. diff --git a/py_workspace/aruco/depth_refine.py b/py_workspace/aruco/depth_refine.py index 02868b2..0f4ed31 100644 --- a/py_workspace/aruco/depth_refine.py +++ b/py_workspace/aruco/depth_refine.py @@ -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 corners: - residual = compute_depth_residual(corner, T, depth_map, K, window_size=5) - if residual is not None: - if confidence_map is not None: - u, v = project_point_to_pixel( - (np.linalg.inv(T) @ np.append(corner, 1.0))[:3], K - ) - if u is not None and v is not None: - h, w = confidence_map.shape[:2] - 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) + for corner in active_corners: + residual = compute_depth_residual(corner, T, depth_map, K, window_size=5) + if residual is None: + # If a point becomes invalid during optimization, assign a large residual + # to discourage the optimizer from going there, but keep the vector length fixed. + # 10.0m is a safe "large" value for depth residuals. + residual = 10.0 + else: + if confidence_map is not None: + u, v = project_point_to_pixel( + (np.linalg.inv(T) @ np.append(corner, 1.0))[:3], K + ) + if u is not None and v is not None: + h, w = confidence_map.shape[:2] + 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, diff --git a/py_workspace/tests/test_depth_units.py b/py_workspace/tests/test_depth_units.py index b677b1d..ce6a8c8 100644 --- a/py_workspace/tests/test_depth_units.py +++ b/py_workspace/tests/test_depth_units.py @@ -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 - mock_cam.retrieve_measure.return_value = sl.ERROR_CODE.SUCCESS + def fake_mat_factory() -> FakeMat: + 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) - mock_init_params_meter.coordinate_units = sl.UNIT.METER - mock_cam.get_init_parameters.return_value = mock_init_params_meter + fake_cam_meter = FakeCamera(sl.UNIT.METER) + cam_meter = cast(sl.Camera, cast(object, fake_cam_meter)) - # We need to patch sl.Mat in the test or just rely on the fact that - # _retrieve_depth creates a new sl.Mat() and calls get_data() on it. - # Since we can't easily mock the sl.Mat() call inside the method without patching, - # let's use a slightly different approach: mock the sl.Mat class itself. + depth_meter = reader._retrieve_depth(cam_meter) # pyright: ignore [reportPrivateUsage] + assert depth_meter is not None + assert np.allclose(depth_meter, 2.0) - with MagicMock() as mock_mat_class: - from aruco import svo_sync + # Case 2: Units are MILLIMETER -> Should divide by 1000 + fake_cam_mm = FakeCamera(sl.UNIT.MILLIMETER) + cam_mm = cast(sl.Camera, cast(object, fake_cam_mm)) - original_mat = svo_sync.sl.Mat - svo_sync.sl.Mat = mock_mat_class - mock_mat_instance = mock_mat_class.return_value - 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 + depth_mm = reader._retrieve_depth(cam_mm) # pyright: ignore [reportPrivateUsage] + assert depth_mm is not None + assert np.allclose(depth_mm, 0.002) if __name__ == "__main__": - test_retrieve_depth_unit_guard() + import pytest + + _ = pytest.main([__file__]) diff --git a/py_workspace/visualize_extrinsics.py b/py_workspace/visualize_extrinsics.py index 3f8a768..1bc45fc 100644 --- a/py_workspace/visualize_extrinsics.py +++ b/py_workspace/visualize_extrinsics.py @@ -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. - # 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. + up=dict(x=0, y=0, z=1), # World +Z is 'up' on screen eye=dict(x=0, y=2.5, z=0), )