Working calibration

dora_calibration/README.md

@@ -0,0 +1,50 @@
+# Dora Calibration Node
+
+Minimal eye-in-hand calibration node for a camera mounted on the robot gripper.
+
+## Inputs
+
+- `image` (configurable via `IMAGE_INPUT`): image stream with metadata containing `encoding` and `shape` or `width/height`.
+- `camera_info` (optional): `[fx, fy, cx, cy]` float array with distortion in metadata.
+- `tcp_pose`: `[x, y, z, roll, pitch, yaw]` from `dora_ulite6` in mm/deg.
+
+## Controls (OpenCV Window)
+
+- `q` quit
+- `r` restart calibration (clear observations and start over)
+
+## Environment Variables
+
+| Variable | Default | Description |
+| --- | --- | --- |
+| `IMAGE_INPUT` | `image` | Image input name |
+| `CAMERA_INFO_INPUT` | `camera_info` | Camera info input name |
+| `POSE_INPUT` | `tcp_pose` | Robot pose input name |
+| `STATUS_INPUT` | `status` | Robot status input name (used for command completion) |
+| `COMMAND_OUTPUT` | `robot_cmd` | Output name for robot command messages |
+| `POSES_FILE` | `config/calibration_poses.yml` | YAML file path for calibration poses |
+| `POSE_UNITS` | `m` | Units for poses file (`m` or `mm`) |
+| `MOVE_SPEED` | _(unset)_ | Move speed override (mm/s) |
+| `OUTPUT_FILE` | `calibration.npz` | Output calibration file (observations saved as `{name}_observations.npz`) |
+| `METHOD` | `TSAI` | Hand-eye method (TSAI, PARK, HORAUD, ANDREFF, DANIILIDIS) |
+| `SQUARES_X` | `4` | ChArUco board squares X |
+| `SQUARES_Y` | `6` | ChArUco board squares Y |
+| `SQUARE_LENGTH` | `0.04` | Square length (m) |
+| `MARKER_LENGTH` | `0.03` | Marker length (m) |
+| `AUTO_RUN` | `true` | Automatically iterate poses and capture |
+| `WAIT_AFTER_MOVE` | `2.0` | Seconds to wait after a move before capture |
+| `MOVE_HOME_ON_START` | `true` | Move to calibration home when starting |
+| `HOME_X` | `200` | Calibration home X (mm) |
+| `HOME_Y` | `0` | Calibration home Y (mm) |
+| `HOME_Z` | `300` | Calibration home Z (mm) |
+| `HOME_ROLL` | `180` | Calibration home roll (deg) |
+| `HOME_PITCH` | `0` | Calibration home pitch (deg) |
+| `HOME_YAW` | `0` | Calibration home yaw (deg) |
+| `MIN_CORNERS` | `6` | Minimum ChArUco corners required for pose |
+| `LOG_INTERVAL` | `2.0` | Seconds between console detection logs |
+| `AUTO_SOLVE` | `true` | Automatically solve and save after last pose |
+
+## Notes
+
+- Calibration runs automatically: moves through poses, captures, and saves results.
+- For safe operation, keep a clear workspace around the robot.

dora_calibration/dora_calibration/__init__.py

@@ -0,0 +1 @@
+"""Dora calibration node package."""

dora_calibration/pyproject.toml

@@ -0,0 +1,33 @@
+[project]
+name = "dora-calibration"
+version = "0.1.0"
+license = { file = "MIT" }
+authors = [{ name = "Dora" }]
+description = "Dora node for eye-in-hand calibration with minimal OpenCV UI"
+
+requires-python = ">=3.8"
+
+dependencies = [
+  "dora-rs >= 0.3.9",
+  "numpy < 2.0.0",
+  "opencv-contrib-python >= 4.1.1",
+  "pyyaml >= 6.0",
+]
+
+[dependency-groups]
+dev = ["pytest >=8.1.1", "ruff >=0.9.1"]
+
+[project.scripts]
+dora-calibration = "dora_calibration.main:main"
+
+[tool.ruff.lint]
+extend-select = [
+  "D",
+  "UP",
+  "PERF",
+  "RET",
+  "RSE",
+  "NPY",
+  "N",
+  "I",
+]

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()

dora_calibration/tools/compare_calibrations.py

@@ -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()

dora_calibration/tools/compare_hand_eye.py

@@ -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()