diff --git a/py_workspace/.beads/issues.jsonl b/py_workspace/.beads/issues.jsonl index 5164a43..744c327 100644 --- a/py_workspace/.beads/issues.jsonl +++ b/py_workspace/.beads/issues.jsonl @@ -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."} diff --git a/py_workspace/compare_pose_sets.py b/py_workspace/compare_pose_sets.py new file mode 100644 index 0000000..78ce5e0 --- /dev/null +++ b/py_workspace/compare_pose_sets.py @@ -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()