#!/usr/bin/env python3 """ Pose diagnostics CLI. Replaces the previous individual helpers (check_mapping, validate_conversion, debug_rotation) with a single entry point that exposes three subcommands: - validate: compare ASCI vs. converted TXT in detail. - check: probe specific frames for translation/rotation parity. - rotations:inspect raw ASCI rotations or run a 90° sanity test. """ from __future__ import annotations import argparse import math from pathlib import Path from typing import Iterable, List, Sequence, Tuple import numpy as np # --------------------------------------------------------------------------- # Shared math helpers # --------------------------------------------------------------------------- def euler_zxy_to_matrix(rx: float, ry: float, rz: float) -> List[List[float]]: sx, cx = math.sin(rx), math.cos(rx) sy, cy = math.sin(ry), math.cos(ry) sz, cz = math.sin(rz), math.cos(rz) Rx = [[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]] Ry = [[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]] Rz = [[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]] return (np.dot(np.dot(Ry, Rx), Rz)).tolist() def convert_yup_to_ydown(rotation: Sequence[Sequence[float]]) -> List[List[float]]: r = rotation return [ [r[0][0], -r[0][1], r[0][2]], [-r[1][0], r[1][1], -r[1][2]], [r[2][0], -r[2][1], r[2][2]], ] def mat_vec_mul(R: Sequence[Sequence[float]], v: Sequence[float]) -> List[float]: return [ R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2], R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2], R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2], ] def read_numeric_rows(path: Path) -> List[List[float]]: rows: List[List[float]] = [] for line in path.read_text().splitlines(): stripped = line.strip() if not stripped: continue if stripped[0] not in "0123456789-.": continue rows.append(list(map(float, stripped.split()))) return rows # --------------------------------------------------------------------------- # validate command # --------------------------------------------------------------------------- def parse_pose_line(line: str) -> Tuple[float, List[float], List[List[float]]]: values = line.strip().split() if len(values) != 19: raise ValueError(f"Expected 19 columns, got {len(values)}") timestamp = float(values[0]) intrinsics = [float(v) for v in values[1:7]] matrix_values = [float(v) for v in values[7:19]] R = [ matrix_values[0:4], matrix_values[4:8], matrix_values[8:12], ] return timestamp, intrinsics, R def validate_rotation_matrix(R: Sequence[Sequence[float]], name: str) -> List[str]: issues: List[str] = [] R_np = np.array(R) det = np.linalg.det(R_np) if abs(det - 1.0) > 0.01: issues.append(f"{name}: determinant {det:.6f} (expected 1.0)") should_be_identity = np.dot(R_np, R_np.T) identity = np.eye(3) max_diff = np.abs(should_be_identity - identity).max() if max_diff > 0.01: issues.append(f"{name}: not orthogonal (max diff {max_diff:.6f})") return issues def cmd_validate(args: argparse.Namespace) -> None: asci_path = args.asci pose_path = args.pose unit_scale = args.unit_scale print(f"Reading ASCI: {asci_path}") print(f"Reading poses: {pose_path}") print(f"Using unit_scale: {unit_scale}") print() asci_lines = [l for l in asci_path.read_text().splitlines() if l.strip()] pose_lines = pose_path.read_text().splitlines() header = pose_lines[0] if pose_lines else "" pose_data = [parse_pose_line(l) for l in pose_lines[1:] if l.strip()] print(f"Found {len(asci_lines)} frames in ASCI") print(f"Found {len(pose_data)} frames in pose file") print(f"Header: {header}") if len(asci_lines) != len(pose_data): print( f"WARNING: Frame count mismatch! ASCI: {len(asci_lines)}, Poses: {len(pose_data)}" ) print() num_check = min(args.frames, len(asci_lines), len(pose_data)) print(f"Detailed comparison of first {num_check} frames:") print("-" * 80) all_issues: List[str] = [] for i in range(num_check): tx, rx_deg, ty, ry_deg, tz, rz_deg = map(float, asci_lines[i].split()) rx, ry, rz = math.radians(rx_deg), math.radians(ry_deg), math.radians(rz_deg) print(f"\nFrame {i}:") print(f" ASCI: T=({tx:.3f}, {ty:.3f}, {tz:.3f}), R=({rx_deg:.2f}°, {ry_deg:.2f}°, {rz_deg:.2f}°)") R_asci = euler_zxy_to_matrix(rx, ry, rz) R_pose = [row[:3] for row in pose_data[i][2]] t_pose = [row[3] for row in pose_data[i][2]] print(f" Pose t: ({t_pose[0]:.6f}, {t_pose[1]:.6f}, {t_pose[2]:.6f})") all_issues.extend(validate_rotation_matrix(R_asci, f"ASCI_R_{i}")) all_issues.extend(validate_rotation_matrix(R_pose, f"Pose_R_{i}")) # Translation check camera_center_scaled = np.array([tx * unit_scale, ty * unit_scale, tz * unit_scale]) expected_t = -np.array(R_pose) @ camera_center_scaled t_diff = np.abs(np.array(t_pose) - expected_t) max_t_diff = t_diff.max() if max_t_diff > 0.01: print(f" WARNING: translation mismatch (max diff {max_t_diff:.6f})") all_issues.append(f"Frame {i}: translation mismatch ({max_t_diff:.6f})") else: print(f" ✓ translation matches (max diff {max_t_diff:.6f})") print("\n" + "=" * 80) if all_issues: print("SUMMARY: issues detected:") for issue in all_issues: print(" -", issue) else: print("SUMMARY: All checks passed ✓") # --------------------------------------------------------------------------- # check command (specific frame parity) # --------------------------------------------------------------------------- def cmd_check(args: argparse.Namespace) -> None: asci_rows = read_numeric_rows(args.asci) pose_rows = read_numeric_rows(args.pose)[1:] # skip header already filtered print(f"Frames in ASCI: {len(asci_rows)}") print(f"Frames in TXT : {len(pose_rows)}") indices = args.frames or [0, len(pose_rows) - 1] for idx in indices: actual_idx = idx if idx >= 0 else len(pose_rows) + idx if actual_idx < 0 or actual_idx >= len(pose_rows): continue tx, rx, ty, ry, tz, rz = asci_rows[actual_idx] R_nuke = euler_zxy_to_matrix(math.radians(rx), math.radians(ry), math.radians(rz)) R_cv = convert_yup_to_ydown(R_nuke) t_expected = [-val for val in mat_vec_mul(R_cv, (tx, ty, tz))] pose_vals = pose_rows[actual_idx] R_export = [ pose_vals[7:11], pose_vals[11:15], pose_vals[15:19], ] rot_diff = max(abs(R_export[r][c] - R_cv[r][c]) for r in range(3) for c in range(3)) print(f"\nFrame {actual_idx}") print("Nuke Tx Ty Tz :", (tx, ty, tz)) print("Export t :", tuple(round(pose_vals[10 + i * 4], 6) for i in range(3))) print("Expected t :", tuple(round(val, 6) for val in t_expected)) print("Rotation diff :", rot_diff) print("R_nuke→OpenCV :") for row in R_cv: print(" ", " ".join(f"{val: .6f}" for val in row)) print("R_export :") for row in R_export: print(" ", " ".join(f"{val: .6f}" for val in row)) # --------------------------------------------------------------------------- # rotations command (ASCI analysis) # --------------------------------------------------------------------------- def analyze_asci_rotations(path: Path, num_frames: int = 5) -> None: print(f"Analyzing rotations in: {path}") print("=" * 80) lines = [l.strip() for l in path.read_text().splitlines() if l.strip()] num_frames = min(num_frames, len(lines)) for i in range(num_frames): values = list(map(float, lines[i].split())) tx, rx_deg, ty, ry_deg, tz, rz_deg = values print(f"\nFrame {i}: T=({tx:.3f}, {ty:.3f}, {tz:.3f}) R=({rx_deg:.2f}°, {ry_deg:.2f}°, {rz_deg:.2f}°)") R_nuke = np.array(euler_zxy_to_matrix(math.radians(rx_deg), math.radians(ry_deg), math.radians(rz_deg))) R_cv = np.array(convert_yup_to_ydown(R_nuke)) print(f" det(Nuke)={np.linalg.det(R_nuke):.6f} det(OpenCV)={np.linalg.det(R_cv):.6f}") def test_90deg_rotation() -> None: print("=" * 80) print("Testing 90° Y-axis rotation conversion") ry = math.radians(90.0) R_nuke = np.array(euler_zxy_to_matrix(0.0, ry, 0.0)) R_cv = np.array(convert_yup_to_ydown(R_nuke)) print("Nuke rotation:\n", R_nuke) print("OpenCV rotation:\n", R_cv) print(f"det Nuke={np.linalg.det(R_nuke):.6f}, det OpenCV={np.linalg.det(R_cv):.6f}") def cmd_rotations(args: argparse.Namespace) -> None: if args.test_90deg: test_90deg_rotation() analyze_asci_rotations(args.asci, args.frames) # --------------------------------------------------------------------------- # CLI # --------------------------------------------------------------------------- def build_parser() -> argparse.ArgumentParser: parser = argparse.ArgumentParser(description="Pose diagnostics helper") subparsers = parser.add_subparsers(dest="command", required=True) validate_parser = subparsers.add_parser( "validate", help="Cross-check ASCI vs converted TXT (detailed)" ) validate_parser.add_argument("asci", type=Path) validate_parser.add_argument("pose", type=Path) validate_parser.add_argument("--unit-scale", type=float, default=1.0) validate_parser.add_argument( "--frames", type=int, default=5, help="Number of initial frames to inspect" ) validate_parser.set_defaults(func=cmd_validate) check_parser = subparsers.add_parser( "check", help="Inspect specific frame indices for rotation/translation parity" ) check_parser.add_argument("asci", type=Path) check_parser.add_argument("pose", type=Path) check_parser.add_argument( "--frames", type=int, nargs="*", default=None, help="Frame indices to inspect (use negative indices for reverse order)", ) check_parser.set_defaults(func=cmd_check) rot_parser = subparsers.add_parser( "rotations", help="Inspect rotations inside an ASCI export" ) rot_parser.add_argument("asci", type=Path) rot_parser.add_argument("--frames", type=int, default=5) rot_parser.add_argument("--test-90deg", action="store_true") rot_parser.set_defaults(func=cmd_rotations) return parser def main() -> None: parser = build_parser() args = parser.parse_args() args.func(args) if __name__ == "__main__": main()