test_rotation_fix.py

#!/usr/bin/env python3
"""
Quick test to verify the Z-180° fix removal resolved the rotation mismatch.
This should show much smaller rotation errors after reprocessing.
"""
import numpy as np
from scipy.spatial.transform import Rotation as R

def test_rotation_convention():
    """Test that incam and world rotations are consistent after Z-180 fix removal."""

    # Example Unity quaternions (from your data)
    cam_quat = np.array([0.0, 0.0, 0.0, 1.0])  # Identity
    pelvis_quat = np.array([0.0, 0.707, 0.0, 0.707])  # 90° around Y

    # Unity -> CV coordinate conversion
    C = np.diag([1.0, -1.0, 1.0])

    # OLD method (with Z-180° fix)
    R_cam_w = R.from_quat(cam_quat).as_matrix()
    R_pel_w = R.from_quat(pelvis_quat).as_matrix()
    R_rel_unity = R_cam_w.T @ R_pel_w
    R_cv_old = C @ R_rel_unity @ C
    R_final_old = R_cv_old @ R.from_euler("z", 180, degrees=True).as_matrix()

    # NEW method (without Z-180° fix)
    R_cv_new = C @ R_rel_unity @ C

    # Compare
    euler_old = R.from_matrix(R_final_old).as_euler('YXZ', degrees=True)
    euler_new = R.from_matrix(R_cv_new).as_euler('YXZ', degrees=True)

    print("=== Rotation Convention Test ===")
    print(f"OLD (with Z-180°): yaw={euler_old[0]:7.2f}, pitch={euler_old[1]:7.2f}, roll={euler_old[2]:7.2f}")
    print(f"NEW (no Z-180°):   yaw={euler_new[0]:7.2f}, pitch={euler_new[1]:7.2f}, roll={euler_new[2]:7.2f}")
    print(f"Difference:        yaw={euler_new[0]-euler_old[0]:7.2f}, pitch={euler_new[1]-euler_old[1]:7.2f}, roll={euler_new[2]-euler_old[2]:7.2f}")
    print()
    print("Expected: ~180° difference in roll (confirming the fix)")
    print()

if __name__ == "__main__":
    test_rotation_convention()
    print("After reprocessing with the updated script, run diagnose_data.py again.")
    print("You should see:")
    print("  - In-camera roll error: ~0-10° (instead of 348°)")
    print("  - World orientation errors: <5° for all axes")
    print("  - Body pose errors: <10° mean")