feat: add explicit 4x4 transformation matrix validation to compare_pose_sets.py

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"}

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)}")
+        raise ValueError(f"[{context}] Expected 16 values for pose, got {len(vals)}")
-    return np.array(vals).reshape((4, 4))
+    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:
+    poses_a = load_poses_from_json(pose_a_json)
-        data_a = json.load(f)
+    poses_b = load_poses_from_json(pose_b_json)
-
-    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"])
 
     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(
+        show_axis: Final[bool] = True
-            [np.eye(3)[:, 0], np.eye(3)[:, 1], np.eye(3)[:, 2]],
+        if show_axis:
-            ["red", "green", "blue"],
+            for axis, color in zip(
-        ):
+                [np.eye(3)[:, 0], np.eye(3)[:, 1], np.eye(3)[:, 2]],
-            fig.add_trace(
+                ["red", "green", "blue"],
-                go.Scatter3d(
+            ):
-                    x=[0, axis[0] * axis_scale * 2],
+                fig.add_trace(
-                    y=[0, axis[1] * axis_scale * 2],
+                    go.Scatter3d(
-                    z=[0, axis[2] * axis_scale * 2],
+                        x=[0, axis[0] * axis_scale],
-                    mode="lines",
+                        y=[0, axis[1] * axis_scale],
-                    line=dict(color=color, width=4),
+                        z=[0, axis[2] * axis_scale],
-                    name=f"World {'XYZ'[np.argmax(axis)]}",
+                        mode="lines",
-                    showlegend=True,
+                        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

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