ChK/appRobotVideoControls
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2 Camera Detection

Browse Source 
Checke wo sich im Bild die Kamera befindet
This commit is contained in:
chk
2026-05-25 16:50:48 +02:00
parent 93ac8a6301
commit f37097ea96
15 changed files with 1285 additions and 727 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
.vscode/launch.json vendored Normal file
View File

@@ -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"
]
}
]
}

272
programs/01_read_ArUco_jpg_to_json.py
View File

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

216
programs/02_build_scene_json.py
View File

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

587
programs/1_detect_aruco_observations.py
View File

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

383
programs/2_estimate_camera_pose_from_aruco_json.py Normal file
View File

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

BIN
programs/__pycache__/1_detect_aruco_observations.cpython-311.pyc Normal file
View File

Binary file not shown.

45
test/00_testSetup.json
View File

@@ -1,45 +0,0 @@
[
{
"name":"Test 0",
"robot":"test/data/robot/robot.json",
"image":[
{
"timestamp":11778819665744,
"file":"test/data/screenShots/snapshot_video0_11778819665744.jpg",
"camera":"c310",
"callibration":"data/settings/callibration_cam0.npz"
},
{
"timestamp":11778819665744,
"file":"test/data/screenShots/snapshot_video1_11778819665744.jpg",
"camera":"c310",
"callibration":"data/settings/callibration_cam0.npz"
}
]
},
{
"name":"Test 1: 45° unterarm",
"robot":"test/data/robot/robot.json",
"timestamp":1779690911822,
"image":[
{
"timestamp":1779690911822,
"file":"test/data/screenShots/snapshot_video0_1779690911822.jpg",
"camera":"c310",
"callibration":"data/settings/callibration_cam0.npz"
},
{
"timestamp":1779690911822,
"file":"test/data/screenShots/snapshot_video0_1779690911822.jpg",
"camera":"c310",
"callibration":"data/settings/callibration_cam0.npz"
},
{
"timestamp":1779690911822,
"file":"test/data/screenShots/1779690911822_DSCF1382.JPG",
"camera":"XT1",
"callibration":"data/callibration/XT1-16mm28-1mFokus_f8/callibration_XT1-16mm_1m_f8.npz"
}
]
}
]

64
test/01_read_ArUco.test.js
View File

@@ -1,64 +0,0 @@
// test/aruco.test.js
const { execSync } = require('child_process');
const fs = require('fs');
const path = require('path');
describe('Aruco Detection Script', () => {
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}`);
}
});
});
});

55
test/02_build_scene.test.js
View File

@@ -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);
});
});

74
test/03a_cameraPose.test.js
View File

@@ -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);
});
});

99
test/0_path_OK.test.js Normal file
View File

@@ -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')
);
}
});
});

70
test/1_detect_aruco_observations.test.js
View File

@@ -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
});
});
});
*/
});

78
test/2_estimate_camera_pose_from_aruco_json.test.js Normal file
View File

@@ -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
});
});
});
*/
});

45
test/data/0_testSetup.json Normal file
View File

@@ -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"]
}
]
}
]

5
test/data/robot/robot.json
View File

@@ -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}, "recognized":{"x":null, "y":null, "z": null, "a":null, "b":null, "c":null, "e": null},
"Elements":["Board","Base","Arm1","Joint1","Arm2","Finger1","Finger2"], "Elements":["Board","Base","Arm1","Joint1","Arm2","Finger1","Finger2"],
"ElementLength":{"Arm1":250, "Arm2":250, "Finger1":100, "Finger2":100}, "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]}, "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]} "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2", "origin":[null, null, null]}
}, },
"MarkerType":"DICT_4X4_250",
"Marker":[ "Marker":[
{"id":205,"on":"Board","position":[0.80, -0.090, 0.0]}, {"id":205,"on":"Board","position":[0.80, -0.090, 0.0]},
{"id":207,"on":"Board","position":[0.80, 0.0, 0.0]}, {"id":207,"on":"Board","position":[0.80, 0.0, 0.0]},