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