From f37097ea96e195ac2112fde7c60cd45c7bc304b1 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Mon, 25 May 2026 16:50:48 +0200 Subject: [PATCH] 2 Camera Detection Checke wo sich im Bild die Kamera befindet --- .vscode/launch.json | 19 + programs/01_read_ArUco_jpg_to_json.py | 272 -------- programs/02_build_scene_json.py | 216 ------- programs/1_detect_aruco_observations.py | 587 ++++++++++++++++++ .../2_estimate_camera_pose_from_aruco_json.py | 383 ++++++++++++ ..._detect_aruco_observations.cpython-311.pyc | Bin 0 -> 148 bytes test/00_testSetup.json | 45 -- test/01_read_ArUco.test.js | 64 -- test/02_build_scene.test.js | 55 -- test/03a_cameraPose.test.js | 74 --- test/0_path_OK.test.js | 99 +++ test/1_detect_aruco_observations.test.js | 70 +++ ...timate_camera_pose_from_aruco_json.test.js | 78 +++ test/data/0_testSetup.json | 45 ++ test/data/robot/robot.json | 5 +- 15 files changed, 1285 insertions(+), 727 deletions(-) create mode 100644 .vscode/launch.json delete mode 100644 programs/01_read_ArUco_jpg_to_json.py delete mode 100644 programs/02_build_scene_json.py create mode 100644 programs/2_estimate_camera_pose_from_aruco_json.py create mode 100644 programs/__pycache__/1_detect_aruco_observations.cpython-311.pyc delete mode 100644 test/00_testSetup.json delete mode 100644 test/01_read_ArUco.test.js delete mode 100644 test/02_build_scene.test.js delete mode 100644 test/03a_cameraPose.test.js create mode 100644 test/0_path_OK.test.js create mode 100644 test/2_estimate_camera_pose_from_aruco_json.test.js create mode 100644 test/data/0_testSetup.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..5794dbd --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,19 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "Debug ArUco Detection", + "type": "python", + "request": "launch", + "program": "${workspaceFolder}/programs/1_detect_aruco_observations.py", + "console": "integratedTerminal", + "args": [ + "-i", "${workspaceFolder}/test/data/screenshots/snapshot_video1_1778819665744.jpg", + "-npz", "${workspaceFolder}/data/settings/callibration_cam1.npz", + "-robot", "${workspaceFolder}/test/data/robot/robot.json", + "-cameraId", "cam1", + "-outDir", "${workspaceFolder}/test/data/screenshots/1778819665744_detection_Step1_testResults" + ] + } + ] +} \ No newline at end of file diff --git a/programs/01_read_ArUco_jpg_to_json.py b/programs/01_read_ArUco_jpg_to_json.py deleted file mode 100644 index c18a145..0000000 --- a/programs/01_read_ArUco_jpg_to_json.py +++ /dev/null @@ -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() diff --git a/programs/02_build_scene_json.py b/programs/02_build_scene_json.py deleted file mode 100644 index 7c0cfb2..0000000 --- a/programs/02_build_scene_json.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/programs/1_detect_aruco_observations.py b/programs/1_detect_aruco_observations.py index e69de29..90f9405 100644 --- a/programs/1_detect_aruco_observations.py +++ b/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() \ No newline at end of file diff --git a/programs/2_estimate_camera_pose_from_aruco_json.py b/programs/2_estimate_camera_pose_from_aruco_json.py new file mode 100644 index 0000000..553afb4 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/programs/__pycache__/1_detect_aruco_observations.cpython-311.pyc b/programs/__pycache__/1_detect_aruco_observations.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5ab07116e939e52b816859f48977d26c42d917f1 GIT binary patch literal 148 zcmZ3^%ge<81lGJFSs?l`h=2h`DC095kTIPhg&~+hlhJP_LlF~@{~09tOTC~dKfNe1 zw^-jWJ|(pzHMt}{v8XgTKR!RHIJKxOu_QA;uUN03@)w6qZhlH>PO4oID^MTE { - const scriptPath = 'programs/01_read_AruCo_jpg_to_json.py'; - const calibrationFile = 'data/settings/callibration_cam0.npz'; - const sourceScreenshotsDir = path.join(__dirname, 'data', 'screenShots'); - const screenshotFiles = [ - 'snapshot_video0_1778819665744.jpg', - 'snapshot_video1_1778819665744.jpg', - ]; - - beforeEach(() => { - screenshotFiles.forEach((file) => { - const src = path.join(sourceScreenshotsDir, file); - if (!fs.existsSync(src)) { - throw new Error(`Missing test fixture screenshot: ${src}`); - } - }); - }); - - afterEach(() => { - // Keep generated detection JSON files in the screenshot directory. - }); - - test('should exist and be executable', () => { - expect(fs.existsSync(scriptPath)).toBe(true); - }); - - test('should have calibration file', () => { - expect(fs.existsSync(calibrationFile)).toBe(true); - }); - - test('should have screenshot directory', () => { - expect(fs.existsSync(sourceScreenshotsDir)).toBe(true); - }); - - test('should process screenshots successfully', () => { - const screenshots = fs.readdirSync(sourceScreenshotsDir) - .filter(file => file.endsWith('.jpg') || file.endsWith('.png')) - .filter(file => screenshotFiles.includes(file)); - - expect(screenshots.length).toBeGreaterThan(0); - - screenshots.forEach(screenshot => { - const screenshotPath = path.join(sourceScreenshotsDir, screenshot); - const jsonPath = screenshotPath.replace(/\.(jpg|png)$/i, '_aruco_detection.json'); - const cmd = `python ${scriptPath} -i "${screenshotPath}" -npz "${calibrationFile}" -cameraId 0`; - - try { - execSync(cmd, { stdio: 'inherit' }); - expect(fs.existsSync(jsonPath)).toBe(true); - - const jsonData = JSON.parse(fs.readFileSync(jsonPath, 'utf8')); - expect(jsonData).toBeDefined(); - expect(typeof jsonData).toBe('object'); - } catch (error) { - fail(`Failed to process ${screenshot}: ${error.message}`); - } - }); - }); -}); \ No newline at end of file diff --git a/test/02_build_scene.test.js b/test/02_build_scene.test.js deleted file mode 100644 index aa7f37f..0000000 --- a/test/02_build_scene.test.js +++ /dev/null @@ -1,55 +0,0 @@ -// test/02_build_scene_json.test.js -const { execSync } = require('child_process'); -const fs = require('fs'); -const path = require('path'); - -describe('Build Scene JSON Script', () => { - const scriptPath = 'programs/02_build_scene_json.py'; - const timestamp = 1778819665744; - const screenshotDir = path.join(__dirname, 'data', 'screenShots'); - const detectionFiles = [ - 'snapshot_video0_1778819665744_aruco_detection.json', - 'snapshot_video1_1778819665744_aruco_detection.json', - ]; - - beforeEach(() => { - detectionFiles.forEach((file) => { - const src = path.join(screenshotDir, file); - if (!fs.existsSync(src)) { - throw new Error(`Missing test fixture detection file: ${src}`); - } - }); - }); - - afterEach(() => { - // Keep the generated scene JSON in the screenshot directory for real-world behavior. - }); - - test('should exist and be executable', () => { - expect(fs.existsSync(scriptPath)).toBe(true); - }); - - test('should build scene JSON with timestamp parameter', () => { - const cmd = `python ${scriptPath} -timestamp ${timestamp} -dir "${screenshotDir}"`; - - try { - execSync(cmd, { stdio: 'inherit' }); - } catch (error) { - throw new Error(`Failed to build scene JSON: ${error.message}`); - } - - const expectedJsonFile = path.join(screenshotDir, `scene_${timestamp}.json`); - expect(fs.existsSync(expectedJsonFile)).toBe(true); - - const jsonData = JSON.parse(fs.readFileSync(expectedJsonFile, 'utf8')); - expect(jsonData).toBeDefined(); - expect(typeof jsonData).toBe('object'); - expect(jsonData.cameras).toBeDefined(); - expect(Object.keys(jsonData.cameras).sort()).toEqual(['0', '1']); - }); - - test('should handle timestamp parameter correctly', () => { - // Überprüfe, ob der Timestamp korrekt verarbeitet wird - expect(timestamp).toBe(1778819665744); - }); -}); \ No newline at end of file diff --git a/test/03a_cameraPose.test.js b/test/03a_cameraPose.test.js deleted file mode 100644 index 0c1a3b0..0000000 --- a/test/03a_cameraPose.test.js +++ /dev/null @@ -1,74 +0,0 @@ -// test/03a_cameraPose.test.js -const { execSync } = require('child_process'); -const fs = require('fs'); -const path = require('path'); - -describe('Camera Pose Script', () => { - const projectRoot = path.resolve(__dirname, '..'); - const scriptPath = path.resolve(projectRoot, 'programs/03a_cameraPose.py'); - const timestamp = 1778819665744; - const screenshotDir = path.join(__dirname, 'data', 'screenShots'); - const detectionFiles = [ - 'snapshot_video0_1778819665744_aruco_detection.json', - 'snapshot_video1_1778819665744_aruco_detection.json', - ]; - const robotFile = path.resolve(projectRoot, 'test/data/robot/robot.json'); - const sceneFile = path.join(screenshotDir, `scene_${timestamp}.json`); - const outputFile = path.join(screenshotDir, `scene_${timestamp}_cameras.json`); - - beforeEach(() => { - detectionFiles.forEach((file) => { - const src = path.join(screenshotDir, file); - if (!fs.existsSync(src)) { - throw new Error(`Missing test fixture detection file: ${src}`); - } - }); - - if (!fs.existsSync(sceneFile)) { - throw new Error(`Missing scene file generated by step 2: ${sceneFile}`); - } - - if (fs.existsSync(outputFile)) { - fs.unlinkSync(outputFile); - } - }); - - afterEach(() => { - // Keep the generated scene and camera JSON in the screenshot directory. - }); - - test('should exist and be executable', () => { - expect(fs.existsSync(scriptPath)).toBe(true); - }); - - test('should have scene file', () => { - expect(fs.existsSync(sceneFile)).toBe(true); - }); - - test('should have robot file', () => { - expect(fs.existsSync(robotFile)).toBe(true); - }); - - test('should compute camera poses with timestamp parameter', () => { - // Führe das Python-Skript mit den korrekten Parametern aus - const cmd = `python "${scriptPath}" -scene "${sceneFile}" -robot "${robotFile}" -out "${outputFile}"`; - - try { - execSync(cmd, { stdio: 'inherit', cwd: projectRoot }); - } catch (error) { - throw new Error(`Failed to compute camera poses: ${error.message}`); - } - // Überprüfe, ob die erwartete Ausgabedatei erstellt wurde - expect(fs.existsSync(outputFile)).toBe(true); - - // Prüfe den Inhalt der JSON-Datei - const jsonData = JSON.parse(fs.readFileSync(outputFile, 'utf8')); - expect(jsonData).toBeDefined(); - expect(typeof jsonData).toBe('object'); - expect(jsonData).toHaveProperty('camera_poses'); - }); - - test('should handle timestamp parameter correctly', () => { - expect(timestamp).toBe(1778819665744); - }); -}); \ No newline at end of file diff --git a/test/0_path_OK.test.js b/test/0_path_OK.test.js new file mode 100644 index 0000000..e5724d0 --- /dev/null +++ b/test/0_path_OK.test.js @@ -0,0 +1,99 @@ +const fs = require('fs'); +const path = require('path'); + + +const PROJECT_PATH = process.cwd() +const TEST_PATH = path.join(__dirname, '.'); +const SCRIPT_FILE = path.join(PROJECT_PATH, 'programs', '1_detect_aruco_observations.py'); +const SOURCE_DIR = path.join(__dirname,'data', 'screenShots'); +const TARGET_DIR = path.join(__dirname,'data', 'screenshots', '1778819665744_detection_testResults'); +const PYTHON_CMD = process.platform === 'win32' ? 'python' : 'python3'; +const TEST_SETUP_FILE = path.join(TEST_PATH, 'data', '0_testSetup.json'); +const ROBOT_PATH = path.join(__dirname, 'data', 'robot', 'robot.json'); +const SCRIPT_FILE_2 = path.join(PROJECT_PATH, 'programs', '2_estimate_camera_pose_from_aruco_json.py'); + + +const cam = { + id : 'cam1', + image: 'snapshot_video1_1779690911822.jpg', + intrinsics: path.join(PROJECT_PATH, 'data', 'settings','callibration_cam0.npz') +}; + + +// Passe diese Werte an deine echten Daten an +const pathsToCheck = { + SOURCE_DIR, + SCRIPT_FILE, + ROBOT_PATH, + SCRIPT_FILE_2, + NPZ_PATH: cam.intrinsics, + IMAGE_PATH: path.join(SOURCE_DIR, cam.image), + PYTHON_SCRIPT: path.join(PROJECT_PATH, 'programs', '1_detect_aruco_observations.py'), + TEST_SETUP_FILE, +}; + + + +describe('Check if Paths exist', () => { + + test('Each relevant Python File etc.', () => { + const missing = []; + + for (const [name, filePath] of Object.entries(pathsToCheck)) { + if (!filePath || !fs.existsSync(filePath)) { + missing.push(`${name} -> ${filePath}`); + } + } + + if (missing.length > 0) { + throw new Error( + 'Folgende Pfade fehlen oder sind ungültig:\n' + + missing.join('\n') + ); + } + }); + + test('Test if files in Setup exist', () => { + + + console.log('TEST_PATH:', TEST_PATH); + console.log('PROJECT_PATH:', PROJECT_PATH); + console.log('SOURCE_DIR:', SOURCE_DIR); + + if (!fs.existsSync(TEST_SETUP_FILE)) { + throw new Error(`Test-Setup Datei fehlt: ${TEST_SETUP_FILE}`); + } + + const raw = fs.readFileSync(TEST_SETUP_FILE, 'utf8'); + const data = JSON.parse(raw); + + + const missing = []; + + data.forEach((testCase, i) => { + + const r = testCase.robot.map(i => i.replace("TEST_PATH", TEST_PATH).replace("PROJECT_PATH", PROJECT_PATH)); + const pathR = path.join(r.map(p => p.toString()).join(path.sep)); + if (!pathR || !fs.existsSync(pathR)) { + missing.push(`${name} -> ${filePath}`); + } + + testCase.image.forEach((img, j) => { + const imgPath = path.join(img.file.map(i => i.replace("TEST_PATH", TEST_PATH).replace("PROJECT_PATH", PROJECT_PATH)).join(path.sep)); + if (!imgPath || !fs.existsSync(imgPath)) { + missing.push(`TestCase ${i} Image ${j} -> ${imgPath}`); + } + + }); + + }); + + console.log('Missing files in test setup:', missing); + if (missing.length > 0) { + throw new Error( + 'Folgende Dateien aus dem Test-Setup fehlen oder sind ungültig:\n' + + missing.join('\n') + ); + } + }); +}); diff --git a/test/1_detect_aruco_observations.test.js b/test/1_detect_aruco_observations.test.js index e69de29..537ea65 100644 --- a/test/1_detect_aruco_observations.test.js +++ b/test/1_detect_aruco_observations.test.js @@ -0,0 +1,70 @@ +const fs = require('fs'); +const path = require('path'); +const { execFileSync } = require('child_process'); + +const PROJECT_PATH = process.cwd() +const TEST_PATH = path.join(__dirname, '.'); +const SOURCE_DIR = path.join(__dirname,'data', 'screenShots'); +const TARGET_DIR = path.join(__dirname,'data', 'screenshots', '1778819665744_detection_singleTest_testResults'); +const PYTHON_CMD = process.platform === 'win32' ? 'python' : 'python3'; +const TEST_SETUP_FILE = path.join(TEST_PATH, 'data', '0_testSetup.json'); +const SCRIPT_FILE = path.join(PROJECT_PATH, 'programs', '1_detect_aruco_observations.py'); +const ROBOT_PATH = path.join(__dirname, 'data', 'robot', 'robot.json'); + + +const cam = { + id : 'cam1', + image: 'snapshot_video1_1779690911822.jpg', + intrinsics: path.join(PROJECT_PATH, 'data', 'settings','callibration_cam0.npz') +}; + + + + + + +describe('Check if Python runs', () => { + + test('First Run of Python', () => { + + + console.log('Intrinsics : ', cam.intrinsics); + execFileSync(PYTHON_CMD, [ + SCRIPT_FILE, + '-i', path.join(SOURCE_DIR, cam.image), + '-npz', cam.intrinsics, + '-robot', ROBOT_PATH, + '-cameraId', cam.id + + , + '-outDir', TARGET_DIR + ], { + stdio: 'inherit', + cwd: SOURCE_DIR // <- wichtig + }); + + + const resultFile = path.join(TARGET_DIR, 'snapshot_video1_1779690911822_aruco_detection.json'); + + if (!fs.existsSync(resultFile)) { + throw new Error(`Erwartete Datei fehlt: ${resultFile}`); + } + + + }); + + /* + // ✅ Cleanup läuft IMMER nach jedem Test + afterEach(() => { + if (!fs.existsSync(TARGET_DIR)) return; + + fs.readdirSync(TARGET_DIR).forEach(file => { + fs.rmSync(path.join(TARGET_DIR, file), { + recursive: true, + force: true + }); + }); + }); + */ + +}); \ No newline at end of file diff --git a/test/2_estimate_camera_pose_from_aruco_json.test.js b/test/2_estimate_camera_pose_from_aruco_json.test.js new file mode 100644 index 0000000..d183271 --- /dev/null +++ b/test/2_estimate_camera_pose_from_aruco_json.test.js @@ -0,0 +1,78 @@ +const fs = require('fs'); +const path = require('path'); +const { execFileSync } = require('child_process'); + +const PROJECT_PATH = process.cwd() +const TEST_PATH = path.join(__dirname, '.'); +const SOURCE_DIR = path.join(__dirname,'data', 'screenShots'); +const TARGET_DIR = path.join(__dirname,'data', 'screenshots', '1778819665744_detection_singleTest_testResults'); +const PYTHON_CMD = process.platform === 'win32' ? 'python' : 'python3'; +const TEST_SETUP_FILE = path.join(TEST_PATH, 'data', '0_testSetup.json'); +const SCRIPT_FILE = path.join(PROJECT_PATH, 'programs', '1_detect_aruco_observations.py'); +const ROBOT_PATH = path.join(__dirname, 'data', 'robot', 'robot.json'); +const SCRIPT_FILE_2 = path.join(PROJECT_PATH, 'programs', '2_estimate_camera_pose_from_aruco_json.py'); + + +const cam = { + id : 'cam1', + image: 'snapshot_video1_1779690911822.jpg', + intrinsics: path.join(PROJECT_PATH, 'data', 'settings','callibration_cam0.npz') +}; + + + +describe('Check if Python 2 runs', () => { + + test('First Run of Python second script', () => { + + + console.log('Intrinsics : ', cam.intrinsics); + execFileSync(PYTHON_CMD, [ + SCRIPT_FILE, + '-i', path.join(SOURCE_DIR, cam.image), + '-npz', cam.intrinsics, + '-robot', ROBOT_PATH, + '-cameraId', cam.id + + , + '-outDir', TARGET_DIR + ], { + stdio: 'inherit', + cwd: SOURCE_DIR // <- wichtig + }); + + + const resultFile = path.join(TARGET_DIR, 'snapshot_video1_1779690911822_aruco_detection.json'); + + if (!fs.existsSync(resultFile)) { + throw new Error(`Erwartete Datei fehlt: ${resultFile}`); + } + + + console.log('Intrinsics : ', cam.intrinsics); + execFileSync(PYTHON_CMD, [ + SCRIPT_FILE_2, + '--detections' , resultFile, + '--robots', ROBOT_PATH + ], { + stdio: 'inherit', + cwd: SOURCE_DIR // <- wichtig + }); + + }); + + /* + // ✅ Cleanup läuft IMMER nach jedem Test + afterEach(() => { + if (!fs.existsSync(TARGET_DIR)) return; + + fs.readdirSync(TARGET_DIR).forEach(file => { + fs.rmSync(path.join(TARGET_DIR, file), { + recursive: true, + force: true + }); + }); + }); + */ + +}); \ No newline at end of file diff --git a/test/data/0_testSetup.json b/test/data/0_testSetup.json new file mode 100644 index 0000000..1939bcb --- /dev/null +++ b/test/data/0_testSetup.json @@ -0,0 +1,45 @@ +[ + { + "name":"Test 0", + "robot":["TEST_PATH","data","robot","robot.json"], + "image":[ + { + "timestamp":11778819665744, + "file":["TEST_PATH","data","screenShots","snapshot_video0_1778819665744.jpg"], + "camera":"c310", + "callibration":["PROJECT_PATH", "data", "settings","callibration_cam0.npz"] + }, + { + "timestamp":11778819665744, + "file":["TEST_PATH","data","screenShots","snapshot_video1_1778819665744.jpg"], + "camera":"c310", + "callibration":["PROJECT_PATH", "data", "settings","callibration_cam0.npz"] + } + ] + }, + { + "name":"Test 1: 45° unterarm", + "robot":["TEST_PATH","data","robot","robot.json"], + "timestamp":1779690911822, + "image":[ + { + "timestamp":1779690911822, + "file":["TEST_PATH","data","screenShots","snapshot_video0_1779690911822.jpg"], + "camera":"c310", + + "callibration":["PROJECT_PATH", "data", "settings","callibration_cam0.npz"] + } , + { + "timestamp":1779690911822, + "file":["TEST_PATH","data","screenShots","snapshot_video1_1779690911822.jpg"], + "camera":"c310", + "callibration":["PROJECT_PATH", "data", "settings","callibration_cam0.npz"] }, + { + "timestamp":1779690911822, + "file":["TEST_PATH","data","screenShots","1779690911822_DSCF1382.JPG"], + "camera":"XT1", + "callibration":["PROJECT_PATH", "data", "XT1-16mm28-1mFokus_f8","callibration_XT1-16mm_1m_f8.npz"] + } + ] + } +] \ No newline at end of file diff --git a/test/data/robot/robot.json b/test/data/robot/robot.json index eb7739a..545364f 100644 --- a/test/data/robot/robot.json +++ b/test/data/robot/robot.json @@ -1,4 +1,8 @@ { + "vision_config":{ + "MarkerType":"DICT_4X4_250", + "MarkerSize":0.025 + }, "recognized":{"x":null, "y":null, "z": null, "a":null, "b":null, "c":null, "e": null}, "Elements":["Board","Base","Arm1","Joint1","Arm2","Finger1","Finger2"], "ElementLength":{"Arm1":250, "Arm2":250, "Finger1":100, "Finger2":100}, @@ -8,7 +12,6 @@ "jointC":{"name":"EllbowLift","type":"revolute","axis":[1,0,0],"parent":"Arm1","child":"Joint1", "origin":[null, null, null]}, "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2", "origin":[null, null, null]} }, - "MarkerType":"DICT_4X4_250", "Marker":[ {"id":205,"on":"Board","position":[0.80, -0.090, 0.0]}, {"id":207,"on":"Board","position":[0.80, 0.0, 0.0]},