Working calibration
This commit is contained in:
cristhian aguilera
74
dora_calibration/tools/analyze_observations.py
Normal file
74
dora_calibration/tools/analyze_observations.py
Normal file
@@ -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()
|
||||
114
dora_calibration/tools/compare_calibrations.py
Normal file
114
dora_calibration/tools/compare_calibrations.py
Normal file
@@ -0,0 +1,114 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Compare dora calibration with reference calibration."""
|
||||
import argparse
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def load_calibration(path):
|
||||
"""Load calibration from .npz file."""
|
||||
data = np.load(path, allow_pickle=True)
|
||||
result = {"T_cam2gripper": data["T_cam2gripper"]}
|
||||
if "camera_matrix" in data:
|
||||
result["camera_matrix"] = data["camera_matrix"]
|
||||
if "distortion" in data:
|
||||
result["distortion"] = data["distortion"]
|
||||
return result
|
||||
|
||||
|
||||
def analyze_transform(T, name):
|
||||
"""Print analysis of a 4x4 transformation matrix."""
|
||||
print(f"\n=== {name} ===")
|
||||
print(f"Translation (mm): [{T[0,3]*1000:.2f}, {T[1,3]*1000:.2f}, {T[2,3]*1000:.2f}]")
|
||||
R = T[:3, :3]
|
||||
rpy = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
|
||||
print(f"RPY (deg): [{rpy[0]:.2f}, {rpy[1]:.2f}, {rpy[2]:.2f}]")
|
||||
print(f"Full matrix:\n{T}")
|
||||
|
||||
|
||||
def analyze_camera(cam_matrix, name):
|
||||
"""Print camera intrinsics analysis."""
|
||||
print(f"\n=== {name} Camera Intrinsics ===")
|
||||
fx, fy = cam_matrix[0, 0], cam_matrix[1, 1]
|
||||
cx, cy = cam_matrix[0, 2], cam_matrix[1, 2]
|
||||
print(f"fx={fx:.2f}, fy={fy:.2f}, cx={cx:.2f}, cy={cy:.2f}")
|
||||
|
||||
# Sanity checks
|
||||
issues = []
|
||||
if fx < 100 or fy < 100:
|
||||
issues.append(f"Focal lengths too small (fx={fx:.1f}, fy={fy:.1f})")
|
||||
if cx < 100 or cy < 100:
|
||||
issues.append(f"Principal point too small (cx={cx:.1f}, cy={cy:.1f})")
|
||||
if abs(fx - fy) > 50:
|
||||
issues.append(f"Large focal length asymmetry: |fx-fy|={abs(fx-fy):.1f}")
|
||||
|
||||
if issues:
|
||||
print("WARNINGS:")
|
||||
for issue in issues:
|
||||
print(f" - {issue}")
|
||||
else:
|
||||
print("Intrinsics appear valid")
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="Compare dora vs reference calibration.")
|
||||
parser.add_argument("--dora", default="calibration.npz",
|
||||
help="Path to dora calibration result")
|
||||
parser.add_argument("--reference", default="reference_calibration.npz",
|
||||
help="Path to reference calibration result")
|
||||
parser.add_argument("--check-intrinsics", action="store_true",
|
||||
help="Also compare camera intrinsics")
|
||||
args = parser.parse_args()
|
||||
|
||||
try:
|
||||
dora_data = load_calibration(args.dora)
|
||||
dora = dora_data["T_cam2gripper"]
|
||||
except FileNotFoundError:
|
||||
print(f"Dora calibration not found: {args.dora}")
|
||||
return
|
||||
|
||||
try:
|
||||
ref_data = load_calibration(args.reference)
|
||||
ref = ref_data["T_cam2gripper"]
|
||||
except FileNotFoundError:
|
||||
print(f"Reference calibration not found: {args.reference}")
|
||||
print("Analyzing dora calibration only:")
|
||||
analyze_transform(dora, "Dora Calibration")
|
||||
if args.check_intrinsics and "camera_matrix" in dora_data:
|
||||
analyze_camera(dora_data["camera_matrix"], "Dora")
|
||||
return
|
||||
|
||||
analyze_transform(dora, "Dora Calibration")
|
||||
analyze_transform(ref, "Reference Calibration")
|
||||
|
||||
# Check camera intrinsics if requested
|
||||
if args.check_intrinsics:
|
||||
if "camera_matrix" in dora_data:
|
||||
analyze_camera(dora_data["camera_matrix"], "Dora")
|
||||
if "camera_matrix" in ref_data:
|
||||
analyze_camera(ref_data["camera_matrix"], "Reference")
|
||||
|
||||
# Compute differences
|
||||
print("\n=== DIFFERENCES ===")
|
||||
trans_diff = np.linalg.norm(dora[:3,3] - ref[:3,3]) * 1000
|
||||
print(f"Translation difference: {trans_diff:.2f} mm")
|
||||
|
||||
R_dora = dora[:3,:3]
|
||||
R_ref = ref[:3,:3]
|
||||
R_diff = R_dora @ R_ref.T
|
||||
angle_diff = np.rad2deg(np.arccos(np.clip((np.trace(R_diff) - 1) / 2, -1, 1)))
|
||||
print(f"Rotation difference: {angle_diff:.2f} deg")
|
||||
|
||||
# Thresholds for acceptable calibration
|
||||
if trans_diff < 5.0 and angle_diff < 2.0:
|
||||
print("\n[OK] Calibrations are within acceptable tolerance")
|
||||
else:
|
||||
print("\n[FAIL] Calibrations differ significantly!")
|
||||
if trans_diff >= 5.0:
|
||||
print(f" Translation diff {trans_diff:.2f}mm exceeds 5mm threshold")
|
||||
if angle_diff >= 2.0:
|
||||
print(f" Rotation diff {angle_diff:.2f} deg exceeds 2 deg threshold")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
65
dora_calibration/tools/compare_hand_eye.py
Normal file
65
dora_calibration/tools/compare_hand_eye.py
Normal file
@@ -0,0 +1,65 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Recompute hand-eye calibration from an observations .npz file."""
|
||||
|
||||
import argparse
|
||||
from typing import List
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _method_from_name(name: str) -> int:
|
||||
name = name.upper()
|
||||
methods = {
|
||||
"TSAI": cv2.CALIB_HAND_EYE_TSAI,
|
||||
"PARK": cv2.CALIB_HAND_EYE_PARK,
|
||||
"HORAUD": cv2.CALIB_HAND_EYE_HORAUD,
|
||||
"ANDREFF": cv2.CALIB_HAND_EYE_ANDREFF,
|
||||
"DANIILIDIS": cv2.CALIB_HAND_EYE_DANIILIDIS,
|
||||
}
|
||||
return methods.get(name, cv2.CALIB_HAND_EYE_TSAI)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Recompute hand-eye from observations npz.")
|
||||
parser.add_argument("observations", help="*_observations.npz path")
|
||||
parser.add_argument("--method", default="TSAI")
|
||||
args = parser.parse_args()
|
||||
|
||||
data = np.load(args.observations, allow_pickle=True)
|
||||
R_gripper2base: List[np.ndarray] = list(data["R_gripper2base"])
|
||||
t_gripper2base: List[np.ndarray] = list(data["t_gripper2base"])
|
||||
R_target2cam: List[np.ndarray] = list(data["R_target2cam"])
|
||||
t_target2cam: List[np.ndarray] = list(data["t_target2cam"])
|
||||
|
||||
method = _method_from_name(args.method)
|
||||
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
|
||||
R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, method=method
|
||||
)
|
||||
|
||||
T_cam2gripper = np.eye(4)
|
||||
T_cam2gripper[:3, :3] = R_cam2gripper
|
||||
T_cam2gripper[:3, 3] = t_cam2gripper.reshape(3)
|
||||
|
||||
base_targets = []
|
||||
for i in range(len(R_gripper2base)):
|
||||
base_T_gripper = np.eye(4)
|
||||
base_T_gripper[:3, :3] = R_gripper2base[i]
|
||||
base_T_gripper[:3, 3] = np.array(t_gripper2base[i]).reshape(3)
|
||||
cam_T_target = np.eye(4)
|
||||
cam_T_target[:3, :3] = R_target2cam[i]
|
||||
cam_T_target[:3, 3] = np.array(t_target2cam[i]).reshape(3)
|
||||
base_T_target = base_T_gripper @ T_cam2gripper @ cam_T_target
|
||||
base_targets.append(base_T_target[:3, 3])
|
||||
base_targets = np.array(base_targets)
|
||||
mean_target = base_targets.mean(axis=0)
|
||||
errors = np.linalg.norm(base_targets - mean_target[None, :], axis=1)
|
||||
|
||||
print("T_cam2gripper:")
|
||||
print(T_cam2gripper)
|
||||
print(f"Mean translation error: {errors.mean()*1000:.2f} mm")
|
||||
print(f"Max translation error: {errors.max()*1000:.2f} mm")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user