Working calibration

dora_calibration/tools/analyze_observations.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+"""Analyze saved observations for issues."""
+import argparse
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Analyze hand-eye observations for issues.")
+    parser.add_argument("observations", nargs="?", default="calibration_observations.npz",
+                        help="Path to observations .npz file")
+    args = parser.parse_args()
+
+    # Load dora observations
+    dora_obs = np.load(args.observations, allow_pickle=True)
+
+    print("=== OBSERVATIONS ANALYSIS ===")
+    print(f"File: {args.observations}")
+    print(f"Keys: {list(dora_obs.keys())}")
+
+    R_gripper2base = dora_obs['R_gripper2base']
+    t_gripper2base = dora_obs['t_gripper2base']
+    R_target2cam = dora_obs['R_target2cam']
+    t_target2cam = dora_obs['t_target2cam']
+
+    n = len(R_gripper2base)
+    print(f"Count: {n}")
+
+    print("\n=== GRIPPER POSES (base_T_gripper) ===")
+    for i in range(n):
+        R = R_gripper2base[i]
+        t = t_gripper2base[i].flatten()
+
+        # Check rotation matrix validity
+        det = np.linalg.det(R)
+        orth = np.allclose(R @ R.T, np.eye(3), atol=1e-6)
+        rpy = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+
+        print(f"\nObs {i+1}:")
+        print(f"  det(R)={det:.6f}, orthogonal={orth}")
+        print(f"  t(mm)=[{t[0]*1000:.1f}, {t[1]*1000:.1f}, {t[2]*1000:.1f}]")
+        print(f"  RPY(deg)=[{rpy[0]:.1f}, {rpy[1]:.1f}, {rpy[2]:.1f}]")
+
+        if not orth:
+            print("  WARNING: Rotation matrix not orthogonal!")
+        if abs(det - 1.0) > 1e-6:
+            print(f"  WARNING: Rotation matrix determinant is {det}, expected 1.0!")
+
+    print("\n=== TARGET POSES (cam_T_target) ===")
+    for i in range(n):
+        R = R_target2cam[i]
+        t = t_target2cam[i].flatten()
+
+        det = np.linalg.det(R)
+        orth = np.allclose(R @ R.T, np.eye(3), atol=1e-6)
+
+        print(f"\nObs {i+1}:")
+        print(f"  det(R)={det:.6f}, orthogonal={orth}")
+        print(f"  t(m)=[{t[0]:.4f}, {t[1]:.4f}, {t[2]:.4f}]")
+        print(f"  distance from camera: {np.linalg.norm(t)*1000:.1f}mm")
+
+    # Check for pose diversity (important for calibration)
+    print("\n=== POSE DIVERSITY CHECK ===")
+    positions = np.array([t_gripper2base[i].flatten() for i in range(n)])
+    pos_range = positions.max(axis=0) - positions.min(axis=0)
+    print(f"Position range (mm): X={pos_range[0]*1000:.1f}, Y={pos_range[1]*1000:.1f}, Z={pos_range[2]*1000:.1f}")
+
+    rotations = np.array([Rotation.from_matrix(R_gripper2base[i]).as_euler('xyz', degrees=True) for i in range(n)])
+    rot_range = rotations.max(axis=0) - rotations.min(axis=0)
+    print(f"Rotation range (deg): Roll={rot_range[0]:.1f}, Pitch={rot_range[1]:.1f}, Yaw={rot_range[2]:.1f}")
+
+
+if __name__ == "__main__":
+    main()