#!/usr/bin/env python3 """ ArUco detection with multi-marker machine-frame fit + camera pose output (OpenCV >= 4.8). - Reads: webCam_1.jpg - Detects DICT_4X4_250 markers (ids expected: 0, 5, 10, 15) - Uses multiple reference markers with known machine coordinates to fit camera->machine transform - Reports positions/orientations of all markers **and the camera** in machine coordinates - Draws detected markers, per-marker axes, and the machine axes - Saves: webCam_1a.jpg (annotated) and marker_poses_machine.csv (poses incl. camera) Usage: python3 readCamPos.py -i snapshot_video1_1764493534200.jpg -npz camera_intrinsics_v0.npz -setting settings.json """ import faulthandler faulthandler.enable() import argparse import os import sys import csv import json import time from typing import Tuple, Dict, List import numpy as np import cv2 # ----------------------- Configuration Defaults ----------------------- IMAGE_PATH = "default.jpg" OUTPUT_IMAGE_PATH = "default.jpg" OUTPUT_CSV_PATH = "default.csv" OUTPUT_JSON_PATH = "default.json" # Marker side length in meters (25 mm) MARKER_LENGTH_M = 0.025 # Axis lengths for visualization (in meters) AXIS_LENGTH_M = 0.05 # per-marker axis MACHINE_AXIS_X_M = 0.200 # 200 mm along +X MACHINE_AXIS_Y_M = -0.100 # -100 mm along Y (towards camera per description) MACHINE_AXIS_Z_M = 0.100 # +Z visualized as 100 mm # Known machine coordinates for reference markers (meters) cam_anchor_pts = {} EXPECTED_IDS = {50, 71, 101} # ----------------------- Utilities ----------------------- def load_intrinsics_npz(npz_path: str) -> Tuple[np.ndarray, np.ndarray]: if os.path.exists(npz_path): print("NPZ from File:", npz_path) data = np.load(npz_path) for k in ('camera_matrix', 'mtx', 'K'): if k in data: camera_matrix = data[k].astype(np.float32) break else: raise KeyError("Camera matrix not found.") for k in ('dist_coeffs', 'dist', 'D'): if k in data: dist = data[k].astype(np.float32).reshape(-1, 1) break else: dist = np.zeros((5, 1), dtype=np.float32) return camera_matrix, dist camera_matrix = np.array([[1400, 0, 640], [0, 1400, 360], [0, 0, 1]], dtype=np.float32) dist_coeffs = np.zeros((5, 1), dtype=np.float32) print("[WARN] Using default approximate intrinsics.") return camera_matrix, dist_coeffs def rvec_to_R(rvec: np.ndarray) -> np.ndarray: R, _ = cv2.Rodrigues(rvec) return R def R_to_euler_zyx(R: np.ndarray) -> Tuple[float, float, float]: yaw = float(np.degrees(np.arctan2(R[1,0], R[0,0]))) sp = np.sqrt(R[2,1]**2 + R[2,2]**2) pitch = float(np.degrees(np.arctan2(-R[2,0], sp))) roll = float(np.degrees(np.arctan2(R[2,1], R[2,2]))) return roll, pitch, yaw def corners_to_image_points(corners: np.ndarray) -> np.ndarray: return corners.reshape(4, 2).astype(np.float32) def get_detector(): dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250) try: params = cv2.aruco.DetectorParameters() except Exception: params = cv2.aruco.DetectorParameters_create() try: detector = cv2.aruco.ArucoDetector(dictionary, params) return detector, None except Exception: return None, (dictionary, params) def detect_markers(image: np.ndarray, detector_tuple): detector, fallback = detector_tuple print(detector) if detector is not None: corners, ids, rejected = detector.detectMarkers(image) else: dictionary, params = fallback corners, ids, rejected = cv2.aruco.detectMarkers(image, dictionary, parameters=params) return corners, ids, rejected def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3" N = A.shape[0] if N < 2: raise ValueError("Need at least 2 points; 3+ recommended.") centroid_A = A.mean(axis=0) centroid_B = B.mean(axis=0) AA = A - centroid_A BB = B - centroid_B H = AA.T @ BB U, S, Vt = np.linalg.svd(H) R = Vt.T @ U.T if np.linalg.det(R) < 0: Vt[-1, :] *= -1 R = Vt.T @ U.T t = centroid_B - R @ centroid_A return R.astype(np.float32), t.astype(np.float32) def readSettings(fileSetting): global cam_anchor_pts print("Read Settings") if(fileSetting == None): cam_anchor_pts = { 50: np.array([0.0, 0.0, 0.0], dtype=np.float32), 71: np.array([0.140, 0.0, 0.0], dtype=np.float32), 101: np.array([-0.0, -0.080, 0.0], dtype=np.float32), #15: np.array([20,20,20]) # add if known } return with open(fileSetting, 'r') as f: settings = json.load(f) for marker_id, coords in settings['coordinateSystem']['KnownMarkers'].items(): cam_anchor_pts[int(marker_id)] = np.array(coords, dtype=np.float32) #KNOWN_MACHINE_POS = {int(k): np.array(v, dtype=np.float32) for k, v in settings.items()} # ----------------------- Main ----------------------- def main(): parser = argparse.ArgumentParser(description="Detect ArUco markers in two images and compute camera poses in machine coordinates.") parser.add_argument('-i', '--images', action='append', required=False, help="Path to image. Provide this option twice: once per camera (e.g., -i2 cam1.jpg -i2 cam2.jpg)") parser.add_argument('-npz', '--npz', action='append', required=False, default=['camera_intrinsics_v1.npz']) parser.add_argument('--cam-calib', action='append', required=False, help="Paths to calibration YAMLs for camera 1 and camera 2 (e.g., cam1.npz cam2.npz)") parser.add_argument('--marker-size-mm', type=float, default=25, help="Marker side length in millimeters (e.g., 50)") parser.add_argument('--dict', default='DICT_4X4_250', help="ArUco dictionary name (default: DICT_4X4_250)") parser.add_argument('-settings', type=str, default=None, help="Json File with Machine Settings") args = parser.parse_args() print("ABC 0") readSettings(args.settings) print("ABC 0") if(args.images is None): image = cv2.imread(IMAGE_PATH) else: image = cv2.imread(args.images[0]) OUTPUT_IMAGE_PATH = args.images[0].replace(".jpg","r.jpg").replace(".PNG","r.PNG") OUTPUT_CSV_PATH = args.images[0].replace(".jpg",".csv").replace(".PNG",".csv") OUTPUT_JSON_PATH = args.images[0].replace(".jpg",".json").replace(".PNG",".json") if image is None: print(f"[ERROR] Cannot read image '{IMAGE_PATH}'.") sys.exit(1) print("ABC 1") camera_matrix, dist_coeffs = load_intrinsics_npz(args.npz[0]) print("ABC 1a") detector_tuple = get_detector() print("ABC 1b") corners_list, ids, rejected = detect_markers(image, detector_tuple) print("ABC 2") if ids is None or len(ids) == 0: print("[ERROR] No markers detected.") sys.exit(1) draw_img = image.copy() cv2.aruco.drawDetectedMarkers(draw_img, corners_list, ids) half = MARKER_LENGTH_M / 2.0 obj_points = np.array([ [-half, half, 0.0], [ half, half, 0.0], [ half, -half, 0.0], [-half, -half, 0.0], ], dtype=np.float32) poses_cam: Dict[int, Tuple[np.ndarray, np.ndarray]] = {} centers_cam: Dict[int, np.ndarray] = {} for i, marker_id in enumerate(ids.flatten()): img_pts = corners_to_image_points(corners_list[i]) success, rvec, tvec = cv2.solvePnP(obj_points, img_pts, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_IPPE_SQUARE) if not success: success, rvec, tvec = cv2.solvePnP(obj_points, img_pts, camera_matrix, dist_coeffs) if success: rvec = rvec.reshape(3,1) tvec = tvec.reshape(3,1) poses_cam[int(marker_id)] = (rvec, tvec) centers_cam[int(marker_id)] = tvec.flatten() try: cv2.drawFrameAxes(draw_img, camera_matrix, dist_coeffs, rvec, tvec, AXIS_LENGTH_M) except Exception: pass else: print(f"[WARN] solvePnP failed for marker {marker_id}") common_ids: List[int] = [mid for mid in cam_anchor_pts.keys() if mid in centers_cam] if len(common_ids) < 2: print(f"[ERROR] Need at least 2 reference markers; found {len(common_ids)}: {common_ids}") sys.exit(1) if len(common_ids) < 3: print(f"[WARN] Only {len(common_ids)} references ({common_ids}). Fit may be less stable; 3+ recommended.") A = np.stack([centers_cam[mid] for mid in common_ids], axis=0) B = np.stack([cam_anchor_pts[mid] for mid in common_ids], axis=0) R_cam_to_machine, t_cam_to_machine = rigid_transform_no_scale(A, B) residuals_mm = [] for i, mid in enumerate(common_ids): pred = R_cam_to_machine @ A[i] + t_cam_to_machine err = np.linalg.norm(pred - B[i]) * 1000.0 residuals_mm.append(err) rms = float(np.sqrt(np.mean(np.square(residuals_mm)))) if residuals_mm else 0.0 print("\nReference fit residuals (mm) per marker:") for mid, e in zip(common_ids, residuals_mm): print(f" ID {mid}: {e:.2f} mm") print(f"RMS residual: {rms:.2f} mm") # Camera pose in machine coordinates: # Camera origin (0,0,0 in camera) maps to t_cam_to_machine cam_pos_machine = t_cam_to_machine cam_R_machine = R_cam_to_machine # camera basis expressed in machine frame cam_roll, cam_pitch, cam_yaw = R_to_euler_zyx(cam_R_machine) rows = [("id", "x_mm", "y_mm", "z_mm", "roll_deg", "pitch_deg", "yaw_deg")] marker_list = [] print("\nMarker Positions and Orientations in Machine Coordinates:") print(f"{'ID':>8} {'X(mm)':>10} {'Y(mm)':>10} {'Z(mm)':>10} {'Roll':>10} {'Pitch':>10} {'Yaw':>10}") # Add camera first cx, cy, cz = (cam_pos_machine * 1000.0).tolist() print(f"{'camera':>8} {cx:10.2f} {cy:10.2f} {cz:10.2f} {cam_roll:10.2f} {cam_pitch:10.2f} {cam_yaw:10.2f}") rows.append(("camera", f"{cx:.3f}", f"{cy:.3f}", f"{cz:.3f}", f"{cam_roll:.3f}", f"{cam_pitch:.3f}", f"{cam_yaw:.3f}")) camera_pose = { "id": "camera", "position_mm": [float(x) for x in cam_pos_machine * 1000.0], "orientation_deg": {"roll": cam_roll, "pitch": cam_pitch, "yaw": cam_yaw} } # Then markers for marker_id in sorted(poses_cam.keys()): rvec, tvec = poses_cam[marker_id] R_marker_cam = rvec_to_R(rvec) pos_machine = R_cam_to_machine @ tvec.flatten() + t_cam_to_machine R_marker_machine = R_cam_to_machine @ R_marker_cam roll_deg, pitch_deg, yaw_deg = R_to_euler_zyx(R_marker_machine) x_mm, y_mm, z_mm = (pos_machine * 1000.0).tolist() print(f"{marker_id:8d} {x_mm:10.2f} {y_mm:10.2f} {z_mm:10.2f} {roll_deg:10.2f} {pitch_deg:10.2f} {yaw_deg:10.2f}") rows.append((marker_id, f"{x_mm:.3f}", f"{y_mm:.3f}", f"{z_mm:.3f}", f"{roll_deg:.3f}", f"{pitch_deg:.3f}", f"{yaw_deg:.3f}")) marker_list.append({"id": marker_id, "position_mm": [x_mm, y_mm, z_mm], "orientation_deg": {"roll": roll_deg, "pitch": pitch_deg, "yaw": yaw_deg}}) # Save CSV try: with open(OUTPUT_CSV_PATH, 'w', newline='') as f: writer = csv.writer(f) writer.writerows(rows) print(f"\n[INFO] Saved CSV poses to '{OUTPUT_CSV_PATH}'.") except Exception as e: print(f"[WARN] Could not save CSV: {e}") # Save JSON json_data = { "metadata": { "timestamp": time.strftime("%Y-%m-%d %H:%M:%S"), "reference_markers": common_ids, "rms_residual_mm": rms, "description": "Multi-marker machine frame fit with camera pose" }, "camera": camera_pose, "markers": marker_list } with open(OUTPUT_JSON_PATH, 'w', encoding='utf-8') as f: json.dump(json_data, f, indent=4) # Warn about expected IDs detected_ids = set(poses_cam.keys()) missing = EXPECTED_IDS - detected_ids if missing: print(f"[WARN] Expected markers not detected: {sorted(missing)}") # Draw machine axes using global transform (machine->camera) R_machine_to_cam = R_cam_to_machine.T t_machine_to_cam = - R_machine_to_cam @ t_cam_to_machine try: machine_axes = np.float32([ [0.0, 0.0, 0.0], [MACHINE_AXIS_X_M, 0.0, 0.0], [0.0, MACHINE_AXIS_Y_M, 0.0], [0.0, 0.0, MACHINE_AXIS_Z_M], ]) rvec_global, _ = cv2.Rodrigues(R_machine_to_cam) imgpts, _ = cv2.projectPoints(machine_axes, rvec_global, t_machine_to_cam, camera_matrix, dist_coeffs) origin = tuple(np.round(imgpts[0].ravel()).astype(int)) x_end = tuple(np.round(imgpts[1].ravel()).astype(int)) y_end = tuple(np.round(imgpts[2].ravel()).astype(int)) z_end = tuple(np.round(imgpts[3].ravel()).astype(int)) cv2.line(draw_img, origin, x_end, (0, 0, 255), 3) cv2.line(draw_img, origin, y_end, (0, 255, 0), 3) cv2.line(draw_img, origin, z_end, (255, 0, 0), 3) cv2.putText(draw_img, "X (200 mm)", x_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2) cv2.putText(draw_img, "Y (-100 mm)", y_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2) cv2.putText(draw_img, "+Z (100 mm)", z_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0), 2) except Exception as e: print(f"[WARN] Failed to draw machine axes: {e}") ok = cv2.imwrite(OUTPUT_IMAGE_PATH, draw_img) if ok: print(f"[INFO] Annotated image saved as '{OUTPUT_IMAGE_PATH}'.") else: print(f"[ERROR] Failed to save annotated image '{OUTPUT_IMAGE_PATH}'.") if __name__ == '__main__': main()