#!/usr/bin/env python3 import argparse import json import os import hashlib import time import uuid from typing import Dict, Any import cv2 import numpy as np # ------------------------------------------------------------ # Utilities # ------------------------------------------------------------ def resolve_path(path): path = os.path.expanduser(path) # Absoluter Pfad → direkt verwenden if os.path.isabs(path): return path # Relativer Pfad → absolut machen (auf Basis aktuellem cwd) return os.path.abspath(path) def load_intrinsics_npz(npz_path: str): data = np.load(npz_path) for k in ('camera_matrix', 'mtx', 'K'): if k in data: K = data[k].astype(np.float32) break else: raise KeyError('Camera matrix not found in npz') for k in ('dist_coeffs', 'dist', 'D'): if k in data: D = data[k].astype(np.float32).reshape(-1, 1) break else: D = np.zeros((5, 1), dtype=np.float32) return K, D # ------------------------------------------------------------ def load_robot_vision_config(robot_json_path: str): robot_json_path = resolve_path(robot_json_path) with open(robot_json_path, 'r', encoding='utf-8') as f: robot = json.load(f) vision_config = robot.get('vision_config', {}) marker_type = vision_config.get('MarkerType', 'DICT_4X4_250') marker_size = float(vision_config.get('MarkerSize', 0.025)) return { 'MarkerType': marker_type, 'MarkerSize': marker_size } # ------------------------------------------------------------ def get_aruco_detector(dict_name: str): mapping = { 'DICT_4X4_250': cv2.aruco.DICT_4X4_250, 'DICT_5X5_100': cv2.aruco.DICT_5X5_100, 'DICT_6X6_250': cv2.aruco.DICT_6X6_250, 'DICT_ARUCO_ORIGINAL': cv2.aruco.DICT_ARUCO_ORIGINAL, } dict_id = mapping.get(dict_name, cv2.aruco.DICT_4X4_250) dictionary = cv2.aruco.getPredefinedDictionary(dict_id) 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, detector_tuple): detector, fallback = detector_tuple 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 hash_file(path): sha = hashlib.sha256() with open(path, 'rb') as f: while True: chunk = f.read(1024 * 1024) if not chunk: break sha.update(chunk) return sha.hexdigest() # ------------------------------------------------------------ def polygon_mask(shape, polygon): mask = np.zeros(shape, dtype=np.uint8) cv2.fillConvexPoly( mask, polygon.astype(np.int32), 255 ) return mask # ------------------------------------------------------------ def shrink_polygon(points, scale=0.80): center = np.mean(points, axis=0) shrunk = center + (points - center) * scale return shrunk.astype(np.float32) # ------------------------------------------------------------ def compute_sharpness(gray_image, polygon): shrunk = shrink_polygon(polygon, scale=0.80) mask = polygon_mask(gray_image.shape, shrunk) pixels = gray_image[mask == 255] if pixels.size == 0: return 0.0 temp = np.zeros_like(gray_image) temp[mask == 255] = gray_image[mask == 255] lap = cv2.Laplacian(temp, cv2.CV_64F) values = lap[mask == 255] if values.size == 0: return 0.0 return float(values.var()) # ------------------------------------------------------------ def compute_contrast(gray_image, polygon): shrunk = shrink_polygon(polygon, scale=0.80) mask = polygon_mask(gray_image.shape, shrunk) pixels = gray_image[mask == 255] if pixels.size == 0: return { 'p05': 0.0, 'p95': 0.0, 'dynamic_range': 0.0, 'mean_gray': 0.0, 'std_gray': 0.0 } p05 = float(np.percentile(pixels, 5)) p95 = float(np.percentile(pixels, 95)) return { 'p05': p05, 'p95': p95, 'dynamic_range': float(p95 - p05), 'mean_gray': float(np.mean(pixels)), 'std_gray': float(np.std(pixels)) } # ------------------------------------------------------------ def compute_edge_ratio(corners): edge_lengths = [] for k in range(4): p1 = corners[k] p2 = corners[(k + 1) % 4] edge_lengths.append( float(np.linalg.norm(p1 - p2)) ) edge_ratio = ( max(edge_lengths) / max(1e-6, min(edge_lengths)) ) return edge_ratio, edge_lengths # ------------------------------------------------------------ def compute_geometry_metrics(center, corners, width, height): image_center = np.array( [width / 2.0, height / 2.0], dtype=np.float32 ) dist_center = np.linalg.norm(center - image_center) max_dist = np.linalg.norm(image_center) distance_center_norm = float( dist_center / max(1e-6, max_dist) ) min_x = np.min(corners[:, 0]) max_x = np.max(corners[:, 0]) min_y = np.min(corners[:, 1]) max_y = np.max(corners[:, 1]) border_distance_px = float(min( min_x, min_y, width - max_x, height - max_y )) return { 'distance_to_center_norm': distance_center_norm, 'distance_to_border_px': border_distance_px } # ------------------------------------------------------------ def compute_confidence( area_px, sharpness, edge_ratio, dynamic_range, border_distance_px ): score = 1.0 # area score *= min(1.0, area_px / 1500.0) # sharpness score *= min(1.0, sharpness / 120.0) # edge distortion score *= 1.0 / max(1.0, edge_ratio) # contrast score *= min(1.0, dynamic_range / 80.0) # border distance score *= min(1.0, max(0.0, border_distance_px) / 50.0) score = max(0.0, min(1.0, score)) return float(score) # ------------------------------------------------------------ def main(): parser = argparse.ArgumentParser() parser.add_argument( '-i', '--image', required=True ) parser.add_argument( '-npz', '--intrinsics', required=True ) parser.add_argument( '-robot', '--robot', required=True ) parser.add_argument( '-cameraId', '--cameraId', required=True, type=str ) parser.add_argument( '-outDir', '--outDir', required=True ) parser.add_argument( '--saveDebugImage', action='store_true', help='Speichert ein Debug-JPG mit eingezeichneten Marker-Rahmen' ) args = parser.parse_args() out_dir = resolve_path(args.outDir) os.makedirs(out_dir, exist_ok=True) # -------------------------------------------------------- # Load robot vision config # -------------------------------------------------------- vision_config = load_robot_vision_config(args.robot) marker_type = vision_config['MarkerType'] marker_size = vision_config['MarkerSize'] # -------------------------------------------------------- # Load image # -------------------------------------------------------- image_path = resolve_path(args.image) image = cv2.imread(image_path) if image is None: raise RuntimeError(f'Cannot read image: {args.image}') gray = cv2.cvtColor( image, cv2.COLOR_BGR2GRAY ) height, width = gray.shape[:2] # -------------------------------------------------------- # Intrinsics # -------------------------------------------------------- intrinsics_path = resolve_path(args.intrinsics) K, D = load_intrinsics_npz(intrinsics_path) # -------------------------------------------------------- # Detection # -------------------------------------------------------- detector_tuple = get_aruco_detector(marker_type) corners_list, ids, rejected = detect_markers( gray, detector_tuple ) # ids_raw: original numpy array für drawDetectedMarkers ids_raw = ids detections = [] # -------------------------------------------------------- # Valid detections # -------------------------------------------------------- if ids is not None: ids = ids.flatten().tolist() for i, marker_id in enumerate(ids): corners = corners_list[i].reshape((4, 2)).astype(np.float32) center = corners.mean(axis=0) area_px = float( cv2.contourArea(corners) ) perimeter_px = float( cv2.arcLength(corners, True) ) edge_ratio, edge_lengths = compute_edge_ratio(corners) sharpness = compute_sharpness( gray, corners ) contrast = compute_contrast( gray, corners ) geometry = compute_geometry_metrics( center, corners, width, height ) confidence = compute_confidence( area_px=area_px, sharpness=sharpness, edge_ratio=edge_ratio, dynamic_range=contrast['dynamic_range'], border_distance_px=geometry['distance_to_border_px'] ) detection = { 'observation_id': str(uuid.uuid4()), 'type': 'aruco', 'marker_id': int(marker_id), 'marker_size_m': marker_size, 'image_points_px': corners.tolist(), 'center_px': center.tolist(), 'quality': { 'area_px': area_px, 'perimeter_px': perimeter_px, 'sharpness': { 'laplacian_var': sharpness }, 'contrast': contrast, 'geometry': geometry, 'edge_ratio': edge_ratio, 'edge_lengths_px': edge_lengths }, 'confidence': confidence } detections.append(detection) # -------------------------------------------------------- # Rejected candidates # -------------------------------------------------------- rejected_candidates = [] if rejected is not None: for candidate in rejected: pts = candidate.reshape((-1, 2)).astype(np.float32) center = pts.mean(axis=0) area_px = float( cv2.contourArea(pts) ) rejected_candidates.append({ 'image_points_px': pts.tolist(), 'center_px': center.tolist(), 'area_px': area_px }) # -------------------------------------------------------- # Final output # -------------------------------------------------------- output = { 'schema_version': '1.0', 'created_utc': time.strftime( '%Y-%m-%dT%H:%M:%SZ', time.gmtime() ), 'vision_config': { 'MarkerType': marker_type, 'MarkerSize': marker_size }, 'camera': { 'camera_id': args.cameraId, 'intrinsics_file': os.path.abspath(args.intrinsics), 'camera_matrix': K.tolist(), 'distortion_coefficients': D.reshape(-1).tolist() }, 'image': { 'image_file': os.path.abspath(args.image), 'image_sha256': hash_file(args.image), 'width_px': int(width), 'height_px': int(height) }, 'aruco': { 'dictionary': marker_type, 'num_detected_markers': len(detections), 'num_rejected_candidates': len(rejected_candidates) }, 'detections': detections, 'rejected_candidates': rejected_candidates } # -------------------------------------------------------- # Output path # -------------------------------------------------------- input_filename = os.path.basename(args.image) input_base = os.path.splitext(input_filename)[0] out_json = os.path.join( out_dir, f'{input_base}_aruco_detection.json' ) # -------------------------------------------------------- # Save JSON # -------------------------------------------------------- with open(out_json, 'w', encoding='utf-8') as f: json.dump( output, f, indent=2 ) print(f'Saved: {out_json}') # -------------------------------------------------------- # Debug-Bild mit Marker-Rahmen # -------------------------------------------------------- if args.saveDebugImage: debug_img = image.copy() if corners_list and ids_raw is not None: cv2.aruco.drawDetectedMarkers(debug_img, corners_list, ids_raw) debug_path = os.path.join( out_dir, f'{input_base}_debug.jpg' ) cv2.imwrite(debug_path, debug_img) print(f'Saved debug: {debug_path}') # ------------------------------------------------------------ if __name__ == '__main__': main()