crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
crosstyan
2026-02-09 03:24:36 +00:00
parent d6c7829b1e
commit c497af7783
3 changed files with 165 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/.beads/issues.jsonl
+4
View File
@@ -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
+98 -39
View File
@@ -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
py_workspace/docs/visualization-conventions.md
+63
View File
@@ -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