diff --git a/py_workspace/.beads/issues.jsonl b/py_workspace/.beads/issues.jsonl index 744c327..bc4b80f 100644 --- a/py_workspace/.beads/issues.jsonl +++ b/py_workspace/.beads/issues.jsonl @@ -1,8 +1,10 @@ {"id":"py_workspace-0mu","title":"Implement --render-space in visualize_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:08:12.543309499Z","created_by":"crosstyan","updated_at":"2026-02-07T16:08:17.303232927Z","closed_at":"2026-02-07T16:08:17.303232927Z","close_reason":"Implemented --render-space with opencv/opengl choices and updated README"} {"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-185","title":"Update visualization conventions docs for compare_pose_sets","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T03:17:36.550951981Z","created_by":"crosstyan","updated_at":"2026-02-09T03:17:42.680340444Z","closed_at":"2026-02-09T03:17:42.680340444Z","close_reason":"Added documentation for compare_pose_sets.py input formats to docs/visualization-conventions.md"} {"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-291","title":"Create camera pose comparison script","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-08T07:51:14.710189364Z","created_by":"crosstyan","updated_at":"2026-02-08T07:53:52.647760731Z","closed_at":"2026-02-08T07:53:52.647760731Z","close_reason":"Implemented compare_pose_sets.py script and verified with provided command."} {"id":"py_workspace-2c1","title":"Add manual ground-plane overlay to visualize_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:15:17.432846006Z","created_by":"crosstyan","updated_at":"2026-02-07T16:16:18.287496896Z","closed_at":"2026-02-07T16:16:18.287496896Z","close_reason":"Implemented ground-plane overlay with CLI options and updated README."} +{"id":"py_workspace-49i","title":"Add explicit validation for 4x4 transformation matrices in compare_pose_sets.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T03:22:47.591167295Z","created_by":"crosstyan","updated_at":"2026-02-09T03:23:53.008806228Z","closed_at":"2026-02-09T03:23:53.008806228Z","close_reason":"Added explicit validation for 4x4 transformation matrices in parse_pose() with context-aware error messages. Verified with existing data."} {"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"} @@ -15,9 +17,11 @@ {"id":"py_workspace-ecz","title":"Update visualization conventions docs with alignment details","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-08T07:47:49.633647436Z","created_by":"crosstyan","updated_at":"2026-02-08T07:48:25.728323257Z","closed_at":"2026-02-08T07:48:25.728323257Z","close_reason":"Added alignment methodology section to docs"} {"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-f23","title":"Add --origin-axes-scale option to visualize_extrinsics.py","status":"closed","priority":2,"issue_type":"feature","owner":"crosstyan@outlook.com","created_at":"2026-02-08T05:37:35.228917793Z","created_by":"crosstyan","updated_at":"2026-02-08T05:38:31.173898101Z","closed_at":"2026-02-08T05:38:31.173898101Z","close_reason":"Implemented --origin-axes-scale option and verified with rendering."} +{"id":"py_workspace-gv2","title":"Create apply_calibration_to_fusion_config.py script","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T03:20:08.635031083Z","created_by":"crosstyan","updated_at":"2026-02-09T03:21:20.005139771Z","closed_at":"2026-02-09T03:21:20.005139771Z","close_reason":"Script created and verified with smoke test and type checking."} {"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-kv8","title":"Update compare_pose_sets.py with Plotly visualization","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-08T07:55:38.911520186Z","created_by":"crosstyan","updated_at":"2026-02-08T07:57:13.711754402Z","closed_at":"2026-02-08T07:57:13.711754402Z","close_reason":"Added Plotly visualization to compare_pose_sets.py with camera frustums, axes, and ground plane overlay."} {"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-nlu","title":"Produce A/B visualization comparison for CV world basis","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-08T03:50:56.386223999Z","created_by":"crosstyan","updated_at":"2026-02-08T03:52:41.232154353Z","closed_at":"2026-02-08T03:52:41.232154353Z","close_reason":"Generated A/B comparison images and analyzed visual differences. Source files remain unchanged."} {"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"} diff --git a/py_workspace/compare_pose_sets.py b/py_workspace/compare_pose_sets.py index 47e09b9..c9f2907 100644 --- a/py_workspace/compare_pose_sets.py +++ b/py_workspace/compare_pose_sets.py @@ -7,17 +7,85 @@ Assumes both pose sets are in world_from_cam convention. import json import sys from pathlib import Path +from typing import Final import click import numpy as np import plotly.graph_objects as go -def parse_pose(pose_str: str) -> np.ndarray: +def parse_pose(pose_str: str, context: str = "") -> np.ndarray: vals = [float(x) for x in pose_str.split()] if len(vals) != 16: - raise ValueError(f"Expected 16 values for pose, got {len(vals)}") - return np.array(vals).reshape((4, 4)) + raise ValueError(f"[{context}] Expected 16 values for pose, got {len(vals)}") + pose = np.array(vals).reshape((4, 4)) + + # Validate transformation matrix properties + # 1. Last row check [0, 0, 0, 1] + last_row = pose[3, :] + expected_last_row = np.array([0, 0, 0, 1], dtype=float) + if not np.allclose(last_row, expected_last_row, atol=1e-5): + raise ValueError( + f"[{context}] Invalid last row in transformation matrix: {last_row}. " + f"Expected [0, 0, 0, 1]" + ) + + # 2. Rotation block orthonormality + R = pose[:3, :3] + # R @ R.T approx I + identity_check = R @ R.T + if not np.allclose(identity_check, np.eye(3), atol=1e-3): + raise ValueError( + f"[{context}] Rotation block is not orthonormal (R @ R.T != I)." + ) + + # 3. Determinant check det(R) approx 1 + det = np.linalg.det(R) + if not np.allclose(det, 1.0, atol=1e-3): + raise ValueError( + f"[{context}] Rotation block determinant is {det:.6f}, expected 1.0 (improper rotation or scaling)." + ) + + return pose + + +def load_poses_from_json(path: str) -> dict[str, np.ndarray]: + """ + Heuristically load poses from a JSON file. + Supports: + 1) flat: {"serial": {"pose": "..."}} + 2) nested Fusion: {"serial": {"FusionConfiguration": {"pose": "..."}}} + """ + with open(path, "r") as f: + data = json.load(f) + + poses: dict[str, np.ndarray] = {} + for serial, entry in data.items(): + if not isinstance(entry, dict): + continue + + context = f"File: {path}, Serial: {serial}" + + # Check nested FusionConfiguration first + if "FusionConfiguration" in entry and isinstance( + entry["FusionConfiguration"], dict + ): + if "pose" in entry["FusionConfiguration"]: + poses[str(serial)] = parse_pose( + entry["FusionConfiguration"]["pose"], context=context + ) + # Then check flat + elif "pose" in entry: + poses[str(serial)] = parse_pose(entry["pose"], context=context) + + if not poses: + raise click.UsageError( + f"No parsable poses found in {path}.\n" + "Expected formats:\n" + ' 1) Flat: {"serial": {"pose": "..."}}\n' + ' 2) Nested: {"serial": {"FusionConfiguration": {"pose": "..."}}}' + ) + return poses def serialize_pose(pose: np.ndarray) -> str: @@ -183,13 +251,13 @@ def add_camera_trace( "--pose-a-json", type=click.Path(exists=True), required=True, - help="Pose set A (serial -> {pose: '...'})", + help="Pose set A. Supports flat {'serial': {'pose': '...'}} or nested FusionConfiguration format.", ) @click.option( "--pose-b-json", type=click.Path(exists=True), required=True, - help="Pose set B (serial -> {pose: '...'} or inside_network format)", + help="Pose set B. Supports flat {'serial': {'pose': '...'}} or nested FusionConfiguration format.", ) @click.option( "--report-json", @@ -210,6 +278,7 @@ def add_camera_trace( @click.option( "--show-plot", is_flag=True, + default=False, help="Show the plot interactively", ) @click.option( @@ -237,25 +306,13 @@ def main( """ Compare two camera pose sets from different world frames using rigid alignment. Both are treated as T_world_from_cam. + + Supports symmetric, heuristic input parsing for both A and B: + 1) flat: {"serial": {"pose": "..."}} + 2) nested Fusion: {"serial": {"FusionConfiguration": {"pose": "..."}}} """ - with open(pose_a_json, "r") as f: - data_a = json.load(f) - - with open(pose_b_json, "r") as f: - data_b = json.load(f) - - poses_a: dict[str, np.ndarray] = {} - for serial, data in data_a.items(): - if "pose" in data: - poses_a[str(serial)] = parse_pose(data["pose"]) - - poses_b: dict[str, np.ndarray] = {} - for serial, data in data_b.items(): - # Support both standard and inside_network.json nested format - if "FusionConfiguration" in data and "pose" in data["FusionConfiguration"]: - poses_b[str(serial)] = parse_pose(data["FusionConfiguration"]["pose"]) - elif "pose" in data: - poses_b[str(serial)] = parse_pose(data["pose"]) + poses_a = load_poses_from_json(pose_a_json) + poses_b = load_poses_from_json(pose_b_json) shared_serials = sorted(list(set(poses_a.keys()) & set(poses_b.keys()))) if len(shared_serials) < 3: @@ -347,23 +404,25 @@ def main( if plot_output or show_plot: fig = go.Figure() - for axis, color in zip( - [np.eye(3)[:, 0], np.eye(3)[:, 1], np.eye(3)[:, 2]], - ["red", "green", "blue"], - ): - fig.add_trace( - go.Scatter3d( - x=[0, axis[0] * axis_scale * 2], - y=[0, axis[1] * axis_scale * 2], - z=[0, axis[2] * axis_scale * 2], - mode="lines", - line=dict(color=color, width=4), - name=f"World {'XYZ'[np.argmax(axis)]}", - showlegend=True, + show_axis: Final[bool] = True + if show_axis: + for axis, color in zip( + [np.eye(3)[:, 0], np.eye(3)[:, 1], np.eye(3)[:, 2]], + ["red", "green", "blue"], + ): + fig.add_trace( + go.Scatter3d( + x=[0, axis[0] * axis_scale], + y=[0, axis[1] * axis_scale], + z=[0, axis[2] * axis_scale], + mode="lines", + line=dict(color=color, width=4), + name=f"World {'XYZ'[np.argmax(axis)]}", + showlegend=True, + ) ) - ) - show_ground = False + show_ground: Final[bool] = False if show_ground: ground_size = 5.0 half_size = ground_size / 2.0 @@ -440,4 +499,4 @@ def main( if __name__ == "__main__": - main() + main() # pylint: disable=no-value-for-parameter diff --git a/py_workspace/docs/visualization-conventions.md b/py_workspace/docs/visualization-conventions.md index 5c879b9..e38a103 100644 --- a/py_workspace/docs/visualization-conventions.md +++ b/py_workspace/docs/visualization-conventions.md @@ -352,6 +352,69 @@ though the absolute world coordinates differ. --- +## `compare_pose_sets.py` Input Formats + +The `compare_pose_sets.py` tool is designed to be agnostic to the source of the JSON files. +It uses a **symmetric, heuristic parser** for both `--pose-a-json` and `--pose-b-json`. + +### Accepted JSON Schemas + +The parser automatically detects and handles either of these two structures for any input file: + +**1. Flat Format (Standard Output)** +Used by `calibrate_extrinsics.py` and `refine_extrinsics.py`. +```json +{ + "SERIAL_NUMBER": { + "pose": "r00 r01 r02 tx r10 r11 r12 ty r20 r21 r22 tz 0 0 0 1" + } +} +``` + +**2. Nested Fusion Format** +Used by ZED Fusion `inside_network.json` configuration files. +```json +{ + "SERIAL_NUMBER": { + "FusionConfiguration": { + "pose": "r00 r01 r02 tx r10 r11 r12 ty r20 r21 r22 tz 0 0 0 1" + } + } +} +``` + +### Key Behaviors + +1. **Interchangeability**: You can swap inputs. Comparing A (ArUco) vs B (Fusion) is valid, + as is A (Fusion) vs B (ArUco). The script aligns B to A. +2. **Pose Semantics**: All poses are interpreted as `T_world_from_cam` (camera-to-world). + The script does **not** invert matrices; it assumes the input strings are already in the + correct convention. +3. **Minimum Overlap**: The script requires at least **3 shared camera serials** between + the two files to compute a rigid alignment. +4. **Heuristic Parsing**: For each serial key, the parser looks for `FusionConfiguration.pose` + first, then falls back to `pose`. + +### Example: Swapped Inputs + +Since the parser is symmetric, you can verify consistency by reversing the alignment direction: + +```bash +# Align Fusion (B) to ArUco (A) +uv run compare_pose_sets.py \ + --pose-a-json output/e2e_refine_depth.json \ + --pose-b-json ../zed_settings/inside_network.json \ + --report-json output/report_aruco_ref.json + +# Align ArUco (B) to Fusion (A) +uv run compare_pose_sets.py \ + --pose-a-json ../zed_settings/inside_network.json \ + --pose-b-json output/e2e_refine_depth.json \ + --report-json output/report_fusion_ref.json +``` + +--- + ## Appendix: Stale README References The following lines in `py_workspace/README.md` reference removed flags and should be