#!/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()