add compare_pose_sets.py for rigid alignment and comparison of camera pose sets
This commit is contained in:
@@ -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()
|
||||
Reference in New Issue
Block a user