217 lines
6.4 KiB
Python
217 lines
6.4 KiB
Python
#!/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()
|