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

add compare_pose_sets.py for rigid alignment and comparison of camera pose sets

Browse Source
This commit is contained in:
crosstyan
2026-02-08 07:54:26 +00:00
parent dc2d34dd7c
commit 4bc4b7dfb8
2 changed files with 220 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/.beads/issues.jsonl
+4 -1
View File
@@ -1,6 +1,7 @@
{"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-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-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"}
@@ -11,12 +12,14 @@
{"id":"py_workspace-afh","title":"Inspect tmp_visualizer.html camera layout","notes":"Inspected tmp_visualizer.html. cam_0 is at (0,0,0). cam_1 is at (1,0,0). cam_2 is at (0, 0.5, 1.0). Axes are RGB=XYZ. Layout matches expected synthetic geometry.","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T15:40:04.162565539Z","created_by":"crosstyan","updated_at":"2026-02-07T15:42:10.721124074Z","closed_at":"2026-02-07T15:42:10.721124074Z","close_reason":"Inspection complete. Layout matches synthetic input."}
{"id":"py_workspace-cg4","title":"Implement geometry-first auto-align heuristic","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:48:33.048250646Z","created_by":"crosstyan","updated_at":"2026-02-07T16:53:54.772815505Z","closed_at":"2026-02-07T16:53:54.772815505Z","close_reason":"Closed"}
{"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-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-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-nlu","title":"Produce A/B visualization comparison for CV world basis","status":"open","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:50:56.386223999Z"}
{"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"}
{"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."}
py_workspace/compare_pose_sets.py
+216
View File
@@ -0,0 +1,216 @@
#!/usr/bin/env python3
"""
Compare two camera pose sets from different world frames using rigid alignment.
Assumes both pose sets are in world_from_cam convention.
"""
import json
import sys
from pathlib import Path
import click
import numpy as np
def parse_pose(pose_str: 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))
def serialize_pose(pose: np.ndarray) -> str:
return " ".join(f"{x:.6f}" for x in pose.flatten())
def rigid_transform_3d(A: np.ndarray, B: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""
Find rigid alignment (R, t) such that R*A + t approx B.
A, B are (N, 3) arrays of points.
Uses Kabsch algorithm.
"""
assert A.shape == B.shape
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
AA = A - centroid_A
BB = B - centroid_B
H = AA.T @ BB
U, S, Vt = np.linalg.svd(H)
R_mat = Vt.T @ U.T
if np.linalg.det(R_mat) < 0:
Vt[2, :] *= -1
R_mat = Vt.T @ U.T
t = centroid_B - R_mat @ centroid_A
return R_mat, t
def get_camera_center(pose: np.ndarray) -> np.ndarray:
return pose[:3, 3]
def get_camera_up(pose: np.ndarray) -> np.ndarray:
# In CV convention, Y is down, so -Y is up.
# R is [x_axis, y_axis, z_axis]
return -pose[:3, 1]
def rotation_error_deg(R1: np.ndarray, R2: np.ndarray) -> float:
R_rel = R1.T @ R2
cos_theta = (np.trace(R_rel) - 1.0) / 2.0
cos_theta = np.clip(cos_theta, -1.0, 1.0)
return np.degrees(np.arccos(cos_theta))
def angle_between_vectors_deg(v1: np.ndarray, v2: np.ndarray) -> float:
v1_u = v1 / np.linalg.norm(v1)
v2_u = v2 / np.linalg.norm(v2)
cos_theta = np.dot(v1_u, v2_u)
cos_theta = np.clip(cos_theta, -1.0, 1.0)
return np.degrees(np.arccos(cos_theta))
@click.command()
@click.option(
"--calibration-json",
type=click.Path(exists=True),
required=True,
help="Calibration output format (serial -> {pose: '...'})",
)
@click.option(
"--inside-network-json",
type=click.Path(exists=True),
required=True,
help="inside_network.json nested format",
)
@click.option(
"--report-json",
type=click.Path(),
required=True,
help="Output path for comparison report",
)
@click.option(
"--aligned-inside-json",
type=click.Path(),
help="Output path for aligned inside poses",
)
def main(
calibration_json: str,
inside_network_json: str,
report_json: str,
aligned_inside_json: str | None,
):
"""
Compare two camera pose sets from different world frames using rigid alignment.
Both are treated as T_world_from_cam.
"""
with open(calibration_json, "r") as f:
calib_data = json.load(f)
with open(inside_network_json, "r") as f:
inside_data = json.load(f)
calib_poses: dict[str, np.ndarray] = {}
for serial, data in calib_data.items():
if "pose" in data:
calib_poses[str(serial)] = parse_pose(data["pose"])
inside_poses: dict[str, np.ndarray] = {}
for serial, data in inside_data.items():
# inside_network.json has FusionConfiguration nested
if "FusionConfiguration" in data and "pose" in data["FusionConfiguration"]:
inside_poses[str(serial)] = parse_pose(data["FusionConfiguration"]["pose"])
shared_serials = sorted(list(set(calib_poses.keys()) & set(inside_poses.keys())))
if len(shared_serials) < 3:
click.echo(
f"Error: Found only {len(shared_serials)} shared serials ({shared_serials}). Need at least 3.",
err=True,
)
sys.exit(1)
pts_inside = np.array([get_camera_center(inside_poses[s]) for s in shared_serials])
pts_calib = np.array([get_camera_center(calib_poses[s]) for s in shared_serials])
# Align inside to calib: R_align * pts_inside + t_align approx pts_calib
R_align, t_align = rigid_transform_3d(pts_inside, pts_calib)
T_align = np.eye(4)
T_align[:3, :3] = R_align
T_align[:3, 3] = t_align
per_cam_results = []
pos_errors = []
rot_errors = []
up_errors = []
for s in shared_serials:
T_inside = inside_poses[s]
T_calib = calib_poses[s]
# T_world_calib_from_cam = T_world_calib_from_world_inside * T_world_inside_from_cam
T_inside_aligned = T_align @ T_inside
pos_err = np.linalg.norm(
get_camera_center(T_inside_aligned) - get_camera_center(T_calib)
)
rot_err = rotation_error_deg(T_inside_aligned[:3, :3], T_calib[:3, :3])
up_inside = get_camera_up(T_inside_aligned)
up_calib = get_camera_up(T_calib)
up_err = angle_between_vectors_deg(up_inside, up_calib)
per_cam_results.append(
{
"serial": s,
"position_error_m": float(pos_err),
"rotation_error_deg": float(rot_err),
"up_consistency_error_deg": float(up_err),
}
)
pos_errors.append(pos_err)
rot_errors.append(rot_err)
up_errors.append(up_err)
report = {
"shared_serials": shared_serials,
"alignment": {
"R_align": R_align.tolist(),
"t_align": t_align.tolist(),
"T_align": T_align.tolist(),
},
"per_camera": per_cam_results,
"summary": {
"mean_position_error_m": float(np.mean(pos_errors)),
"max_position_error_m": float(np.max(pos_errors)),
"mean_rotation_error_deg": float(np.mean(rot_errors)),
"max_rotation_error_deg": float(np.max(rot_errors)),
"mean_up_consistency_error_deg": float(np.mean(up_errors)),
"max_up_consistency_error_deg": float(np.max(up_errors)),
},
}
Path(report_json).parent.mkdir(parents=True, exist_ok=True)
with open(report_json, "w") as f:
json.dump(report, f, indent=4)
click.echo(f"Report written to {report_json}")
if aligned_inside_json:
aligned_data = {}
for s, T_inside in inside_poses.items():
T_inside_aligned = T_align @ T_inside
aligned_data[s] = {"pose": serialize_pose(T_inside_aligned)}
Path(aligned_inside_json).parent.mkdir(parents=True, exist_ok=True)
with open(aligned_inside_json, "w") as f:
json.dump(aligned_data, f, indent=4)
click.echo(f"Aligned inside poses written to {aligned_inside_json}")
if __name__ == "__main__":
main()