2 Camera Detection

Checke wo sich im Bild die Kamera befindet

programs/01_read_ArUco_jpg_to_json.py

@@ -1,272 +0,0 @@
-#!/usr/bin/env python3
-
-import argparse
-import json
-import os
-import hashlib
-import time
-from typing import Dict, Any
-
-import cv2
-import numpy as np
-
-
-# ------------------------------------------------------------
-# Utilities
-# ------------------------------------------------------------
-
-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 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 compute_sharpness(gray_roi):
-    if gray_roi.size == 0:
-        return 0.0
-
-    return float(cv2.Laplacian(gray_roi, cv2.CV_64F).var())
-
-
-# ------------------------------------------------------------
-
-def compute_local_stats(gray_roi):
-    if gray_roi.size == 0:
-        return 0.0, 0.0
-
-    mean = float(np.mean(gray_roi))
-    std = float(np.std(gray_roi))
-
-    return mean, std
-
-
-# ------------------------------------------------------------
-
-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 main():
-
-    parser = argparse.ArgumentParser()
-
-    parser.add_argument('-i', '--image', required=True)
-    parser.add_argument('-npz', '--intrinsics', required=True)
-    parser.add_argument('-cameraId', '--cameraId', required=True)
-
-    parser.add_argument(
-        '--dict',
-        default='DICT_4X4_250'
-    )
-
-    parser.add_argument(
-        '-o',
-        '--output',
-        default=None
-    )
-
-    args = parser.parse_args()
-
-    image = cv2.imread(args.image)
-
-    if image is None:
-        raise RuntimeError(f'Cannot read image: {args.image}')
-
-    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
-
-    h, w = gray.shape[:2]
-
-    K, D = load_intrinsics_npz(args.intrinsics)
-
-    detector_tuple = get_aruco_detector(args.dict)
-
-    corners_list, ids, rejected = detect_markers(gray, detector_tuple)
-
-    observations = []
-
-    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))
-
-            x_min = float(np.min(corners[:,0]))
-            x_max = float(np.max(corners[:,0]))
-            y_min = float(np.min(corners[:,1]))
-            y_max = float(np.max(corners[:,1]))
-
-            bbox_x = int(max(0, np.floor(x_min)))
-            bbox_y = int(max(0, np.floor(y_min)))
-            bbox_w = int(min(w - bbox_x, np.ceil(x_max - x_min)))
-            bbox_h = int(min(h - bbox_y, np.ceil(y_max - y_min)))
-
-            roi = gray[
-                bbox_y:bbox_y+bbox_h,
-                bbox_x:bbox_x+bbox_w
-            ]
-
-            sharpness = compute_sharpness(roi)
-
-            mean_gray, std_gray = compute_local_stats(roi)
-
-            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))
-            )
-
-            obs = {
-                'marker_id': int(marker_id),
-
-                'corners_px': corners.tolist(),
-
-                'center_px': center.tolist(),
-
-                'bbox_px': {
-                    'x': x_min,
-                    'y': y_min,
-                    'w': x_max - x_min,
-                    'h': y_max - y_min
-                },
-
-                'quality': {
-                    'area_px': area_px,
-                    'perimeter_px': perimeter_px,
-                    'sharpness_laplacian': sharpness,
-                    'mean_gray': mean_gray,
-                    'std_gray': std_gray,
-                    'edge_ratio': edge_ratio,
-                    'edge_lengths_px': edge_lengths
-                }
-            }
-
-            observations.append(obs)
-
-    output = {
-        'schema_version': '1.0',
-
-        'created_utc': time.strftime('%Y-%m-%dT%H:%M:%SZ', time.gmtime()),
-
-        '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(w),
-            'height_px': int(h)
-        },
-
-        'aruco': {
-            'dictionary': args.dict,
-            'num_detected_markers': len(observations),
-            'num_rejected_candidates': int(len(rejected))
-        },
-
-        'observations': observations
-    }
-
-    if args.output is None:
-        base = os.path.splitext(args.image)[0]
-        out_json = f'{base}_aruco_detection.json'
-    else:
-        out_json = args.output
-
-    with open(out_json, 'w', encoding='utf-8') as f:
-        json.dump(output, f, indent=2)
-
-    print(f'Saved: {out_json}')
-
-
-# ------------------------------------------------------------
-
-if __name__ == '__main__':
-    main()

programs/02_build_scene_json.py

@@ -1,216 +0,0 @@
-#!/usr/bin/env python3
-
-import argparse
-import glob
-import json
-import os
-import time
-from collections import defaultdict
-
-
-# ------------------------------------------------------------
-
-def load_detection_json(path):
-
-    with open(path, 'r', encoding='utf-8') as f:
-        data = json.load(f)
-
-    return data
-
-
-# ------------------------------------------------------------
-
-def find_detection_files(directory, timestamp):
-
-    pattern = os.path.join(
-        directory,
-        f'*_{timestamp}_aruco_detection.json'
-    )
-
-    files = sorted(glob.glob(pattern))
-
-    return files
-
-
-# ------------------------------------------------------------
-
-def build_scene(detection_files, timestamp):
-
-    scene = {
-
-        "schema_version": "1.0",
-
-        "scene_timestamp": str(timestamp),
-
-        "created_utc": time.strftime(
-            "%Y-%m-%dT%H:%M:%SZ",
-            time.gmtime()
-        ),
-
-        "cameras": {},
-
-        "markers": {}
-    }
-
-    marker_map = defaultdict(list)
-
-    # --------------------------------------------------------
-    # Read all camera detection jsons
-    # --------------------------------------------------------
-
-    for json_file in detection_files:
-
-        data = load_detection_json(json_file)
-
-        cam = data["camera"]
-        img = data["image"]
-
-        basename = os.path.basename(json_file)
-        # snapshot_video1_1778819665744_aruco_detection.json
-        parts = basename.split('_')
-        video_part = parts[1]          # "video1"
-        camera_id = video_part.replace('video', '')
-
-        # ----------------------------------------------------
-        # Camera information
-        # ----------------------------------------------------
-
-        scene["cameras"][camera_id] = {
-
-            "camera_id": camera_id,
-
-            "intrinsics_file": cam["intrinsics_file"],
-
-            "camera_matrix": cam["camera_matrix"],
-
-            "distortion_coefficients":
-                cam["distortion_coefficients"],
-
-            "image": {
-
-                "image_file": img["image_file"],
-
-                "image_sha256": img["image_sha256"],
-
-                "width_px": img["width_px"],
-
-                "height_px": img["height_px"]
-            }
-        }
-
-        # ----------------------------------------------------
-        # Marker observations
-        # ----------------------------------------------------
-
-        for obs in data["observations"]:
-
-            marker_id = str(obs["marker_id"])
-
-            observation = {
-
-                "camera_id": camera_id,
-
-                "center_px": obs["center_px"],
-
-                "corners_px": obs["corners_px"],
-
-                "bbox_px": obs["bbox_px"],
-
-                "quality": obs["quality"]
-            }
-
-            marker_map[marker_id].append(observation)
-
-    # --------------------------------------------------------
-    # Convert marker map
-    # --------------------------------------------------------
-
-    for marker_id, observations in marker_map.items():
-
-        scene["markers"][marker_id] = {
-
-            "marker_id": int(marker_id),
-
-            "num_observations": len(observations),
-
-            "observations": observations
-        }
-
-    return scene
-
-
-# ------------------------------------------------------------
-
-def main():
-
-    parser = argparse.ArgumentParser()
-
-    parser.add_argument(
-        '-timestamp',
-        required=True,
-        help='Scene timestamp'
-    )
-
-    parser.add_argument(
-        '-dir',
-        default='.',
-        help='Directory containing detection json files'
-    )
-
-    parser.add_argument(
-        '-o',
-        '--output',
-        default=None
-    )
-
-    args = parser.parse_args()
-
-    detection_files = find_detection_files(
-        args.dir,
-        args.timestamp
-    )
-
-    if len(detection_files) == 0:
-
-        raise RuntimeError(
-            f'No detection json files found for timestamp '
-            f'{args.timestamp}'
-        )
-
-    print('Found detection files:')
-
-    for f in detection_files:
-        print('  ', f)
-
-    scene = build_scene(
-        detection_files,
-        args.timestamp
-    )
-
-    # --------------------------------------------------------
-    # Output filename
-    # --------------------------------------------------------
-
-    if args.output is None:
-
-        out_json = os.path.join(
-            args.dir,
-            f'scene_{args.timestamp}.json'
-        )
-
-    else:
-
-        out_json = args.output
-
-    with open(out_json, 'w', encoding='utf-8') as f:
-
-        json.dump(scene, f, indent=2)
-
-    print()
-    print(f'Saved scene: {out_json}')
-
-
-# ------------------------------------------------------------
-
-if __name__ == '__main__':
-    main()

programs/1_detect_aruco_observations.py

@@ -0,0 +1,587 @@
+#!/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 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):
+
+    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
+    )
+
+    args = parser.parse_args()
+
+    os.makedirs(args.outDir, 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 = cv2.imread(args.image)
+
+    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
+    # --------------------------------------------------------
+
+    K, D = load_intrinsics_npz(args.intrinsics)
+
+    # --------------------------------------------------------
+    # Detection
+    # --------------------------------------------------------
+
+    detector_tuple = get_aruco_detector(marker_type)
+
+    corners_list, ids, rejected = detect_markers(
+        gray,
+        detector_tuple
+    )
+
+    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(
+        args.outDir,
+        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}')
+
+
+# ------------------------------------------------------------
+
+if __name__ == '__main__':
+    main()

programs/2_estimate_camera_pose_from_aruco_json.py

@@ -0,0 +1,383 @@
+#!/usr/bin/env python3
+"""
+estimate_camera_pose_from_aruco_json.py
+
+Berechnet die Kameraposition im Maschinen-/Board-Koordinatensystem
+aus:
+  1. einer ArUco-Detections-JSON
+  2. robots.json mit bekannten Marker-Positionen
+
+Das Script verwendet ausschließlich bekannte Marker
+und bestimmt daraus die Kamera-Extrinsics mittels solvePnP.
+
+Ergebnis:
+- Kamera-Position im Weltkoordinatensystem
+- Kamera-Orientierung (Roll/Pitch/Yaw)
+- optionale Reprojektion zur Qualitätskontrolle
+
+Benötigt:
+    pip install opencv-python numpy
+
+Beispiel:
+    python 2_estimate_camera_pose_from_aruco_json.py \
+        --detections detection.json \
+        --robots robots.json
+"""
+
+import argparse
+import json
+import math
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+# ============================================================
+# Hilfsfunktionen
+# ============================================================
+
+def rvec_tvec_to_camera_pose(rvec, tvec):
+    """
+    OpenCV liefert:
+        X_cam = R * X_world + t
+
+    Gesucht:
+        Kamera-Pose im Weltkoordinatensystem
+
+    => R_wc = R^T
+    => C = -R^T * t
+    """
+
+    R_cw, _ = cv2.Rodrigues(rvec)
+
+    R_wc = R_cw.T
+    cam_pos = -R_wc @ tvec
+
+    return R_wc, cam_pos.reshape(3)
+
+
+def rotation_matrix_to_euler_zyx(R):
+    """
+    Euler ZYX:
+        yaw(Z), pitch(Y), roll(X)
+    """
+
+    yaw = math.degrees(math.atan2(R[1, 0], R[0, 0]))
+
+    sp = math.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)
+
+    pitch = math.degrees(math.atan2(-R[2, 0], sp))
+
+    roll = math.degrees(math.atan2(R[2, 1], R[2, 2]))
+
+    return roll, pitch, yaw
+
+
+def build_marker_lookup(robot_data):
+    """
+    Liest nur Marker mit ABSOLUTER Position.
+
+    Unterstützt:
+        "position"  -> absolute Weltposition [m]
+        "relPos"    -> wird aktuell ignoriert
+    """
+
+    marker_lookup = {}
+
+    for marker in robot_data.get("Marker", []):
+
+        marker_id = int(marker.get("id", -1))
+
+        # negative IDs ignorieren
+        if marker_id < 0:
+            continue
+
+        # Nur absolute Weltpositionen verwenden
+        if "position" not in marker:
+            continue
+
+        pos = marker["position"]
+
+        if pos is None:
+            continue
+
+        if len(pos) != 3:
+            continue
+
+        marker_lookup[marker_id] = np.array(
+            pos,
+            dtype=np.float32
+        )
+
+    return marker_lookup
+
+def build_marker_object_points(marker_center_world, marker_size_m):
+    """
+    Baut die 3D-Eckpunkte eines Markers auf.
+
+    Wichtig:
+    Die Corner-Reihenfolge MUSS zur OpenCV-ArUco-Reihenfolge passen.
+
+    Reihenfolge:
+        top-left
+        top-right
+        bottom-right
+        bottom-left
+    """
+
+    half = marker_size_m / 2.0
+
+    local = np.array([
+        [-half,  half, 0.0],
+        [ half,  half, 0.0],
+        [ half, -half, 0.0],
+        [-half, -half, 0.0],
+    ], dtype=np.float32)
+
+    return local + marker_center_world.reshape(1, 3)
+
+
+# ============================================================
+# Main
+# ============================================================
+
+def main():
+
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument(
+        "--detections",
+        required=True,
+        help="ArUco detection JSON"
+    )
+
+    parser.add_argument(
+        "--robots",
+        required=True,
+        help="robots.json"
+    )
+
+    parser.add_argument(
+        "--min-confidence",
+        type=float,
+        default=0.5,
+        help="Minimale Marker-Confidence"
+    )
+
+    parser.add_argument(
+        "--max-reprojection-error",
+        type=float,
+        default=3.0,
+        help="Maximal erlaubter Reprojektionsfehler in Pixel"
+    )
+
+    args = parser.parse_args()
+
+    # ============================================================
+    # JSON laden
+    # ============================================================
+
+    with open(args.detections, "r", encoding="utf-8") as f:
+        detection_data = json.load(f)
+
+    with open(args.robots, "r", encoding="utf-8") as f:
+        robot_data = json.load(f)
+
+    # ============================================================
+    # Kamera-Parameter
+    # ============================================================
+
+    K = np.array(
+        detection_data["camera"]["camera_matrix"],
+        dtype=np.float32
+    )
+
+    D = np.array(
+        detection_data["camera"]["distortion_coefficients"],
+        dtype=np.float32
+    ).reshape(-1, 1)
+
+    # ============================================================
+    # Bekannte Marker
+    # ============================================================
+
+    known_markers = build_marker_lookup(robot_data)
+
+    # ============================================================
+    # 2D/3D Punktlisten aufbauen
+    # ============================================================
+
+    object_points = []
+    image_points = []
+
+    used_markers = []
+
+    detections = detection_data["detections"]
+
+    for det in detections:
+
+        marker_id = int(det["marker_id"])
+
+        confidence = float(det.get("confidence", 1.0))
+
+        if confidence < args.min_confidence:
+            continue
+
+        if marker_id not in known_markers:
+            continue
+
+        marker_center_world = known_markers[marker_id]
+
+        marker_size = float(det["marker_size_m"])
+
+        obj_pts = build_marker_object_points(
+            marker_center_world,
+            marker_size
+        )
+
+        img_pts = np.array(
+            det["image_points_px"],
+            dtype=np.float32
+        )
+
+        object_points.append(obj_pts)
+        image_points.append(img_pts)
+
+        used_markers.append(marker_id)
+
+    if len(object_points) == 0:
+        raise RuntimeError(
+            "Keine bekannten Marker gefunden."
+        )
+
+    object_points = np.concatenate(object_points, axis=0)
+    image_points = np.concatenate(image_points, axis=0)
+
+    print()
+    print("==================================================")
+    print("Bekannte Marker verwendet:")
+    print(sorted(set(used_markers)))
+    print("==================================================")
+    print()
+
+    # ============================================================
+    # solvePnP
+    # ============================================================
+
+    success, rvec, tvec = cv2.solvePnP(
+        object_points,
+        image_points,
+        K,
+        D,
+        flags=cv2.SOLVEPNP_ITERATIVE
+    )
+
+    if not success:
+        raise RuntimeError("solvePnP fehlgeschlagen")
+
+    # ============================================================
+    # Kamera-Pose berechnen
+    # ============================================================
+
+    R_wc, cam_pos = rvec_tvec_to_camera_pose(
+        rvec,
+        tvec
+    )
+
+    roll, pitch, yaw = rotation_matrix_to_euler_zyx(R_wc)
+
+    # ============================================================
+    # Reprojektionsfehler
+    # ============================================================
+
+    projected, _ = cv2.projectPoints(
+        object_points,
+        rvec,
+        tvec,
+        K,
+        D
+    )
+
+    projected = projected.reshape(-1, 2)
+
+    reproj_error = np.linalg.norm(
+        projected - image_points,
+        axis=1
+    )
+
+    rms = np.sqrt(np.mean(reproj_error ** 2))
+    max_err = np.max(reproj_error)
+
+    # ============================================================
+    # Ausgabe
+    # ============================================================
+
+    print("==================================================")
+    print("KAMERA-POSE")
+    print("==================================================")
+    print()
+
+    print("Position [m]")
+    print(f"  X = {cam_pos[0]: .6f}")
+    print(f"  Y = {cam_pos[1]: .6f}")
+    print(f"  Z = {cam_pos[2]: .6f}")
+
+    print()
+
+    print("Orientation [deg]")
+    print(f"  Roll  = {roll: .3f}")
+    print(f"  Pitch = {pitch: .3f}")
+    print(f"  Yaw   = {yaw: .3f}")
+
+    print()
+
+    print("Reprojection Error")
+    print(f"  RMS = {rms:.3f} px")
+    print(f"  MAX = {max_err:.3f} px")
+
+    print()
+
+    if max_err > args.max_reprojection_error:
+        print("[WARNUNG] Reprojektionsfehler relativ hoch")
+    else:
+        print("[OK] Pose stabil")
+
+    print()
+
+    # ============================================================
+    # JSON speichern
+    # ============================================================
+
+    out = {
+        "camera_pose": {
+            "position_m": {
+                "x": float(cam_pos[0]),
+                "y": float(cam_pos[1]),
+                "z": float(cam_pos[2]),
+            },
+            "orientation_deg": {
+                "roll": float(roll),
+                "pitch": float(pitch),
+                "yaw": float(yaw),
+            }
+        },
+        "quality": {
+            "reprojection_rms_px": float(rms),
+            "reprojection_max_px": float(max_err),
+            "num_markers_used": len(set(used_markers)),
+            "markers_used": sorted(set(used_markers))
+        }
+    }
+
+    out_file = Path(args.detections).with_suffix(".camera_pose.json")
+
+    with open(out_file, "w", encoding="utf-8") as f:
+        json.dump(out, f, indent=2)
+
+    print(f"Pose gespeichert in:")
+    print(out_file)
+
+
+if __name__ == "__main__":
+    main()