From 67257d975418077b1d59a0c54f7a26a635960f25 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Wed, 10 Jun 2026 13:58:21 +0200 Subject: [PATCH] Callibration Board 0 --- public/calibration.html | 99 +- scripts/1_detect_aruco_observations.py | 636 +++++++ .../2_estimate_camera_from_observations.py | 834 +++++++++ scripts/3_multiview_bundle_adjustment_v4.py | 1499 +++++++++++++++++ ...bundle_adjustment_v5_board_anchor_patch.py | 271 +++ scripts/3b_corner_marker_poses.py | 189 +++ scripts/A0_60Arucos_25mm_Seet223.json | 83 + scripts/A0_60Arucos_25mm_Seet223.pdf | 74 + scripts/robot_1781069752019.json | 433 +++++ server/server.js | 214 ++- 10 files changed, 4286 insertions(+), 46 deletions(-) create mode 100644 scripts/1_detect_aruco_observations.py create mode 100644 scripts/2_estimate_camera_from_observations.py create mode 100644 scripts/3_multiview_bundle_adjustment_v4.py create mode 100644 scripts/3_multiview_bundle_adjustment_v5_board_anchor_patch.py create mode 100644 scripts/3b_corner_marker_poses.py create mode 100644 scripts/A0_60Arucos_25mm_Seet223.json create mode 100644 scripts/A0_60Arucos_25mm_Seet223.pdf create mode 100644 scripts/robot_1781069752019.json diff --git a/public/calibration.html b/public/calibration.html index e79abb4..66f20cb 100644 --- a/public/calibration.html +++ b/public/calibration.html @@ -253,19 +253,22 @@
-

Board offen

-
- Ziel: Extrinsische Position des Marker-Boards im Kamera-Koordinatensystem bestimmen - (Rotations- und Translationsvektor via solvePnP).

- Geplante Aktionen: Kamerabild mit erkannten Markern anzeigen · Pose berechnen · - Kalibrierungsdatei speichern.

- Aktionen werden ergänzt sobald das Konzept feststeht. +

Board – ArUco & Kamera-Pose

+
+ Ablauf + + Foto aufnehmen → ArUco erkennen → Kamera-Pose schätzen + + Schritte + + 1_detect_aruco_observations  →  2_estimate_camera_from_observations + + Letzter Run + –
-
- - - - +
+ +
@@ -543,6 +546,78 @@ btn.disabled = false; } }); + + // ── Board ────────────────────────────────────────────────────────────────── + + const logBoard = document.getElementById('log-board'); + + function logB(msg) { + const ts = new Date().toLocaleTimeString('de-CH'); + logBoard.value += `[${ts}] ${msg}\n`; + logBoard.scrollTop = logBoard.scrollHeight; + } + + // SSE-Stream lesen (gleiche Logik wie compute) + async function readSseStream(response, logFn, onDone) { + const reader = response.body.getReader(); + const decoder = new TextDecoder(); + let buffer = ''; + while (true) { + const { done, value } = await reader.read(); + if (done) break; + buffer += decoder.decode(value, { stream: true }); + const parts = buffer.split('\n\n'); + buffer = parts.pop(); + for (const part of parts) { + for (const line of part.split('\n')) { + if (!line.startsWith('data: ')) continue; + try { + const evt = JSON.parse(line.slice(6)); + if (evt.type === 'log') { + if (evt.text !== '') logFn(evt.text); + } else if (evt.type === 'done') { + onDone(evt); + } + } catch { /* ignore */ } + } + } + } + } + + document.getElementById('btn-board-run').addEventListener('click', async () => { + logB('Board-Erkennung wird gestartet …'); + const btn = document.getElementById('btn-board-run'); + btn.disabled = true; + + try { + const response = await fetch('/api/board/run', { method: 'POST' }); + + if (!response.ok) { + const raw = await response.text().catch(() => ''); + let msg; + try { msg = JSON.parse(raw).error || raw; } + catch { msg = raw.slice(0, 300) || `HTTP ${response.status}`; } + logB(`❌ HTTP ${response.status}: ${msg}`); + return; + } + + await readSseStream(response, logB, (evt) => { + if (evt.exitCode === 0) { + logB('✅ Board-Run abgeschlossen.'); + if (evt.runDir) { + document.getElementById('board-last-run').textContent = evt.runDir; + } + } else { + logB(`❌ Beendet mit Exit-Code ${evt.exitCode}`); + } + }); + + } catch (err) { + logB(`❌ Fehler: ${err}`); + } finally { + btn.disabled = false; + } + }); diff --git a/scripts/1_detect_aruco_observations.py b/scripts/1_detect_aruco_observations.py new file mode 100644 index 0000000..6749c83 --- /dev/null +++ b/scripts/1_detect_aruco_observations.py @@ -0,0 +1,636 @@ +#!/usr/bin/env python3 + +import argparse +import json +import os +import hashlib +import time +import uuid +from typing import Dict, Any + +import cv2 +import numpy as np + + +# ------------------------------------------------------------ +# Utilities +# ------------------------------------------------------------ + + +def resolve_path(path): + path = os.path.expanduser(path) + + # Absoluter Pfad → direkt verwenden + if os.path.isabs(path): + return path + + # Relativer Pfad → absolut machen (auf Basis aktuellem cwd) + return os.path.abspath(path) + +def load_intrinsics_npz(npz_path: str): + data = np.load(npz_path) + + for k in ('camera_matrix', 'mtx', 'K'): + if k in data: + K = data[k].astype(np.float32) + break + else: + raise KeyError('Camera matrix not found in npz') + + for k in ('dist_coeffs', 'dist', 'D'): + if k in data: + D = data[k].astype(np.float32).reshape(-1, 1) + break + else: + D = np.zeros((5, 1), dtype=np.float32) + + return K, D + + +# ------------------------------------------------------------ + +def load_robot_vision_config(robot_json_path: str): + + + + robot_json_path = resolve_path(robot_json_path) + with open(robot_json_path, 'r', encoding='utf-8') as f: + robot = json.load(f) + + vision_config = robot.get('vision_config', {}) + + marker_type = vision_config.get('MarkerType', 'DICT_4X4_250') + marker_size = float(vision_config.get('MarkerSize', 0.025)) + + return { + 'MarkerType': marker_type, + 'MarkerSize': marker_size + } + + +# ------------------------------------------------------------ + +def get_aruco_detector(dict_name: str): + + mapping = { + 'DICT_4X4_250': cv2.aruco.DICT_4X4_250, + 'DICT_5X5_100': cv2.aruco.DICT_5X5_100, + 'DICT_6X6_250': cv2.aruco.DICT_6X6_250, + 'DICT_ARUCO_ORIGINAL': cv2.aruco.DICT_ARUCO_ORIGINAL, + } + + dict_id = mapping.get(dict_name, cv2.aruco.DICT_4X4_250) + + dictionary = cv2.aruco.getPredefinedDictionary(dict_id) + + try: + params = cv2.aruco.DetectorParameters() + except Exception: + params = cv2.aruco.DetectorParameters_create() + + try: + detector = cv2.aruco.ArucoDetector(dictionary, params) + return detector, None + + except Exception: + return None, (dictionary, params) + + +# ------------------------------------------------------------ + +def detect_markers(image, detector_tuple): + + detector, fallback = detector_tuple + + if detector is not None: + + corners, ids, rejected = detector.detectMarkers(image) + + else: + + dictionary, params = fallback + + corners, ids, rejected = cv2.aruco.detectMarkers( + image, + dictionary, + parameters=params + ) + + return corners, ids, rejected + + +# ------------------------------------------------------------ + +def hash_file(path): + + sha = hashlib.sha256() + + with open(path, 'rb') as f: + + while True: + + chunk = f.read(1024 * 1024) + + if not chunk: + break + + sha.update(chunk) + + return sha.hexdigest() + + +# ------------------------------------------------------------ + +def polygon_mask(shape, polygon): + + mask = np.zeros(shape, dtype=np.uint8) + + cv2.fillConvexPoly( + mask, + polygon.astype(np.int32), + 255 + ) + + return mask + + +# ------------------------------------------------------------ + +def shrink_polygon(points, scale=0.80): + + center = np.mean(points, axis=0) + + shrunk = center + (points - center) * scale + + return shrunk.astype(np.float32) + + +# ------------------------------------------------------------ + +def compute_sharpness(gray_image, polygon): + + shrunk = shrink_polygon(polygon, scale=0.80) + + mask = polygon_mask(gray_image.shape, shrunk) + + pixels = gray_image[mask == 255] + + if pixels.size == 0: + return 0.0 + + temp = np.zeros_like(gray_image) + temp[mask == 255] = gray_image[mask == 255] + + lap = cv2.Laplacian(temp, cv2.CV_64F) + + values = lap[mask == 255] + + if values.size == 0: + return 0.0 + + return float(values.var()) + + +# ------------------------------------------------------------ + +def compute_contrast(gray_image, polygon): + + shrunk = shrink_polygon(polygon, scale=0.80) + + mask = polygon_mask(gray_image.shape, shrunk) + + pixels = gray_image[mask == 255] + + if pixels.size == 0: + + return { + 'p05': 0.0, + 'p95': 0.0, + 'dynamic_range': 0.0, + 'mean_gray': 0.0, + 'std_gray': 0.0 + } + + p05 = float(np.percentile(pixels, 5)) + p95 = float(np.percentile(pixels, 95)) + + return { + 'p05': p05, + 'p95': p95, + 'dynamic_range': float(p95 - p05), + 'mean_gray': float(np.mean(pixels)), + 'std_gray': float(np.std(pixels)) + } + + +# ------------------------------------------------------------ + +def compute_edge_ratio(corners): + + edge_lengths = [] + + for k in range(4): + + p1 = corners[k] + p2 = corners[(k + 1) % 4] + + edge_lengths.append( + float(np.linalg.norm(p1 - p2)) + ) + + edge_ratio = ( + max(edge_lengths) / + max(1e-6, min(edge_lengths)) + ) + + return edge_ratio, edge_lengths + + +# ------------------------------------------------------------ + +def compute_geometry_metrics(center, corners, width, height): + + image_center = np.array( + [width / 2.0, height / 2.0], + dtype=np.float32 + ) + + dist_center = np.linalg.norm(center - image_center) + + max_dist = np.linalg.norm(image_center) + + distance_center_norm = float( + dist_center / max(1e-6, max_dist) + ) + + min_x = np.min(corners[:, 0]) + max_x = np.max(corners[:, 0]) + + min_y = np.min(corners[:, 1]) + max_y = np.max(corners[:, 1]) + + border_distance_px = float(min( + min_x, + min_y, + width - max_x, + height - max_y + )) + + return { + 'distance_to_center_norm': distance_center_norm, + 'distance_to_border_px': border_distance_px + } + + +# ------------------------------------------------------------ + +def compute_confidence( + area_px, + sharpness, + edge_ratio, + dynamic_range, + border_distance_px +): + + score = 1.0 + + # area + score *= min(1.0, area_px / 1500.0) + + # sharpness + score *= min(1.0, sharpness / 120.0) + + # edge distortion + score *= 1.0 / max(1.0, edge_ratio) + + # contrast + score *= min(1.0, dynamic_range / 80.0) + + # border distance + score *= min(1.0, max(0.0, border_distance_px) / 50.0) + + score = max(0.0, min(1.0, score)) + + return float(score) + + +# ------------------------------------------------------------ + +def main(): + + parser = argparse.ArgumentParser() + + parser.add_argument( + '-i', + '--image', + required=True + ) + + parser.add_argument( + '-npz', + '--intrinsics', + required=True + ) + + parser.add_argument( + '-robot', + '--robot', + required=True + ) + + parser.add_argument( + '-cameraId', + '--cameraId', + required=True, + type=str + ) + + parser.add_argument( + '-outDir', + '--outDir', + required=True + ) + + parser.add_argument( + '--saveDebugImage', + action='store_true', + help='Speichert ein Debug-JPG mit eingezeichneten Marker-Rahmen' + ) + + args = parser.parse_args() + + out_dir = resolve_path(args.outDir) + os.makedirs(out_dir, exist_ok=True) + + + # -------------------------------------------------------- + # Load robot vision config + # -------------------------------------------------------- + + vision_config = load_robot_vision_config(args.robot) + + marker_type = vision_config['MarkerType'] + marker_size = vision_config['MarkerSize'] + + # -------------------------------------------------------- + # Load image + # -------------------------------------------------------- + + + image_path = resolve_path(args.image) + image = cv2.imread(image_path) + + + if image is None: + raise RuntimeError(f'Cannot read image: {args.image}') + + gray = cv2.cvtColor( + image, + cv2.COLOR_BGR2GRAY + ) + + height, width = gray.shape[:2] + + # -------------------------------------------------------- + # Intrinsics + # -------------------------------------------------------- + + + intrinsics_path = resolve_path(args.intrinsics) + K, D = load_intrinsics_npz(intrinsics_path) + + # -------------------------------------------------------- + # Detection + # -------------------------------------------------------- + + detector_tuple = get_aruco_detector(marker_type) + + corners_list, ids, rejected = detect_markers( + gray, + detector_tuple + ) + + # ids_raw: original numpy array für drawDetectedMarkers + ids_raw = ids + + detections = [] + + # -------------------------------------------------------- + # Valid detections + # -------------------------------------------------------- + + if ids is not None: + + ids = ids.flatten().tolist() + + for i, marker_id in enumerate(ids): + + corners = corners_list[i].reshape((4, 2)).astype(np.float32) + + center = corners.mean(axis=0) + + area_px = float( + cv2.contourArea(corners) + ) + + perimeter_px = float( + cv2.arcLength(corners, True) + ) + + edge_ratio, edge_lengths = compute_edge_ratio(corners) + + sharpness = compute_sharpness( + gray, + corners + ) + + contrast = compute_contrast( + gray, + corners + ) + + geometry = compute_geometry_metrics( + center, + corners, + width, + height + ) + + confidence = compute_confidence( + area_px=area_px, + sharpness=sharpness, + edge_ratio=edge_ratio, + dynamic_range=contrast['dynamic_range'], + border_distance_px=geometry['distance_to_border_px'] + ) + + detection = { + + 'observation_id': str(uuid.uuid4()), + + 'type': 'aruco', + + 'marker_id': int(marker_id), + + 'marker_size_m': marker_size, + + 'image_points_px': corners.tolist(), + + 'center_px': center.tolist(), + + 'quality': { + + 'area_px': area_px, + + 'perimeter_px': perimeter_px, + + 'sharpness': { + 'laplacian_var': sharpness + }, + + 'contrast': contrast, + + 'geometry': geometry, + + 'edge_ratio': edge_ratio, + + 'edge_lengths_px': edge_lengths + }, + + 'confidence': confidence + } + + detections.append(detection) + + # -------------------------------------------------------- + # Rejected candidates + # -------------------------------------------------------- + + rejected_candidates = [] + + if rejected is not None: + + for candidate in rejected: + + pts = candidate.reshape((-1, 2)).astype(np.float32) + + center = pts.mean(axis=0) + + area_px = float( + cv2.contourArea(pts) + ) + + rejected_candidates.append({ + + 'image_points_px': pts.tolist(), + + 'center_px': center.tolist(), + + 'area_px': area_px + }) + + # -------------------------------------------------------- + # Final output + # -------------------------------------------------------- + + output = { + + 'schema_version': '1.0', + + 'created_utc': time.strftime( + '%Y-%m-%dT%H:%M:%SZ', + time.gmtime() + ), + + 'vision_config': { + 'MarkerType': marker_type, + 'MarkerSize': marker_size + }, + + 'camera': { + + 'camera_id': args.cameraId, + + 'intrinsics_file': os.path.abspath(args.intrinsics), + + 'camera_matrix': K.tolist(), + + 'distortion_coefficients': D.reshape(-1).tolist() + }, + + 'image': { + + 'image_file': os.path.abspath(args.image), + + 'image_sha256': hash_file(args.image), + + 'width_px': int(width), + + 'height_px': int(height) + }, + + 'aruco': { + + 'dictionary': marker_type, + + 'num_detected_markers': len(detections), + + 'num_rejected_candidates': len(rejected_candidates) + }, + + 'detections': detections, + + 'rejected_candidates': rejected_candidates + } + + # -------------------------------------------------------- + # Output path + # -------------------------------------------------------- + + input_filename = os.path.basename(args.image) + + input_base = os.path.splitext(input_filename)[0] + + out_json = os.path.join( + out_dir, + f'{input_base}_aruco_detection.json' + ) + + # -------------------------------------------------------- + # Save JSON + # -------------------------------------------------------- + + with open(out_json, 'w', encoding='utf-8') as f: + + json.dump( + output, + f, + indent=2 + ) + + print(f'Saved: {out_json}') + + # -------------------------------------------------------- + # Debug-Bild mit Marker-Rahmen + # -------------------------------------------------------- + + if args.saveDebugImage: + + debug_img = image.copy() + + if corners_list and ids_raw is not None: + cv2.aruco.drawDetectedMarkers(debug_img, corners_list, ids_raw) + + debug_path = os.path.join( + out_dir, + f'{input_base}_debug.jpg' + ) + + cv2.imwrite(debug_path, debug_img) + print(f'Saved debug: {debug_path}') + + +# ------------------------------------------------------------ + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/scripts/2_estimate_camera_from_observations.py b/scripts/2_estimate_camera_from_observations.py new file mode 100644 index 0000000..f6197c7 --- /dev/null +++ b/scripts/2_estimate_camera_from_observations.py @@ -0,0 +1,834 @@ +#!/usr/bin/env python3 +""" +2_estimate_camera_from_observations.py + +Estimate a single camera pose from ArUco observations stored in +*_aruco_detection.json, using marker world positions from robot.json. + +This follows the same mathematical idea as readTwoImages.py: +1) use detected marker observations, +2) get an initial pose from a rigid transform, +3) refine with Levenberg-Marquardt on normalized reprojection residuals. + +Difference to readTwoImages.py: +- No image processing here. +- Input is the observation JSON created by 1_detect_aruco_observations.py. +- Output is xxx_camera_pose.json. +- Unknown marker reconstruction is intentionally omitted. + +Assumptions: +- robot.json contains a marker list for the board/world frame. +- At minimum, marker positions are present for the reference markers. +- The detection JSON contains camera intrinsics and marker corners. + +Typical usage: + python3 2_estimate_camera_from_observations.py \ + -i frame_0001_aruco_detection.json \ + -robot robot.json \ + -outDir results/ + +Output: + frame_0001_camera_pose.json + +Notes on uncertainty: +- The script computes an approximate 6x6 covariance for the pose parameters + [rvec_x, rvec_y, rvec_z, t_x, t_y, t_z]. +- It also propagates that covariance to camera center uncertainty in world + coordinates and to approximate roll/pitch/yaw uncertainty. +""" + +from __future__ import annotations + +import argparse +import json +import os +import sys +import time +from typing import Any, Dict, List, Optional, Tuple + +import cv2 +import numpy as np + + +# --------------------------------------------------------------------- +# Path / JSON helpers +# --------------------------------------------------------------------- + +def resolve_path(path: str) -> str: + path = os.path.expanduser(path) + if os.path.isabs(path): + return path + return os.path.abspath(path) + + +def load_json(path: str) -> Dict[str, Any]: + with open(resolve_path(path), "r", encoding="utf-8") as f: + return json.load(f) + + +def save_json(path: str, data: Dict[str, Any]) -> None: + with open(resolve_path(path), "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + + +# --------------------------------------------------------------------- +# Intrinsics +# --------------------------------------------------------------------- + +def load_intrinsics_from_detection(detection: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: + """ + Primary source: the embedded camera intrinsics in the detection JSON. + """ + camera = detection.get("camera", {}) + K = camera.get("camera_matrix", None) + D = camera.get("distortion_coefficients", None) + + if K is None: + raise KeyError("camera_matrix missing in detection JSON.") + if D is None: + D = [0, 0, 0, 0, 0] + + K = np.array(K, dtype=np.float32).reshape(3, 3) + D = np.array(D, dtype=np.float32).reshape(-1, 1) + return K, D + + +# --------------------------------------------------------------------- +# Robot JSON parsing +# --------------------------------------------------------------------- + +def _rotation_matrix_from_any(rotation: Any) -> np.ndarray: + """ + Best-effort parser for marker rotation. + + Supported inputs: + - 3x3 matrix as nested list + - flat 9 list + - dict with keys: + * rotation_matrix / matrix + * rvec / rodriques / rodrigues + * euler_deg / rpy_deg / roll_pitch_yaw_deg + * euler_rad / rpy_rad / roll_pitch_yaw_rad + * quaternion / quat (best-effort, expects [x,y,z,w] unless specified) + - None => identity + + The pose estimator below only needs marker positions, but we keep + this parser for completeness and future extension. + """ + if rotation is None: + return np.eye(3, dtype=np.float32) + + # Direct matrix + if isinstance(rotation, (list, tuple, np.ndarray)): + arr = np.array(rotation, dtype=np.float32) + if arr.shape == (3, 3): + return arr + if arr.size == 9: + return arr.reshape(3, 3).astype(np.float32) + if arr.size == 3: + # Treat as Rodrigues vector + R, _ = cv2.Rodrigues(arr.reshape(3, 1)) + return R.astype(np.float32) + return np.eye(3, dtype=np.float32) + + if isinstance(rotation, dict): + for key in ("rotation_matrix", "matrix"): + if key in rotation: + return _rotation_matrix_from_any(rotation[key]) + + for key in ("rvec", "rodrigues", "rodriques"): + if key in rotation: + v = np.array(rotation[key], dtype=np.float32).reshape(3, 1) + R, _ = cv2.Rodrigues(v) + return R.astype(np.float32) + + def euler_to_R(roll: float, pitch: float, yaw: float, degrees: bool = True) -> np.ndarray: + if degrees: + roll = np.deg2rad(roll) + pitch = np.deg2rad(pitch) + yaw = np.deg2rad(yaw) + cr, sr = np.cos(roll), np.sin(roll) + cp, sp = np.cos(pitch), np.sin(pitch) + cy, sy = np.cos(yaw), np.sin(yaw) + + Rx = np.array([[1, 0, 0], + [0, cr, -sr], + [0, sr, cr]], dtype=np.float32) + Ry = np.array([[cp, 0, sp], + [0, 1, 0], + [-sp, 0, cp]], dtype=np.float32) + Rz = np.array([[cy, -sy, 0], + [sy, cy, 0], + [0, 0, 1]], dtype=np.float32) + # ZYX convention + return (Rz @ Ry @ Rx).astype(np.float32) + + for key in ("euler_deg", "rpy_deg", "roll_pitch_yaw_deg"): + if key in rotation: + vals = np.array(rotation[key], dtype=np.float32).reshape(-1) + if vals.size == 3: + return euler_to_R(float(vals[0]), float(vals[1]), float(vals[2]), degrees=True) + + for key in ("euler_rad", "rpy_rad", "roll_pitch_yaw_rad"): + if key in rotation: + vals = np.array(rotation[key], dtype=np.float32).reshape(-1) + if vals.size == 3: + return euler_to_R(float(vals[0]), float(vals[1]), float(vals[2]), degrees=False) + + for key in ("quaternion", "quat"): + if key in rotation: + q = np.array(rotation[key], dtype=np.float32).reshape(-1) + if q.size == 4: + # Best-effort: try [x,y,z,w] + x, y, z, w = [float(v) for v in q] + R = np.array([ + [1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w], + [2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w], + [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y] + ], dtype=np.float32) + return R + + return np.eye(3, dtype=np.float32) + + +def get_marker_rotation(marker: Dict[str, Any]) -> np.ndarray: + """ + Flexible rotation extraction. Falls back to identity if absent. + """ + for key in ("rotation", "rotation_matrix", "matrix", "pose_rotation", "orientation"): + if key in marker: + return _rotation_matrix_from_any(marker[key]) + + # Also allow flat pose-style fields + if "rvec" in marker or "rodrigues" in marker: + return _rotation_matrix_from_any({"rvec": marker.get("rvec", marker.get("rodrigues"))}) + if "euler_deg" in marker: + return _rotation_matrix_from_any({"euler_deg": marker["euler_deg"]}) + if "rpy_deg" in marker: + return _rotation_matrix_from_any({"rpy_deg": marker["rpy_deg"]}) + if "quaternion" in marker: + return _rotation_matrix_from_any({"quaternion": marker["quaternion"]}) + + return np.eye(3, dtype=np.float32) + + +def load_marker_lookup(robot_json_path: str) -> Dict[int, Dict[str, Any]]: + """ + Supports the new format: + robot_data["links"]["Board"]["markers"] + + Fallback: + robot_data["Marker"] + """ + robot_json_path = resolve_path(robot_json_path) + with open(robot_json_path, "r", encoding="utf-8") as f: + robot_data = json.load(f) + + length_units = str(robot_data.get("units", {}).get("length", "")).strip().lower() + length_scale = 1.0 + if length_units in ("mm", "millimeter", "millimeters"): + length_scale = 1.0 / 1000.0 + elif length_units in ("cm", "centimeter", "centimeters"): + length_scale = 1.0 / 100.0 + + marker_lookup: Dict[int, Dict[str, Any]] = {} + + links = robot_data.get("links", {}) + board = links.get("Board") + + markers = None + if board and "markers" in board: + markers = board["markers"] + + if not markers: + markers = robot_data.get("Marker", []) + + for marker in markers: + marker_id = int(marker.get("id", -1)) + if marker_id < 0: + continue + + if "position" not in marker: + continue + + pos = marker.get("position") + if pos is None: + continue + + if len(pos) != 3: + continue + + rotation = get_marker_rotation(marker) + + marker_lookup[marker_id] = { + "position": np.array(pos, dtype=np.float32) * np.float32(length_scale), + "rotation": rotation, + "on": marker.get("on", "unknown"), + } + + return marker_lookup + + +def load_robot_marker_size(robot_json_path: str) -> Optional[float]: + """ + Best-effort marker size reader from robot.json. + Returns meters if found, otherwise None. + """ + robot_json_path = resolve_path(robot_json_path) + with open(robot_json_path, "r", encoding="utf-8") as f: + robot_data = json.load(f) + + vision_config = robot_data.get("vision_config", {}) + size = vision_config.get("MarkerSize", None) + if size is None: + return None + try: + return float(size) + except Exception: + return None + + +# --------------------------------------------------------------------- +# Geometry / pose helpers +# --------------------------------------------------------------------- + +def marker_local_corners(marker_size_m: float) -> np.ndarray: + half = marker_size_m / 2.0 + # Same corner order as the readTwoImages.py example + return np.array([ + [-half, half, 0.0], + [ half, half, 0.0], + [ half, -half, 0.0], + [-half, -half, 0.0], + ], dtype=np.float32) + + +def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Find R, t such that B ≈ R A + t. + A, B: Nx3 + """ + assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3" + N = A.shape[0] + if N < 2: + raise ValueError("Need at least 2 points; 3+ recommended.") + + centroid_A = A.mean(axis=0) + centroid_B = B.mean(axis=0) + + AA = A - centroid_A + BB = B - centroid_B + + H = AA.T @ BB + U, S, Vt = np.linalg.svd(H) + R = Vt.T @ U.T + + if np.linalg.det(R) < 0: + Vt[-1, :] *= -1 + R = Vt.T @ U.T + + t = centroid_B - R @ centroid_A + return R.astype(np.float32), t.astype(np.float32) + + +def undistort_to_normalized(points_px: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray: + pts = points_px.reshape(-1, 1, 2).astype(np.float32) + und = cv2.undistortPoints(pts, K, D, P=None) + return und.reshape(-1, 2).astype(np.float32) + + +def rvec_to_R(rvec: np.ndarray) -> np.ndarray: + R, _ = cv2.Rodrigues(rvec.reshape(3, 1)) + return R.astype(np.float32) + + +def R_to_euler_zyx(R: np.ndarray) -> Tuple[float, float, float]: + """ + Return roll, pitch, yaw in degrees using ZYX convention. + """ + yaw = float(np.degrees(np.arctan2(R[1, 0], R[0, 0]))) + sp = np.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2) + pitch = float(np.degrees(np.arctan2(-R[2, 0], sp))) + roll = float(np.degrees(np.arctan2(R[2, 1], R[2, 2]))) + return roll, pitch, yaw + + +def theta_to_camera_pose(theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + """ + theta = [omega_x, omega_y, omega_z, t_x, t_y, t_z] + Returns: + R_wc, t_wc, camera_center_world + """ + omega = theta[0:3] + t_wc = theta[3:6].reshape(3, 1).astype(np.float32) + R_wc, _ = cv2.Rodrigues(omega.reshape(3, 1)) + R_wc = R_wc.astype(np.float32) + R_cw = R_wc.T + camera_center_world = (-R_cw @ t_wc).reshape(3) + return R_wc, t_wc.reshape(3), camera_center_world + + +def build_projection_matrix(K: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray: + return K @ np.hstack([R, t.reshape(3, 1)]) + + +# --------------------------------------------------------------------- +# LM on normalized residuals (same style as readTwoImages.py) +# --------------------------------------------------------------------- + +def pack_params(omega: np.ndarray, t: np.ndarray) -> np.ndarray: + return np.hstack([omega.reshape(3), t.reshape(3)]).astype(np.float64) + + +def unpack_params(theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + omega = theta[0:3] + t = theta[3:6] + return omega, t + + +def residuals_centers_normalized(theta: np.ndarray, + X_world: np.ndarray, + obs_norm: np.ndarray) -> np.ndarray: + """ + Residuals in normalized coordinates: + obs_norm - project(R X_world + t) + """ + omega, t = unpack_params(theta) + R_wc = cv2.Rodrigues(omega.reshape(3, 1))[0].astype(np.float64) + X_cam = (R_wc @ X_world.T + t.reshape(3, 1)).T + uv = X_cam[:, :2] / X_cam[:, 2:3] + r = (obs_norm - uv).reshape(-1) + return r + + +def numerical_jacobian(f, theta: np.ndarray, eps: float, *args) -> Tuple[np.ndarray, np.ndarray]: + r0 = f(theta, *args) + m = r0.size + n = theta.size + J = np.zeros((m, n), dtype=np.float64) + for k in range(n): + th = theta.copy() + th[k] += eps + rk = f(th, *args) + J[:, k] = (rk - r0) / eps + return J, r0 + + +def lm_solve(theta0: np.ndarray, + X_world: np.ndarray, + obs_norm: np.ndarray, + max_iter: int = 60, + eps_jac: float = 1e-6, + lambda_init: float = 1e-3) -> Tuple[np.ndarray, Dict[str, List[float]]]: + lam = lambda_init + theta = theta0.copy().astype(np.float64) + history = {"iters": [], "rms": [], "lambda": []} + + for it in range(max_iter): + J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac, X_world, obs_norm) + rms = float(np.sqrt(np.mean(r * r))) if r.size else 0.0 + history["iters"].append(it) + history["rms"].append(rms) + history["lambda"].append(lam) + + JTJ = J.T @ J + g = J.T @ r + H = JTJ + lam * np.eye(JTJ.shape[0], dtype=np.float64) + + try: + delta = -np.linalg.solve(H, g) + except np.linalg.LinAlgError: + delta, *_ = np.linalg.lstsq(H, -g, rcond=None) + + theta_trial = theta + delta + r_trial = residuals_centers_normalized(theta_trial, X_world, obs_norm) + rms_trial = float(np.sqrt(np.mean(r_trial * r_trial))) if r_trial.size else rms + + if rms_trial < rms: + theta = theta_trial + lam *= 0.5 + else: + lam *= 2.0 + + if np.linalg.norm(delta) < 1e-10: + break + if abs(rms - rms_trial) < 1e-12: + break + + return theta, history + + +def pose_covariance(theta: np.ndarray, + X_world: np.ndarray, + obs_norm: np.ndarray, + eps_jac: float = 1e-6) -> Tuple[np.ndarray, float, np.ndarray]: + """ + Returns: + cov_theta_6x6, sigma2, residual_vector + """ + J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac, X_world, obs_norm) + m = r.size + n = theta.size + dof = max(1, m - n) + sigma2 = float((r @ r) / dof) + + JTJ = J.T @ J + cov = sigma2 * np.linalg.pinv(JTJ) + return cov.astype(np.float64), sigma2, r + + +def propagate_covariance(theta: np.ndarray, + cov_theta: np.ndarray) -> Dict[str, Any]: + """ + Propagate pose covariance to camera center and Euler angle uncertainties. + """ + def camera_center_fn(th: np.ndarray) -> np.ndarray: + _, _, c = theta_to_camera_pose(th) + return c.astype(np.float64) + + def euler_fn(th: np.ndarray) -> np.ndarray: + R_wc, _, _ = theta_to_camera_pose(th) + return np.array(R_to_euler_zyx(R_wc), dtype=np.float64) # deg + + Jc, _ = numerical_jacobian(lambda th, *_: camera_center_fn(th), theta, 1e-6) + cov_center = Jc @ cov_theta @ Jc.T + + Je, _ = numerical_jacobian(lambda th, *_: euler_fn(th), theta, 1e-6) + cov_euler = Je @ cov_theta @ Je.T + + center_std_m = np.sqrt(np.maximum(0.0, np.diag(cov_center))) + euler_std_deg = np.sqrt(np.maximum(0.0, np.diag(cov_euler))) + + # Parameter std directly from covariance + param_std = np.sqrt(np.maximum(0.0, np.diag(cov_theta))) + rvec_std_deg = np.degrees(param_std[0:3]) + tvec_std_m = param_std[3:6] + + return { + "pose_covariance_6x6": cov_theta.tolist(), + "parameter_std": { + "rvec_std_deg": [float(x) for x in rvec_std_deg], + "tvec_std_m": [float(x) for x in tvec_std_m], + }, + "camera_center_std_m": [float(x) for x in center_std_m], + "camera_center_std_mm": [float(x * 1000.0) for x in center_std_m], + "orientation_std_deg": { + "roll": float(euler_std_deg[0]), + "pitch": float(euler_std_deg[1]), + "yaw": float(euler_std_deg[2]), + }, + } + + +# --------------------------------------------------------------------- +# Marker processing +# --------------------------------------------------------------------- + +def build_object_corners_from_world_position(position_m: np.ndarray, + marker_size_m: float) -> np.ndarray: + """ + Marker corners in world coordinates, assuming the marker frame is aligned + with the world frame and only translated to 'position_m'. + + This is the direct analogue of readTwoImages.py using marker center positions. + """ + h = marker_size_m / 2.0 + local = np.array([ + [-h, h, 0.0], + [ h, h, 0.0], + [ h, -h, 0.0], + [-h, -h, 0.0], + ], dtype=np.float32) + return local + position_m.reshape(1, 3) + + +def solve_single_marker_pose(corners_px: np.ndarray, + K: np.ndarray, + D: np.ndarray, + marker_size_m: float) -> Optional[Tuple[np.ndarray, np.ndarray]]: + obj = marker_local_corners(marker_size_m) + success, rvec, tvec = cv2.solvePnP( + obj, + corners_px.astype(np.float32), + K, + D, + flags=cv2.SOLVEPNP_IPPE_SQUARE + ) + if not success: + success, rvec, tvec = cv2.solvePnP( + obj, + corners_px.astype(np.float32), + K, + D, + flags=cv2.SOLVEPNP_ITERATIVE + ) + if not success: + return None + return rvec.reshape(3), tvec.reshape(3) + + +# --------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------- + +def main() -> None: + parser = argparse.ArgumentParser(description="Estimate camera pose from ArUco observation JSON") + parser.add_argument("-i", "--input", required=True, help="*_aruco_detection.json") + parser.add_argument("-robot", "--robot", required=True, help="robot.json with board markers") + parser.add_argument("-outDir", "--outDir", default=None, help="Optional output directory") + parser.add_argument("--minConfidence", type=float, default=0.0, + help="Skip detections below this confidence") + parser.add_argument("--minCommonMarkers", type=int, default=3, + help="Minimum number of world markers required") + parser.add_argument("--maxRmsPx", type=float, default=None, + help="Optional soft warning threshold for final reprojection RMS in pixels") + parser.add_argument("--epsJac", type=float, default=1e-6, help="Finite-difference epsilon") + args = parser.parse_args() + + detection_path = resolve_path(args.input) + robot_path = resolve_path(args.robot) + + detection = load_json(detection_path) + marker_lookup = load_marker_lookup(robot_path) + + K, D = load_intrinsics_from_detection(detection) + + robot_marker_size = load_robot_marker_size(robot_path) + det_marker_size = detection.get("vision_config", {}).get("MarkerSize", None) + if det_marker_size is not None: + marker_size_m = float(det_marker_size) + elif robot_marker_size is not None: + marker_size_m = float(robot_marker_size) + else: + marker_size_m = 0.025 + + detections = detection.get("detections", []) + if not isinstance(detections, list): + raise TypeError("detection['detections'] must be a list") + + used_ids: List[int] = [] + used_world_positions: List[np.ndarray] = [] + used_obs_centers_px: List[np.ndarray] = [] + used_obs_centers_norm: List[np.ndarray] = [] + used_marker_cam_centers: List[np.ndarray] = [] + used_marker_meta: List[Dict[str, Any]] = [] + + sanity_notes: List[str] = [] + + for det in detections: + if det.get("type", "aruco") != "aruco": + continue + + marker_id = int(det.get("marker_id", -1)) + if marker_id < 0: + continue + + if marker_id not in marker_lookup: + continue + + confidence = float(det.get("confidence", 1.0)) + if confidence < args.minConfidence: + continue + + corners = det.get("image_points_px", None) + if corners is None: + continue + + corners_px = np.array(corners, dtype=np.float32).reshape(4, 2) + center_from_corners = corners_px.mean(axis=0) + + center_px = np.array(det.get("center_px", center_from_corners), dtype=np.float32).reshape(2) + center_delta = float(np.linalg.norm(center_from_corners - center_px)) + if center_delta > 0.75: + sanity_notes.append( + f"marker {marker_id}: center_px differs from corner-mean by {center_delta:.2f}px" + ) + + pnp = solve_single_marker_pose(corners_px, K, D, marker_size_m) + if pnp is None: + continue + + rvec_m, tvec_m = pnp + world_pos = marker_lookup[marker_id]["position"].astype(np.float32) + + used_ids.append(marker_id) + used_world_positions.append(world_pos) + used_obs_centers_px.append(center_from_corners.astype(np.float32)) + used_obs_centers_norm.append(undistort_to_normalized(center_from_corners.reshape(1, 2), K, D)[0]) + used_marker_cam_centers.append(tvec_m.astype(np.float32)) + used_marker_meta.append({ + "marker_id": marker_id, + "confidence": confidence, + "center_px": [float(center_from_corners[0]), float(center_from_corners[1])], + "marker_size_m": marker_size_m, + }) + + # Unique / deduplicate by marker_id while preserving order + dedup: Dict[int, int] = {} + uniq_ids: List[int] = [] + uniq_world_positions: List[np.ndarray] = [] + uniq_obs_px: List[np.ndarray] = [] + uniq_obs_norm: List[np.ndarray] = [] + uniq_cam_centers: List[np.ndarray] = [] + uniq_meta: List[Dict[str, Any]] = [] + + for idx, mid in enumerate(used_ids): + if mid in dedup: + continue + dedup[mid] = idx + uniq_ids.append(mid) + uniq_world_positions.append(used_world_positions[idx]) + uniq_obs_px.append(used_obs_centers_px[idx]) + uniq_obs_norm.append(used_obs_centers_norm[idx]) + uniq_cam_centers.append(used_marker_cam_centers[idx]) + uniq_meta.append(used_marker_meta[idx]) + + if len(uniq_ids) < args.minCommonMarkers: + raise RuntimeError( + f"Need at least {args.minCommonMarkers} common markers; found {len(uniq_ids)}: {uniq_ids}" + ) + + X_world = np.stack(uniq_world_positions, axis=0).astype(np.float64) + obs_px = np.stack(uniq_obs_px, axis=0).astype(np.float64) + obs_norm = np.stack(uniq_obs_norm, axis=0).astype(np.float64) + marker_cam_centers = np.stack(uniq_cam_centers, axis=0).astype(np.float64) + + # Initial pose from rigid transform of per-marker camera-frame centers to world positions + # B ≈ R A + t -> world = R * camera + t + R_cw_init, t_cw_init = rigid_transform_no_scale(marker_cam_centers, X_world) + R_wc_init = R_cw_init.T + t_wc_init = -(R_wc_init @ t_cw_init).reshape(3) + + omega_init = cv2.Rodrigues(R_wc_init)[0].reshape(3) + theta0 = pack_params(omega_init, t_wc_init) + + theta_opt, hist = lm_solve( + theta0=theta0, + X_world=X_world, + obs_norm=obs_norm, + max_iter=60, + eps_jac=args.epsJac, + lambda_init=1e-3, + ) + + R_wc, t_wc, camera_center_world = theta_to_camera_pose(theta_opt) + + cov_theta, sigma2, residual_vec = pose_covariance( + theta_opt, X_world, obs_norm, eps_jac=args.epsJac + ) + propagated = propagate_covariance(theta_opt, cov_theta) + + # Exact pixel-space reprojection statistics + proj_pts, _ = cv2.projectPoints( + X_world.reshape(-1, 1, 3).astype(np.float32), + theta_opt[0:3].reshape(3, 1).astype(np.float32), + theta_opt[3:6].reshape(3, 1).astype(np.float32), + K, + D, + ) + proj_pts = proj_pts.reshape(-1, 2) + reproj_err_px = np.linalg.norm(proj_pts - obs_px, axis=1) + rms_px = float(np.sqrt(np.mean(reproj_err_px ** 2))) if reproj_err_px.size else 0.0 + median_px = float(np.median(reproj_err_px)) if reproj_err_px.size else 0.0 + max_px = float(np.max(reproj_err_px)) if reproj_err_px.size else 0.0 + + if args.maxRmsPx is not None and rms_px > args.maxRmsPx: + print(f"[WARN] Final reprojection RMS is {rms_px:.3f}px (threshold {args.maxRmsPx:.3f}px).") + + # Convert outputs + roll, pitch, yaw = R_to_euler_zyx(R_wc) + position_mm = (camera_center_world * 1000.0).astype(float).tolist() + + # Reproject each used marker center for QA + per_marker_results = [] + proj_pts_exact, _ = cv2.projectPoints( + X_world.reshape(-1, 1, 3).astype(np.float32), + theta_opt[0:3].reshape(3, 1).astype(np.float32), + theta_opt[3:6].reshape(3, 1).astype(np.float32), + K, + D, + ) + proj_pts_exact = proj_pts_exact.reshape(-1, 2) + + for idx, mid in enumerate(uniq_ids): + x = proj_pts_exact[idx] + err = float(np.linalg.norm(x - obs_px[idx])) + per_marker_results.append({ + "marker_id": int(mid), + "observed_center_px": [float(obs_px[idx, 0]), float(obs_px[idx, 1])], + "projected_center_px": [float(x[0]), float(x[1])], + "reprojection_error_px": err, + "confidence": float(uniq_meta[idx]["confidence"]), + }) + + # Output directory + in_base = os.path.splitext(os.path.basename(detection_path))[0] + out_name = in_base.replace("_aruco_detection", "_camera_pose") + ".json" + + if args.outDir is not None: + out_dir = resolve_path(args.outDir) + else: + out_dir = os.path.dirname(detection_path) or "." + + os.makedirs(out_dir, exist_ok=True) + out_json = os.path.join(out_dir, out_name) + + output = { + "schema_version": "1.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "source": { + "detection_json": detection_path, + "robot_json": robot_path, + }, + "camera": { + "camera_id": detection.get("camera", {}).get("camera_id", "unknown"), + "camera_matrix": K.tolist(), + "distortion_coefficients": D.reshape(-1).tolist(), + }, + "estimation": { + "method": "single_camera_marker_center_lm", + "description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.", + "marker_size_m": float(marker_size_m), + "num_used_markers": int(len(uniq_ids)), + "used_marker_ids": [int(x) for x in uniq_ids], + "history": hist, + "residual_rms_px": float(rms_px), + "residual_median_px": float(median_px), + "residual_max_px": float(max_px), + "sigma2_normalized": float(sigma2), + }, + "camera_pose": { + "world_to_camera": { + "rotation_matrix": R_wc.tolist(), + "translation_m": [float(x) for x in t_wc.tolist()], + "rvec_rad": [float(x) for x in theta_opt[0:3].tolist()], + }, + "camera_in_world": { + "position_m": [float(x) for x in camera_center_world.tolist()], + "position_mm": [float(x) for x in position_mm], + "orientation_deg": { + "roll": float(roll), + "pitch": float(pitch), + "yaw": float(yaw), + }, + }, + "uncertainty": propagated, + }, + "observations": { + "markers": per_marker_results, + }, + "qa": { + "sanity_notes": sanity_notes, + }, + } + + save_json(out_json, output) + print(f"[INFO] Saved camera pose JSON: {out_json}") + + +if __name__ == "__main__": + try: + main() + except Exception as exc: + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) diff --git a/scripts/3_multiview_bundle_adjustment_v4.py b/scripts/3_multiview_bundle_adjustment_v4.py new file mode 100644 index 0000000..0f629b5 --- /dev/null +++ b/scripts/3_multiview_bundle_adjustment_v4.py @@ -0,0 +1,1499 @@ +#!/usr/bin/env python3 +""" +3_multiview_bundle_adjustment_v4.py + +Multi-view ArUco marker position optimization with explicit, switchable +degrees-of-freedom constraints. + +Mathematical model +------------------ +We estimate 3D marker positions X_i ∈ R^3 by minimizing + + E(X) = + Σ_{i,c} w_ic || π_c(X_i) - u_ic ||² + + λ_r Σ_j w_j^r || ||X_a - X_b|| - d_j ||² + + λ_rev Σ_k w_k^rev || (X_b - X_a)·a_k - t_k ||² + + λ_pri Σ_m w_m^pri ( ||(X_b - X_a)·u_m - t_u||² + + ||(X_b - X_a)·v_m - t_v||² ) + +where: +- u_ic are observed normalized image coordinates for marker i in camera c +- π_c(.) is the normalized reprojection model +- w_ic are observation weights from detection quality / marker priors / range +- rigid-link constraints preserve internal marker geometry of a link +- revolute joints keep the projection along the joint axis constant +- prismatic joints keep the two orthogonal projection components constant + +Important design choices +------------------------ +- robot.json is used as a kinematic description, not as a direct source of + world-space marker positions. +- constraint families are explicit, switchable, and easy to compare across + versions. +- legacy chain-propagation constraints are retained only as an optional family + and are OFF by default. +- observation weighting remains separate from constraint weighting so both can + be tested independently. + +Dependencies: + numpy, opencv-python, scipy (optional for optimization) + +Example: + python 3_multiview_bundle_adjustment_v4.py ^ + -det cam1_aruco_detection.json cam2_aruco_detection.json cam3_aruco_detection.json ^ + -pose cam1_camera_pose.json cam2_camera_pose.json cam3_camera_pose.json ^ + -robot robot.json ^ + -lambdaWeight 100.0 +""" +from __future__ import annotations + +import argparse +import json +import os +import sys +import time +from dataclasses import dataclass +from itertools import combinations +from typing import Any, Dict, List, Optional, Tuple + +import cv2 +import numpy as np + + +# =================================================================== +# Path / JSON helpers +# =================================================================== + +def resolve_path(path: str) -> str: + path = os.path.expanduser(path) + if os.path.isabs(path): + return path + return os.path.abspath(path) + + +def load_json(path: str) -> Dict[str, Any]: + with open(resolve_path(path), "r", encoding="utf-8") as f: + return json.load(f) + + +def save_json(path: str, data: Dict[str, Any]) -> None: + with open(resolve_path(path), "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + + +# =================================================================== +# Units +# =================================================================== + +def get_length_scale(robot_data: Dict[str, Any]) -> float: + units = robot_data.get("units", {}) or {} + length_unit = str(units.get("length", "")).strip().lower() + if length_unit in ("mm", "millimeter", "millimeters"): + return 1.0 / 1000.0 + if length_unit in ("cm", "centimeter", "centimeters"): + return 1.0 / 100.0 + return 1.0 + + +# =================================================================== +# Small geometry helpers +# =================================================================== + +def safe_norm(v: np.ndarray, eps: float = 1e-12) -> float: + return float(np.linalg.norm(v) + eps) + + +def normalize_vector(v: np.ndarray, eps: float = 1e-12) -> np.ndarray: + return np.asarray(v, dtype=np.float64) / safe_norm(v, eps) + + +def clamp(v: float, lo: float, hi: float) -> float: + return float(max(lo, min(hi, v))) + + +def principal_axis_id(axis: np.ndarray, threshold: float = 0.95) -> Optional[int]: + """Return 0,1,2 for x,y,z if axis is close enough to a principal axis.""" + a = normalize_vector(np.asarray(axis, dtype=np.float64)) + idx = int(np.argmax(np.abs(a))) + if abs(a[idx]) >= threshold: + return idx + return None + + +def camera_center_from_world_to_cam(R_wc: np.ndarray, t_wc: np.ndarray) -> np.ndarray: + """world_to_camera: X_cam = R_wc * X_world + t_wc; camera center is -R^T t.""" + return -R_wc.T @ t_wc + + +def principal_axis_vector(axis: np.ndarray) -> np.ndarray: + """Convert a near-principal axis to an exact signed principal axis vector.""" + a = normalize_vector(axis) + idx = int(np.argmax(np.abs(a))) + out = np.zeros(3, dtype=np.float64) + out[idx] = 1.0 if a[idx] >= 0 else -1.0 + return normalize_vector(out) + + +def orthonormal_basis_from_axis(axis: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Build two unit vectors orthogonal to axis, with a deterministic orientation. + """ + a = normalize_vector(axis) + ref = np.array([1.0, 0.0, 0.0], dtype=np.float64) + if abs(float(np.dot(a, ref))) > 0.90: + ref = np.array([0.0, 1.0, 0.0], dtype=np.float64) + u = np.cross(a, ref) + if np.linalg.norm(u) < 1e-12: + ref = np.array([0.0, 0.0, 1.0], dtype=np.float64) + u = np.cross(a, ref) + u = normalize_vector(u) + v = normalize_vector(np.cross(a, u)) + return u, v + + +# =================================================================== +# Configuration +# =================================================================== + +@dataclass +class ConstraintRuleConfig: + rigid_distance_enabled: bool = True + rigid_distance_mode: str = "mst" # mst | star | full + rigid_distance_weight: float = 1.0 + + # Revolute joints: keep the projection along the axis constant. + revolute_axis_enabled: bool = True + revolute_axis_max_pairs: int = 2 + revolute_axis_weight: float = 0.5 + + # Prismatic joints: keep the two orthogonal projection components constant. + prismatic_orthogonal_enabled: bool = True + prismatic_orthogonal_max_pairs: int = 2 + prismatic_orthogonal_weight: float = 0.35 + + # Legacy / optional chain propagation, disabled by default. + chain_axis_enabled: bool = False + chain_axis_max_depth: int = 3 + chain_axis_max_pairs: int = 2 + chain_axis_weight: float = 0.3 + + axis_alignment_threshold: float = 0.95 + + strict_unique_marker_ids: bool = False + show_skipped_constraints: bool = True + + enable_observation_weights: bool = True + weight_floor: float = 0.30 + weight_ceiling: float = 3.00 + ref_distance_m: float = 0.75 + ref_marker_size_px: float = 50.0 + use_detection_confidence: bool = True + use_detection_size_px: bool = True + use_initial_range: bool = True + use_marker_size_prior: bool = True + + +def _bool_or_default(value: Any, default: bool) -> bool: + if value is None: + return default + return bool(value) + + +def _float_or_default(value: Any, default: float) -> float: + if value is None: + return default + return float(value) + + +def _int_or_default(value: Any, default: int) -> int: + if value is None: + return default + return int(value) + + +def load_constraint_rule_config(robot_data: Dict[str, Any], args: argparse.Namespace) -> ConstraintRuleConfig: + """ + Merge built-in defaults with optional robot.json constraint_rules and CLI flags. + Backward compatibility: + - joint_axis_projection -> revolute_axis + """ + rules = robot_data.get("constraint_rules", {}) or {} + + cfg = ConstraintRuleConfig() + rigid = rules.get("rigid_distance", {}) or {} + revolute = rules.get("joint_revolute_axis", {}) or rules.get("joint_axis_projection", {}) or {} + prismatic = rules.get("joint_prismatic_orthogonal", {}) or {} + chain = rules.get("chain_axis_projection", {}) or {} + obs = rules.get("observation_weights", {}) or {} + + cfg.rigid_distance_enabled = _bool_or_default(rigid.get("enabled"), cfg.rigid_distance_enabled) + cfg.rigid_distance_mode = str(rigid.get("mode", cfg.rigid_distance_mode)).strip().lower() + cfg.rigid_distance_weight = _float_or_default(rigid.get("weight"), cfg.rigid_distance_weight) + + cfg.revolute_axis_enabled = _bool_or_default(revolute.get("enabled"), cfg.revolute_axis_enabled) + cfg.revolute_axis_max_pairs = _int_or_default(revolute.get("max_pairs"), cfg.revolute_axis_max_pairs) + cfg.revolute_axis_weight = _float_or_default(revolute.get("weight"), cfg.revolute_axis_weight) + + cfg.prismatic_orthogonal_enabled = _bool_or_default(prismatic.get("enabled"), cfg.prismatic_orthogonal_enabled) + cfg.prismatic_orthogonal_max_pairs = _int_or_default(prismatic.get("max_pairs"), cfg.prismatic_orthogonal_max_pairs) + cfg.prismatic_orthogonal_weight = _float_or_default(prismatic.get("weight"), cfg.prismatic_orthogonal_weight) + + cfg.chain_axis_enabled = _bool_or_default(chain.get("enabled"), cfg.chain_axis_enabled) + cfg.chain_axis_max_depth = _int_or_default(chain.get("max_depth"), cfg.chain_axis_max_depth) + cfg.chain_axis_max_pairs = _int_or_default(chain.get("max_pairs"), cfg.chain_axis_max_pairs) + cfg.chain_axis_weight = _float_or_default(chain.get("weight"), cfg.chain_axis_weight) + + cfg.axis_alignment_threshold = _float_or_default( + rules.get("axis_alignment_threshold"), cfg.axis_alignment_threshold + ) + + cfg.enable_observation_weights = _bool_or_default(obs.get("enabled"), cfg.enable_observation_weights) + cfg.weight_floor = _float_or_default(obs.get("weight_floor"), cfg.weight_floor) + cfg.weight_ceiling = _float_or_default(obs.get("weight_ceiling"), cfg.weight_ceiling) + cfg.ref_distance_m = _float_or_default(obs.get("ref_distance_m"), cfg.ref_distance_m) + cfg.ref_marker_size_px = _float_or_default(obs.get("ref_marker_size_px"), cfg.ref_marker_size_px) + cfg.use_detection_confidence = _bool_or_default(obs.get("use_detection_confidence"), cfg.use_detection_confidence) + cfg.use_detection_size_px = _bool_or_default(obs.get("use_detection_size_px"), cfg.use_detection_size_px) + cfg.use_initial_range = _bool_or_default(obs.get("use_initial_range"), cfg.use_initial_range) + cfg.use_marker_size_prior = _bool_or_default(obs.get("use_marker_size_prior"), cfg.use_marker_size_prior) + + if getattr(args, "strictUniqueMarkerIds", False): + cfg.strict_unique_marker_ids = True + if getattr(args, "showSkippedConstraints", False): + cfg.show_skipped_constraints = True + if getattr(args, "noShowSkippedConstraints", False): + cfg.show_skipped_constraints = False + + return cfg + + +# =================================================================== +# Observation / constraint definitions +# =================================================================== + +@dataclass +class Observation: + cam_idx: int + norm_coords: np.ndarray + meta: Dict[str, Any] + + +@dataclass +class MarkerDistanceConstraint: + marker_id_a: int + marker_id_b: int + link_name: str + target_distance_m: float + weight: float = 1.0 + enabled: bool = True + source: str = "rigid_distance" + + +@dataclass +class JointAxisConstraint: + marker_id_parent: int + marker_id_child: int + parent_link: str + child_link: str + joint_axis: np.ndarray + target_delta_along_axis_m: float + weight: float = 1.0 + enabled: bool = True + source: str = "joint_axis_projection" + + +Constraint = MarkerDistanceConstraint | JointAxisConstraint + + +# =================================================================== +# Robot parsing +# =================================================================== + +def parse_robot_markers( + robot_data: Dict[str, Any], + length_scale: float, + strict_unique_marker_ids: bool = False +) -> Tuple[Dict[int, str], Dict[str, List[Dict[str, Any]]], List[str], Dict[int, Dict[str, Any]]]: + links = robot_data.get("links", {}) or {} + + marker_to_link: Dict[int, str] = {} + link_markers: Dict[str, List[Dict[str, Any]]] = {} + issues: List[str] = [] + marker_meta: Dict[int, Dict[str, Any]] = {} + + seen_global: Dict[int, str] = {} + + for link_name, link_data in links.items(): + markers = link_data.get("markers", []) or [] + collected: List[Dict[str, Any]] = [] + seen_local: set[int] = set() + + for idx, marker in enumerate(markers): + marker_id = int(marker.get("id", -1)) + pos = marker.get("position", None) + + if marker_id < 0 or pos is None or len(pos) != 3: + issues.append(f"[WARN] link={link_name}: skipped invalid marker entry at index {idx}") + continue + + if marker_id in seen_local: + msg = f"[WARN] duplicate marker id {marker_id} inside link '{link_name}'" + if strict_unique_marker_ids: + raise ValueError(msg) + issues.append(msg + " -> skipped duplicate inside same link") + continue + + if marker_id in seen_global: + msg = ( + f"[WARN] duplicate marker id {marker_id} appears in link '{link_name}' " + f"and already in link '{seen_global[marker_id]}'" + ) + if strict_unique_marker_ids: + raise ValueError(msg) + issues.append(msg + " -> skipped duplicate occurrence") + continue + + seen_local.add(marker_id) + seen_global[marker_id] = link_name + + pos_raw = np.array(pos, dtype=np.float64) + pos_m = pos_raw * float(length_scale) + + item = { + "id": marker_id, + "name": marker.get("name", f"marker_{marker_id}"), + "position_raw": pos_raw, + "position_m": pos_m, + "normal": np.array(marker.get("normal", [0, 0, 1]), dtype=np.float64), + "size": marker.get("size", None), + "spin": marker.get("spin", None), + } + collected.append(item) + marker_to_link[marker_id] = link_name + marker_meta[marker_id] = { + "link_name": link_name, + "name": item["name"], + "position_m": pos_m, + "normal": item["normal"], + "size": item["size"], + "spin": item["spin"], + } + + link_markers[link_name] = collected + + return marker_to_link, link_markers, issues, marker_meta + + +def get_link_parent_map(robot_data: Dict[str, Any]) -> Dict[str, Optional[str]]: + links = robot_data.get("links", {}) or {} + return {link_name: (link_data.get("parent", None)) for link_name, link_data in links.items()} + + +def get_joint_info(robot_data: Dict[str, Any], child_link: str) -> Dict[str, Any]: + links = robot_data.get("links", {}) or {} + return (links.get(child_link, {}) or {}).get("jointToParent", {}) or {} + + +def get_joint_axis(robot_data: Dict[str, Any], child_link: str) -> Optional[np.ndarray]: + joint = get_joint_info(robot_data, child_link) + axis = joint.get("axis", None) + if axis is None: + return None + axis = np.asarray(axis, dtype=np.float64) + if safe_norm(axis) < 1e-12: + return None + return normalize_vector(axis) + + +def get_vision_marker_size_default(robot_data: Dict[str, Any]) -> float: + vision = robot_data.get("vision_config", {}) or {} + ms = vision.get("MarkerSize", None) + if ms is None: + return 0.025 + return float(ms) + + +# =================================================================== +# Constraint compilation helpers +# =================================================================== + +def get_enabled_link_rule( + robot_data: Dict[str, Any], + link_name: str, + rule_name: str, + default_enabled: bool = True +) -> bool: + overrides = robot_data.get("constraint_overrides", {}) or {} + link_override = overrides.get(link_name, {}) or {} + rule_override = link_override.get(rule_name, {}) or {} + if "enabled" in rule_override: + return bool(rule_override["enabled"]) + return default_enabled + + +def select_anchor_marker_ids( + markers: List[Dict[str, Any]], + axis: Optional[np.ndarray] = None, + max_count: int = 2 +) -> List[int]: + if not markers: + return [] + if len(markers) == 1: + return [int(markers[0]["id"])] + + ids = [int(m["id"]) for m in markers] + pos = np.stack([np.asarray(m["position_m"], dtype=np.float64) for m in markers], axis=0) + + selected: List[int] = [] + + if axis is not None and safe_norm(axis) > 1e-12: + a = normalize_vector(axis) + proj = pos @ a + min_idx = int(np.argmin(proj)) + max_idx = int(np.argmax(proj)) + selected = [ids[min_idx], ids[max_idx]] + else: + centroid = np.mean(pos, axis=0) + d = np.linalg.norm(pos - centroid, axis=1) + min_idx = int(np.argmin(d)) + max_idx = int(np.argmax(d)) + selected = [ids[min_idx], ids[max_idx]] + + if len(selected) < max_count: + for mid in ids: + if mid not in selected: + selected.append(mid) + if len(selected) >= max_count: + break + + out: List[int] = [] + seen: set[int] = set() + for mid in selected: + if mid not in seen: + seen.add(mid) + out.append(mid) + if len(out) >= max_count: + break + return out + + +def mst_edges_for_link(markers: List[Dict[str, Any]]) -> List[Tuple[int, int]]: + n = len(markers) + if n < 2: + return [] + + ids = [int(m["id"]) for m in markers] + pos = np.stack([np.asarray(m["position_m"], dtype=np.float64) for m in markers], axis=0) + in_tree = np.zeros(n, dtype=bool) + in_tree[0] = True + edges: List[Tuple[int, int]] = [] + dist = np.linalg.norm(pos[:, None, :] - pos[None, :, :], axis=2) + + for _ in range(n - 1): + best = None + best_d = float("inf") + for i in range(n): + if not in_tree[i]: + continue + for j in range(n): + if in_tree[j]: + continue + d = float(dist[i, j]) + if d < best_d: + best_d = d + best = (i, j) + if best is None: + break + i, j = best + in_tree[j] = True + edges.append((ids[i], ids[j])) + return edges + + +def compile_rigid_distance_constraints( + robot_data: Dict[str, Any], + link_markers: Dict[str, List[Dict[str, Any]]], + cfg: ConstraintRuleConfig +) -> List[MarkerDistanceConstraint]: + constraints: List[MarkerDistanceConstraint] = [] + + for link_name, markers in link_markers.items(): + if not get_enabled_link_rule(robot_data, link_name, "rigid_distance", cfg.rigid_distance_enabled): + continue + if len(markers) < 2: + continue + + mode = cfg.rigid_distance_mode + if mode == "full": + pairs = [(int(a["id"]), int(b["id"])) for a, b in combinations(markers, 2)] + elif mode == "star": + anchor_ids = select_anchor_marker_ids(markers, axis=None, max_count=1) + anchor_id = anchor_ids[0] + pairs = [] + for m in markers: + mid = int(m["id"]) + if mid != anchor_id: + pairs.append((anchor_id, mid)) + elif mode == "mst": + pairs = mst_edges_for_link(markers) + else: + raise ValueError(f"Unknown rigid_distance_mode='{mode}'. Use mst|star|full.") + + pos_map = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in markers} + seen_pairs: set[Tuple[int, int]] = set() + + for mid_a, mid_b in pairs: + if mid_a == mid_b: + continue + key = tuple(sorted((int(mid_a), int(mid_b)))) + if key in seen_pairs: + continue + seen_pairs.add(key) + + pos_a = pos_map[mid_a] + pos_b = pos_map[mid_b] + target = float(np.linalg.norm(pos_b - pos_a)) + + constraints.append( + MarkerDistanceConstraint( + marker_id_a=mid_a, + marker_id_b=mid_b, + link_name=link_name, + target_distance_m=target, + weight=cfg.rigid_distance_weight, + enabled=True, + source=f"rigid_distance:{mode}", + ) + ) + + return constraints + + +def compile_joint_dof_constraints( + robot_data: Dict[str, Any], + link_markers: Dict[str, List[Dict[str, Any]]], + cfg: ConstraintRuleConfig +) -> List[JointAxisConstraint]: + """ + Compile local joint constraints from robot.json. + + Revolute joints: one scalar constraint per anchor pair + (projection along the joint axis stays constant) + + Prismatic joints: two scalar constraints per anchor pair + (the orthogonal projections stay constant) + + Both are emitted as JointAxisConstraint objects so the rest of the + optimization pipeline remains unchanged. + """ + constraints: List[JointAxisConstraint] = [] + links = robot_data.get("links", {}) or {} + + for child_link, child_data in links.items(): + parent_link = child_data.get("parent", None) + if not parent_link: + continue + + joint_info = child_data.get("jointToParent", {}) or {} + joint_type = str(joint_info.get("type", "")).strip().lower() + + joint_axis = get_joint_axis(robot_data, child_link) + if joint_axis is None: + continue + + axis_vec = principal_axis_vector(joint_axis) + + parent_markers = link_markers.get(parent_link, []) + child_markers = link_markers.get(child_link, []) + if len(parent_markers) == 0 or len(child_markers) == 0: + continue + + parent_pos = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in parent_markers} + child_pos = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in child_markers} + + seen: set[Tuple[int, int]] = set() + + if joint_type == "revolute": + if not get_enabled_link_rule( + robot_data, child_link, "joint_revolute_axis", cfg.revolute_axis_enabled + ): + continue + + max_pairs = max(1, int(cfg.revolute_axis_max_pairs)) + parent_anchor_ids = select_anchor_marker_ids(parent_markers, axis=axis_vec, max_count=max_pairs) + child_anchor_ids = select_anchor_marker_ids(child_markers, axis=axis_vec, max_count=max_pairs) + + for mid_p in parent_anchor_ids: + for mid_c in child_anchor_ids: + if mid_p == mid_c: + continue + key = (mid_p, mid_c) + if key in seen: + continue + seen.add(key) + + delta = child_pos[mid_c] - parent_pos[mid_p] + target = float(np.dot(delta, axis_vec)) + + constraints.append( + JointAxisConstraint( + marker_id_parent=mid_p, + marker_id_child=mid_c, + parent_link=parent_link, + child_link=child_link, + joint_axis=axis_vec, + target_delta_along_axis_m=target, + weight=cfg.revolute_axis_weight, + enabled=True, + source="revolute_axis_projection", + ) + ) + + elif joint_type == "linear": + if not get_enabled_link_rule( + robot_data, child_link, "joint_prismatic_orthogonal", cfg.prismatic_orthogonal_enabled + ): + continue + + max_pairs = max(1, int(cfg.prismatic_orthogonal_max_pairs)) + parent_anchor_ids = select_anchor_marker_ids(parent_markers, axis=axis_vec, max_count=max_pairs) + child_anchor_ids = select_anchor_marker_ids(child_markers, axis=axis_vec, max_count=max_pairs) + basis_u, basis_v = orthonormal_basis_from_axis(axis_vec) + + for mid_p in parent_anchor_ids: + for mid_c in child_anchor_ids: + if mid_p == mid_c: + continue + key = (mid_p, mid_c) + if key in seen: + continue + seen.add(key) + + delta = child_pos[mid_c] - parent_pos[mid_p] + + constraints.append( + JointAxisConstraint( + marker_id_parent=mid_p, + marker_id_child=mid_c, + parent_link=parent_link, + child_link=child_link, + joint_axis=basis_u, + target_delta_along_axis_m=float(np.dot(delta, basis_u)), + weight=cfg.prismatic_orthogonal_weight, + enabled=True, + source="prismatic_orthogonal_projection:u", + ) + ) + constraints.append( + JointAxisConstraint( + marker_id_parent=mid_p, + marker_id_child=mid_c, + parent_link=parent_link, + child_link=child_link, + joint_axis=basis_v, + target_delta_along_axis_m=float(np.dot(delta, basis_v)), + weight=cfg.prismatic_orthogonal_weight, + enabled=True, + source="prismatic_orthogonal_projection:v", + ) + ) + + else: + continue + + return constraints + + + + +def compile_constraints( + robot_data: Dict[str, Any], + length_scale: float, + cfg: ConstraintRuleConfig +) -> Tuple[Dict[int, str], Dict[str, List[Dict[str, Any]]], List[Constraint], List[str], Dict[int, Dict[str, Any]]]: + marker_to_link, link_markers, issues, marker_meta = parse_robot_markers( + robot_data, + length_scale=length_scale, + strict_unique_marker_ids=cfg.strict_unique_marker_ids, + ) + + constraints: List[Constraint] = [] + constraints.extend(compile_rigid_distance_constraints(robot_data, link_markers, cfg)) + constraints.extend(compile_joint_dof_constraints(robot_data, link_markers, cfg)) + + # Legacy optional family, OFF by default. + if cfg.chain_axis_enabled: + constraints.extend(compile_chain_axis_constraints(robot_data, link_markers, cfg)) + + unique_constraints: List[Constraint] = [] + seen_keys: set[Tuple[Any, ...]] = set() + + for c in constraints: + if isinstance(c, MarkerDistanceConstraint): + key = ( + "d", + min(c.marker_id_a, c.marker_id_b), + max(c.marker_id_a, c.marker_id_b), + c.link_name, + round(c.target_distance_m, 9), + ) + else: + key = ( + "j", + c.parent_link, + c.child_link, + c.marker_id_parent, + c.marker_id_child, + tuple(np.round(c.joint_axis, 9).tolist()), + round(c.target_delta_along_axis_m, 9), + ) + if key in seen_keys: + continue + seen_keys.add(key) + unique_constraints.append(c) + + return marker_to_link, link_markers, unique_constraints, issues, marker_meta + + +# =================================================================== +# Observation quality / weighting +# =================================================================== + +def _optional_float(meta: Dict[str, Any], keys: List[str]) -> Optional[float]: + for k in keys: + if k in meta and meta[k] is not None: + try: + return float(meta[k]) + except Exception: + pass + return None + + +def detection_quality_from_metadata(det_obj: Dict[str, Any], cfg: ConstraintRuleConfig) -> float: + q = 1.0 + + if cfg.use_detection_confidence: + conf = _optional_float(det_obj, ["confidence", "score", "quality", "det_confidence"]) + if conf is not None: + q *= clamp(conf, 0.1, 1.0) + + if cfg.use_detection_size_px: + size_px = _optional_float(det_obj, ["size_px", "marker_size_px", "side_px", "side_length_px"]) + if size_px is None and "corners_px" in det_obj and isinstance(det_obj["corners_px"], list): + try: + corners = np.asarray(det_obj["corners_px"], dtype=np.float64).reshape(-1, 2) + if len(corners) >= 4: + edges = [] + for i in range(len(corners)): + p = corners[i] + q2 = corners[(i + 1) % len(corners)] + edges.append(float(np.linalg.norm(q2 - p))) + size_px = float(np.mean(edges)) + except Exception: + size_px = None + if size_px is not None: + q *= clamp(size_px / max(cfg.ref_marker_size_px, 1e-6), 0.25, 3.0) + + sharpness = _optional_float(det_obj, ["sharpness", "corner_sharpness"]) + if sharpness is not None: + q *= clamp(sharpness / 2500.0, 0.5, 1.5) + + normal_alignment = _optional_float(det_obj, ["normal_alignment", "view_cosine", "cos_to_camera"]) + if normal_alignment is not None: + q *= clamp(normal_alignment, 0.3, 1.0) + + return float(q) + + +def marker_size_prior_factor(marker_meta: Dict[str, Any], default_marker_size_m: float) -> float: + size_val = marker_meta.get("size", None) + if size_val is None: + return 1.0 + + try: + size_val = float(size_val) + except Exception: + return 1.0 + + size_m = size_val / 1000.0 if size_val > 1.0 else size_val + ref = max(default_marker_size_m, 1e-6) + return clamp(size_m / ref, 0.7, 1.3) + + +def compute_observation_weights( + marker_observations: Dict[int, List[Observation]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], + initial_positions: Dict[int, np.ndarray], + marker_meta: Dict[int, Dict[str, Any]], + cfg: ConstraintRuleConfig, + robot_data: Dict[str, Any] +) -> Dict[Tuple[int, int], float]: + weights: Dict[Tuple[int, int], float] = {} + default_marker_size_m = get_vision_marker_size_default(robot_data) + + for marker_id, obs_list in marker_observations.items(): + X = initial_positions.get(marker_id, None) + size_prior = marker_size_prior_factor(marker_meta.get(marker_id, {}), default_marker_size_m) + + for obs_idx, obs in enumerate(obs_list): + w = 1.0 + q = detection_quality_from_metadata(obs.meta, cfg) + w *= q + + if cfg.use_marker_size_prior: + w *= size_prior + + if cfg.use_initial_range and X is not None: + _, _, R_wc, t_wc = cameras[obs.cam_idx] + C = camera_center_from_world_to_cam(R_wc, t_wc) + dist = float(np.linalg.norm(X - C)) + if np.isfinite(dist): + w *= clamp(cfg.ref_distance_m / max(dist, 1e-6), 0.4, 2.0) + + weights[(marker_id, obs_idx)] = clamp(w, cfg.weight_floor, cfg.weight_ceiling) + + return weights + + +# =================================================================== +# Multi-view loading +# =================================================================== + +def load_observations_and_poses( + detection_files: List[str], + pose_files: List[str] +) -> Tuple[ + Dict[int, List[Observation]], + List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], + List[Dict[str, Any]] +]: + if len(detection_files) != len(pose_files): + raise ValueError(f"Mismatch: {len(detection_files)} detections vs {len(pose_files)} poses") + + marker_observations: Dict[int, List[Observation]] = {} + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = [] + obs_metadata: List[Dict[str, Any]] = [] + + for cam_idx, (det_file, pose_file) in enumerate(zip(detection_files, pose_files)): + det = load_json(det_file) + pose_data = load_json(pose_file) + + cam_section = det.get("camera", {}) or {} + K = np.array(cam_section.get("camera_matrix", []), dtype=np.float64).reshape(3, 3) + D = np.array(cam_section.get("distortion_coefficients", []), dtype=np.float64).reshape(-1, 1) + + pose_section = pose_data.get("camera_pose", {}) or {} + world_to_cam = pose_section.get("world_to_camera", {}) or {} + R_wc = np.array(world_to_cam.get("rotation_matrix", []), dtype=np.float64).reshape(3, 3) + t_wc = np.array(world_to_cam.get("translation_m", []), dtype=np.float64).reshape(3) + + cameras.append((K, D, R_wc, t_wc)) + + detections = det.get("detections", []) or [] + for det_obj in detections: + marker_id = int(det_obj.get("marker_id", -1)) + if marker_id < 0: + continue + + center_px = np.array(det_obj.get("center_px", []), dtype=np.float64) + if center_px.shape != (2,): + continue + + pts = center_px.reshape(1, 1, 2).astype(np.float32) + und = cv2.undistortPoints(pts, K.astype(np.float32), D.astype(np.float32), P=None) + norm_coords = und.reshape(2).astype(np.float64) + + obs = Observation(cam_idx=cam_idx, norm_coords=norm_coords, meta=dict(det_obj)) + marker_observations.setdefault(marker_id, []).append(obs) + + obs_metadata.append( + { + "detection_file": det_file, + "pose_file": pose_file, + "num_detections": len(detections), + } + ) + + return marker_observations, cameras, obs_metadata + + +# =================================================================== +# Initial triangulation +# =================================================================== + +def triangulate_marker_initial( + marker_id: int, + observations: List[Observation], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] +) -> Optional[np.ndarray]: + if len(observations) < 2: + return None + + best_pair = None + best_baseline = -1.0 + + for obs_i, obs_j in combinations(observations, 2): + cam_i, cam_j = obs_i.cam_idx, obs_j.cam_idx + _, _, R1, t1 = cameras[cam_i] + _, _, R2, t2 = cameras[cam_j] + c1 = camera_center_from_world_to_cam(R1, t1) + c2 = camera_center_from_world_to_cam(R2, t2) + baseline = float(np.linalg.norm(c2 - c1)) + if baseline > best_baseline: + best_baseline = baseline + best_pair = (obs_i, obs_j) + + if best_pair is None: + return None + + obs_i, obs_j = best_pair + cam_i, cam_j = obs_i.cam_idx, obs_j.cam_idx + norm_coords_i = obs_i.norm_coords + norm_coords_j = obs_j.norm_coords + + K1, D1, R1, t1 = cameras[cam_i] + K2, D2, R2, t2 = cameras[cam_j] + + x1_px = K1[0, 0] * norm_coords_i[0] + K1[0, 2] + y1_px = K1[1, 1] * norm_coords_i[1] + K1[1, 2] + x2_px = K2[0, 0] * norm_coords_j[0] + K2[0, 2] + y2_px = K2[1, 1] * norm_coords_j[1] + K2[1, 2] + + P1 = K1 @ np.hstack([R1, t1.reshape(3, 1)]) + P2 = K2 @ np.hstack([R2, t2.reshape(3, 1)]) + + try: + X_h = cv2.triangulatePoints( + P1, + P2, + np.array([[x1_px], [y1_px]], dtype=np.float64), + np.array([[x2_px], [y2_px]], dtype=np.float64), + ) + X = (X_h[:3] / X_h[3]).reshape(3).astype(np.float64) + if not np.all(np.isfinite(X)): + return None + return X + except Exception: + return None + + +def initial_triangulation( + marker_observations: Dict[int, List[Observation]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] +) -> Dict[int, np.ndarray]: + triangulated: Dict[int, np.ndarray] = {} + for marker_id, observations in marker_observations.items(): + X = triangulate_marker_initial(marker_id, observations, cameras) + if X is not None: + triangulated[marker_id] = X + return triangulated + + +# =================================================================== +# Weighted residuals / optimization +# =================================================================== + +def bundle_adjustment_residuals( + marker_positions_flat: np.ndarray, + marker_ids: List[int], + marker_observations: Dict[int, List[Observation]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], + constraints: List[Constraint], + obs_weights: Dict[Tuple[int, int], float], + lambda_constraint: float = 100.0 +) -> np.ndarray: + marker_dict: Dict[int, np.ndarray] = {} + for i, marker_id in enumerate(marker_ids): + marker_dict[marker_id] = marker_positions_flat[i * 3:(i + 1) * 3] + + residuals: List[float] = [] + + for marker_id, observations in marker_observations.items(): + if marker_id not in marker_dict: + continue + + X_world = marker_dict[marker_id] + for obs_idx, obs in enumerate(observations): + cam_idx, norm_coords_obs = obs.cam_idx, obs.norm_coords + K, D, R_wc, t_wc = cameras[cam_idx] + X_cam = R_wc @ X_world + t_wc + if X_cam[2] > 1e-6: + proj_norm = X_cam[:2] / X_cam[2] + r = proj_norm - norm_coords_obs + w = float(np.sqrt(obs_weights.get((marker_id, obs_idx), 1.0))) + residuals.append(w * float(r[0])) + residuals.append(w * float(r[1])) + + for constraint in constraints: + if isinstance(constraint, MarkerDistanceConstraint): + if constraint.marker_id_a in marker_dict and constraint.marker_id_b in marker_dict: + pos_a = marker_dict[constraint.marker_id_a] + pos_b = marker_dict[constraint.marker_id_b] + actual_dist = float(np.linalg.norm(pos_b - pos_a)) + residuals.append((actual_dist - constraint.target_distance_m) * constraint.weight * lambda_constraint) + + elif isinstance(constraint, JointAxisConstraint): + if constraint.marker_id_parent in marker_dict and constraint.marker_id_child in marker_dict: + pos_parent = marker_dict[constraint.marker_id_parent] + pos_child = marker_dict[constraint.marker_id_child] + delta = pos_child - pos_parent + actual_delta = float(np.dot(delta, constraint.joint_axis)) + residuals.append((actual_delta - constraint.target_delta_along_axis_m) * constraint.weight * lambda_constraint) + + return np.asarray(residuals, dtype=np.float64) + + +def optimize_with_constraints( + initial_positions: Dict[int, np.ndarray], + marker_observations: Dict[int, List[Observation]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], + constraints: List[Constraint], + obs_weights: Dict[Tuple[int, int], float], + lambda_constraint: float = 100.0, + max_iterations: int = 50 +) -> Dict[int, np.ndarray]: + try: + from scipy.optimize import least_squares + except ImportError: + print("[WARN] scipy not available, skipping optimization.") + return initial_positions + + marker_ids = sorted(initial_positions.keys()) + if not marker_ids: + return {} + + x0 = np.concatenate([initial_positions[mid] for mid in marker_ids]) + + def residuals_fn(x: np.ndarray) -> np.ndarray: + return bundle_adjustment_residuals( + x, marker_ids, marker_observations, cameras, constraints, obs_weights, lambda_constraint + ) + + print(f"[INFO] Starting optimization with {len(x0)} variables and {len(constraints)} constraints...") + + result = least_squares( + residuals_fn, + x0, + max_nfev=max_iterations * max(1, len(marker_ids)), + verbose=1, + ) + + optimized = {} + for i, marker_id in enumerate(marker_ids): + optimized[marker_id] = result.x[i * 3:(i + 1) * 3] + + print(f"[INFO] Optimization complete. Final cost: {float(np.sum(result.fun ** 2)):.6f}") + return optimized + + +# =================================================================== +# Reporting helpers +# =================================================================== + +def print_constraint_summary(constraints: List[Constraint]) -> None: + num_dist = sum(isinstance(c, MarkerDistanceConstraint) for c in constraints) + num_joint = sum(isinstance(c, JointAxisConstraint) for c in constraints) + num_other = len(constraints) - num_dist - num_joint + extra = f" other={num_other}" if num_other else "" + print(f"[INFO] Constraint summary: total={len(constraints)} distance={num_dist} joint/chain={num_joint}{extra}") + + +def print_constraint_list(constraints: List[Constraint]) -> None: + print("\n[INFO] Constraint list:") + for idx, constraint in enumerate(constraints): + if isinstance(constraint, MarkerDistanceConstraint): + print( + f" [{idx:03d}] DISTANCE | " + f"Link='{constraint.link_name}' | " + f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | " + f"Target={constraint.target_distance_m:.6f} m | " + f"Weight={constraint.weight} | " + f"Source={constraint.source}" + ) + elif isinstance(constraint, JointAxisConstraint): + axis_str = np.array2string(constraint.joint_axis, precision=3, suppress_small=True) + print( + f" [{idx:03d}] JOINT_AXIS | " + f"{constraint.parent_link}(M{constraint.marker_id_parent}) -> " + f"{constraint.child_link}(M{constraint.marker_id_child}) | " + f"Axis={axis_str} | " + f"TargetDelta={constraint.target_delta_along_axis_m:.6f} m | " + f"Weight={constraint.weight} | " + f"Source={constraint.source}" + ) + else: + print( + f" [{idx:03d}] {type(constraint).__name__} | " + f"weight={getattr(constraint, 'weight', '?')} | " + f"source={getattr(constraint, 'source', '?')}" + ) + + +def print_constraints_with_errors( + title: str, + constraints: List[Constraint], + positions: Dict[int, np.ndarray], + show_skipped: bool = True +) -> None: + print(f"\n[INFO] {title}") + + active = 0 + skipped = 0 + + for idx, constraint in enumerate(constraints): + if isinstance(constraint, MarkerDistanceConstraint): + if constraint.marker_id_a not in positions or constraint.marker_id_b not in positions: + skipped += 1 + if show_skipped: + print( + f" [{idx:03d}] DISTANCE | " + f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | SKIPPED (missing marker)" + ) + continue + + pos_a = positions[constraint.marker_id_a] + pos_b = positions[constraint.marker_id_b] + actual = float(np.linalg.norm(pos_b - pos_a)) + error = actual - constraint.target_distance_m + active += 1 + + print( + f" [{idx:03d}] DISTANCE | " + f"Link='{constraint.link_name}' | " + f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | " + f"target={constraint.target_distance_m*1000:.2f} mm | " + f"actual={actual*1000:.2f} mm | " + f"error={error*1000:+.2f} mm" + ) + + elif isinstance(constraint, JointAxisConstraint): + if constraint.marker_id_parent not in positions or constraint.marker_id_child not in positions: + skipped += 1 + if show_skipped: + print( + f" [{idx:03d}] JOINT_AXIS | " + f"M{constraint.marker_id_parent} -> M{constraint.marker_id_child} | SKIPPED (missing marker)" + ) + continue + + pos_parent = positions[constraint.marker_id_parent] + pos_child = positions[constraint.marker_id_child] + delta = pos_child - pos_parent + actual = float(np.dot(delta, constraint.joint_axis)) + error = actual - constraint.target_delta_along_axis_m + active += 1 + + axis_str = np.array2string(constraint.joint_axis, precision=2, suppress_small=True) + print( + f" [{idx:03d}] JOINT_AXIS | " + f"{constraint.parent_link}(M{constraint.marker_id_parent}) -> " + f"{constraint.child_link}(M{constraint.marker_id_child}) | " + f"axis={axis_str} | " + f"target={constraint.target_delta_along_axis_m*1000:.2f} mm | " + f"actual={actual*1000:.2f} mm | " + f"error={error*1000:+.2f} mm" + ) + + print(f"[INFO] Active constraints: {active} | Skipped: {skipped}") + + +def print_observation_weight_summary(obs_weights: Dict[Tuple[int, int], float]) -> None: + if not obs_weights: + print("[INFO] Observation weighting: disabled or empty") + return + values = np.array(list(obs_weights.values()), dtype=np.float64) + print( + "[INFO] Observation weights: " + f"min={values.min():.3f} mean={values.mean():.3f} " + f"median={np.median(values):.3f} max={values.max():.3f}" + ) + + +def serialize_vec3(v: Any) -> List[float]: + arr = np.asarray(v, dtype=np.float64).reshape(3) + n = np.linalg.norm(arr) + if n > 1e-12: + arr = arr / n + return [float(x) for x in arr] + + +# =================================================================== +# Main +# =================================================================== + +def main() -> None: + parser = argparse.ArgumentParser( + description="Multi-view bundle adjustment with rule-based geometric constraints" + ) + parser.add_argument( + "-det", "--detections", + action="append", + required=True, + help="*_aruco_detection.json files" + ) + parser.add_argument( + "-pose", "--poses", + action="append", + required=True, + help="*_camera_pose.json files" + ) + parser.add_argument( + "-robot", "--robot", + required=True, + help="robot.json" + ) + parser.add_argument( + "-outDir", "--outDir", + default=None, + help="Output directory" + ) + parser.add_argument( + "-lambdaWeight", "--lambdaWeight", + type=float, + default=100.0, + help="Constraint weight multiplier" + ) + parser.add_argument( + "--strictUniqueMarkerIds", + action="store_true", + help="Fail if a marker ID appears more than once in robot.json" + ) + parser.add_argument( + "--showSkippedConstraints", + action="store_true", + help="Print skipped constraints in the report" + ) + parser.add_argument( + "--noShowSkippedConstraints", + action="store_true", + help="Hide skipped constraints in the report" + ) + parser.add_argument( + "--saveConstraintReport", + action="store_true", + help="Save constraint report JSON files" + ) + parser.add_argument( + "--saveObservationWeightReport", + action="store_true", + help="Save observation-weight report JSON file" + ) + + args = parser.parse_args() + + if args.showSkippedConstraints and args.noShowSkippedConstraints: + print("[ERROR] Choose only one of --showSkippedConstraints or --noShowSkippedConstraints") + sys.exit(1) + + if len(args.detections) != len(args.poses): + print(f"[ERROR] Mismatch: {len(args.detections)} detection files vs {len(args.poses)} pose files") + sys.exit(1) + + robot_data = load_json(args.robot) + length_scale = get_length_scale(robot_data) + cfg = load_constraint_rule_config(robot_data, args) + + print("[STEP 1] Compile constraints from robot.json structure...") + print( + "[INFO] Constraint families: " + f"rigid_distance={'on' if cfg.rigid_distance_enabled else 'off'}, " + f"revolute={'on' if cfg.revolute_axis_enabled else 'off'}, " + f"prismatic={'on' if cfg.prismatic_orthogonal_enabled else 'off'}, " + f"chain_legacy={'on' if cfg.chain_axis_enabled else 'off'}, " + f"observation_weights={'on' if cfg.enable_observation_weights else 'off'}" + ) + marker_to_link, link_markers, constraints, issues, marker_meta = compile_constraints(robot_data, length_scale, cfg) + + for issue in issues: + print(issue) + + print(f"[INFO] Links with markers: {sum(1 for v in link_markers.values() if len(v) > 0)}") + print(f"[INFO] Unique marker IDs: {len(marker_to_link)}") + print_constraint_summary(constraints) + print_constraint_list(constraints) + + print("\n[STEP 2] Load observations and camera poses...") + marker_observations, cameras, obs_metadata = load_observations_and_poses(args.detections, args.poses) + print(f"[INFO] {len(cameras)} cameras, {len(marker_observations)} observed markers") + print(f"[INFO] Detection files loaded: {len(obs_metadata)}") + + print("\n[STEP 3] Initial triangulation...") + initial_pos = initial_triangulation(marker_observations, cameras) + print(f"[INFO] Triangulated {len(initial_pos)} markers") + + out_dir = args.outDir or os.path.dirname(args.detections[0]) or "." + os.makedirs(resolve_path(out_dir), exist_ok=True) + + # camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2] + cameras_section = [] + for idx, (K, D, R_wc, t_wc) in enumerate(cameras): + C = -R_wc.T @ t_wc + cam_id = str(idx) + base = os.path.basename(str(obs_metadata[idx].get("pose_file", ""))) if idx < len(obs_metadata) else "" + if base.startswith("render_") and "_camera_pose" in base: + cam_id = base[len("render_"):base.index("_camera_pose")] + cameras_section.append({ + "camera_id": cam_id, + "position_m": [float(v) for v in C], + "position_mm": [float(v * 1000.0) for v in C], + "direction": [float(v) for v in R_wc[2]], + }) + + initial_output_markers = [] + for marker_id, position in sorted(initial_pos.items()): + normal = marker_meta.get(marker_id, {}).get("normal", None) + initial_output_markers.append( + { + "marker_id": int(marker_id), + "position_m": [float(x) for x in position], + "position_mm": [float(x * 1000.0) for x in position], + "link": marker_to_link.get(marker_id, "unknown"), + "normal": serialize_vec3(normal) if normal is not None else None, + } + ) + + initial_output = { + "schema_version": "1.2", + "stage": "initial_triangulation", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_cameras": len(cameras), + "num_markers": len(initial_pos), + "num_constraints": len(constraints), + }, + "cameras": cameras_section, + "markers": initial_output_markers, + } + initial_out_file = os.path.join(out_dir, "aruco_positions_initial.json") + save_json(initial_out_file, initial_output) + print(f"[INFO] Initial triangulation saved to {initial_out_file}") + + obs_weights = compute_observation_weights( + marker_observations=marker_observations, + cameras=cameras, + initial_positions=initial_pos, + marker_meta=marker_meta, + cfg=cfg, + robot_data=robot_data, + ) + print_observation_weight_summary(obs_weights) + + print_constraints_with_errors( + "Constraint list BEFORE optimization", + constraints, + initial_pos, + show_skipped=cfg.show_skipped_constraints, + ) + + print("\n[STEP 4] Bundle adjustment with constraints...") + optimized_pos = optimize_with_constraints( + initial_pos, + marker_observations, + cameras, + constraints, + obs_weights, + lambda_constraint=args.lambdaWeight, + ) + + print_constraints_with_errors( + "Constraint list AFTER optimization", + constraints, + optimized_pos, + show_skipped=cfg.show_skipped_constraints, + ) + + output_markers = [] + for marker_id, position in sorted(optimized_pos.items()): + normal = marker_meta.get(marker_id, {}).get("normal", None) + output_markers.append( + { + "marker_id": int(marker_id), + "position_m": [float(x) for x in position], + "position_mm": [float(x * 1000.0) for x in position], + "link": marker_to_link.get(marker_id, "unknown"), + "normal": serialize_vec3(normal) if normal is not None else None, + } + ) + + output = { + "schema_version": "1.2", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_cameras": len(cameras), + "num_markers": len(optimized_pos), + "num_constraints": len(constraints), + }, + "cameras": cameras_section, + "markers": output_markers, + } + out_file = os.path.join(out_dir, "aruco_positions_optimized.json") + save_json(out_file, output) + print(f"\n[INFO] Saved to {out_file}") + + if args.saveConstraintReport: + report = { + "schema_version": "1.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_constraints": len(constraints), + "num_links_with_markers": sum(1 for v in link_markers.values() if len(v) > 0), + "num_observed_markers": len(marker_observations), + "num_triangulated_markers": len(initial_pos), + "num_optimized_markers": len(optimized_pos), + }, + "constraints": [], + } + for c in constraints: + if isinstance(c, MarkerDistanceConstraint): + report["constraints"].append( + { + "kind": "distance", + "link_name": c.link_name, + "marker_id_a": c.marker_id_a, + "marker_id_b": c.marker_id_b, + "target_distance_m": c.target_distance_m, + "weight": c.weight, + "source": c.source, + } + ) + else: + report["constraints"].append( + { + "kind": "joint_axis", + "parent_link": c.parent_link, + "child_link": c.child_link, + "marker_id_parent": c.marker_id_parent, + "marker_id_child": c.marker_id_child, + "joint_axis": [float(x) for x in c.joint_axis], + "target_delta_along_axis_m": c.target_delta_along_axis_m, + "weight": c.weight, + "source": c.source, + } + ) + report_file = os.path.join(out_dir, "constraint_report.json") + save_json(report_file, report) + print(f"[INFO] Constraint report saved to {report_file}") + + if args.saveObservationWeightReport: + obs_report = { + "schema_version": "1.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_weighted_observations": len(obs_weights), + }, + "observation_weights": [ + { + "marker_id": int(mid), + "observation_index": int(obs_idx), + "weight": float(w), + } + for (mid, obs_idx), w in sorted(obs_weights.items()) + ], + } + obs_file = os.path.join(out_dir, "observation_weight_report.json") + save_json(obs_file, obs_report) + print(f"[INFO] Observation-weight report saved to {obs_file}") + + +if __name__ == "__main__": + main() diff --git a/scripts/3_multiview_bundle_adjustment_v5_board_anchor_patch.py b/scripts/3_multiview_bundle_adjustment_v5_board_anchor_patch.py new file mode 100644 index 0000000..68f8fdd --- /dev/null +++ b/scripts/3_multiview_bundle_adjustment_v5_board_anchor_patch.py @@ -0,0 +1,271 @@ +#!/usr/bin/env python3 +""" +3_board_anchor_patch.py +----------------------- +DROP-IN PATCH for 3_multiview_bundle_adjustment_v4.py + +What it adds +------------ +Board markers have KNOWN world positions (they are fixed to the physical board, +which defines the coordinate frame). Letting the optimizer move them freely +makes them drift, which degrades all arm-marker estimates. + +This patch adds a PositionAnchorConstraint that pins each observed Board +marker to its robot.json world position during bundle adjustment. + +How to integrate +---------------- +Copy the three sections labelled [PATCH – copy into v4] into the v4 file: + + 1. The PositionAnchorConstraint dataclass → after the JointAxisConstraint dataclass + 2. Update the Constraint type alias → replace the existing line + 3. The load_board_anchors() function → anywhere before main() + 4. The change to bundle_adjustment_residuals → add one branch inside the for-loop + 5. The three lines in main() → just before "STEP 4" + +Quick-test without editing v4 +------------------------------ +You can also run this file standalone — it imports from v4 and patches it at +runtime. Then call main_anchored() instead of main() for the patched run. + + python 3_board_anchor_patch.py -det ... -pose ... -robot ... -outDir ... +""" + +from __future__ import annotations + +# ══════════════════════════════════════════════════════════════ +# [PATCH section 1 – copy into v4 after JointAxisConstraint] +# ══════════════════════════════════════════════════════════════ +from dataclasses import dataclass +import numpy as np +from typing import Dict, Any, List, Optional, Tuple + + +@dataclass +class PositionAnchorConstraint: + """ + Pins a single marker to a known world-space position. + Used to anchor Board markers whose world positions are read from robot.json. + """ + marker_id: int + target_world_m: np.ndarray # shape (3,), in metres + weight: float = 100.0 + source: str = "board_anchor" + + +# ══════════════════════════════════════════════════════════════ +# [PATCH section 2 – replace the Constraint type alias in v4] +# ══════════════════════════════════════════════════════════════ +# OLD: Constraint = MarkerDistanceConstraint | JointAxisConstraint +# NEW: +# from 3_board_anchor_patch import PositionAnchorConstraint +# Constraint = MarkerDistanceConstraint | JointAxisConstraint | PositionAnchorConstraint + + +# ══════════════════════════════════════════════════════════════ +# [PATCH section 3 – new function, copy anywhere before main()] +# ══════════════════════════════════════════════════════════════ + +def load_board_anchors( + robot_data: Dict[str, Any], + length_scale: float, + weight: float = 100.0, +) -> List[PositionAnchorConstraint]: + """ + Build PositionAnchorConstraints for every marker on the 'Board' link. + + Board is the root link; its marker positions are in the Board (world) frame. + length_scale = 1/1000 when robot.json uses mm (the standard). + + Parameters + ---------- + robot_data : loaded robot.json + length_scale : float (1/1000 to convert mm → m) + weight : constraint weight (100 ≈ 1 mm anchor tolerance at λ=100) + """ + links = robot_data.get("links", {}) or {} + board = links.get("Board", {}) or {} + anchors: List[PositionAnchorConstraint] = [] + + for m in board.get("markers", []): + mid = int(m.get("id", -1)) + pos = m.get("position", None) + if mid < 0 or pos is None or len(pos) != 3: + continue + world_m = np.array(pos, dtype=np.float64) * float(length_scale) + anchors.append( + PositionAnchorConstraint( + marker_id=mid, + target_world_m=world_m, + weight=weight, + source="board_anchor", + ) + ) + + return anchors + + +# ══════════════════════════════════════════════════════════════ +# [PATCH section 4 – add this branch inside bundle_adjustment_residuals, +# in the "for constraint in constraints:" loop, after the JointAxisConstraint branch] +# ══════════════════════════════════════════════════════════════ + +def _anchor_residuals(constraint: PositionAnchorConstraint, + marker_dict: Dict[int, np.ndarray], + lambda_constraint: float) -> List[float]: + """ + Returns 3 residual components w·λ·(X_observed - X_target) for each axis. + + Paste the body of this function into bundle_adjustment_residuals like this: + + elif isinstance(constraint, PositionAnchorConstraint): + if constraint.marker_id in marker_dict: + diff = marker_dict[constraint.marker_id] - constraint.target_world_m + for d in diff: + residuals.append(float(d) * constraint.weight * lambda_constraint) + """ + residuals: List[float] = [] + if constraint.marker_id in marker_dict: + diff = marker_dict[constraint.marker_id] - constraint.target_world_m + for d in diff: + residuals.append(float(d) * constraint.weight * lambda_constraint) + return residuals + + +# ══════════════════════════════════════════════════════════════ +# [PATCH section 5 – add these lines in main(), just BEFORE +# "print('\n[STEP 4] Bundle adjustment with constraints...')"] +# ══════════════════════════════════════════════════════════════ +# +# board_anchors = load_board_anchors(robot_data, length_scale, weight=100.0) +# constraints += board_anchors +# print(f"[INFO] Board anchors added: {len(board_anchors)}") +# +# ══════════════════════════════════════════════════════════════ + + +# ────────────────────────────────────────────────────────────── +# STANDALONE RUNTIME PATCH (alternative to editing v4) +# Run this file directly and it patches v4 in memory. +# ────────────────────────────────────────────────────────────── + +def _monkey_patch_v4(): + """ + Import the v4 module under an alias and patch it so Board markers are + anchored without touching the original file. + """ + import importlib.util, sys, types, pathlib + + v4_path = pathlib.Path(__file__).parent / "3_multiview_bundle_adjustment_v4.py" + if not v4_path.exists(): + raise FileNotFoundError( + f"Expected v4 at {v4_path}.\n" + "Place this patch file alongside 3_multiview_bundle_adjustment_v4.py." + ) + + spec = importlib.util.spec_from_file_location("ba_v4", v4_path) + mod = importlib.util.module_from_spec(spec) + sys.modules["ba_v4"] = mod # must be registered before exec_module for @dataclass + spec.loader.exec_module(mod) + + # Inject PositionAnchorConstraint into the module namespace + mod.PositionAnchorConstraint = PositionAnchorConstraint + + # Patch bundle_adjustment_residuals ───────────────────────── + _orig_residuals = mod.bundle_adjustment_residuals + + def _patched_residuals(marker_positions_flat, marker_ids, marker_observations, + cameras, constraints, obs_weights, lambda_constraint=100.0): + r = _orig_residuals(marker_positions_flat, marker_ids, marker_observations, + cameras, constraints, obs_weights, lambda_constraint) + # Rebuild marker_dict (same as inside the original function) + marker_dict = { + mid: marker_positions_flat[i*3:(i+1)*3] + for i, mid in enumerate(marker_ids) + } + extra = [] + for c in constraints: + if isinstance(c, PositionAnchorConstraint): + extra.extend(_anchor_residuals(c, marker_dict, lambda_constraint)) + if extra: + r = np.concatenate([r, np.array(extra, dtype=np.float64)]) + return r + + mod.bundle_adjustment_residuals = _patched_residuals + + # Patch main to inject board anchors ──────────────────────── + _orig_main = mod.main + + def _patched_main(): + import sys as _sys + # We hijack the module-level function calls by patching compile_constraints + _orig_compile = mod.compile_constraints + _orig_initial = mod.initial_triangulation + + # robot_data / length_scale are captured here so the initial-triangulation + # wrapper below can pin Board markers to their true world positions. + # In v4.main(), compile_constraints() runs BEFORE initial_triangulation(), + # so this dict is populated by the time the triangulation wrapper fires. + captured: Dict[str, Any] = {} + + def _compile_with_anchors(robot_data, length_scale, cfg): + captured["robot_data"] = robot_data + captured["length_scale"] = length_scale + + marker_to_link, link_markers, constraints, issues, marker_meta = \ + _orig_compile(robot_data, length_scale, cfg) + + anchors = load_board_anchors(robot_data, length_scale, weight=100.0) + print(f"[PATCH] Board anchors added: {len(anchors)}") + constraints = constraints + anchors + return marker_to_link, link_markers, constraints, issues, marker_meta + + def _initial_with_board(marker_observations, cameras): + """ + Run the normal multi-view triangulation, then overwrite every + OBSERVED Board marker with its exact robot.json world position. + Board markers define the world frame, so they must not be left at + their (z-noisy) triangulated values — not even in the initial JSON. + """ + tri = _orig_initial(marker_observations, cameras) + + robot_data = captured.get("robot_data") + length_scale = captured.get("length_scale", 1.0) + if robot_data is None: + return tri + + board = (robot_data.get("links", {}) or {}).get("Board", {}) or {} + pinned = 0 + for m in board.get("markers", []): + mid = int(m.get("id", -1)) + pos = m.get("position", None) + if mid < 0 or pos is None or len(pos) != 3: + continue + # Only pin markers that were actually observed/triangulated, + # so we don't introduce markers that no camera ever saw. + if mid in tri: + tri[mid] = np.array(pos, dtype=np.float64) * float(length_scale) + pinned += 1 + print(f"[PATCH] Board markers pinned to robot.json in initial triangulation: {pinned}") + return tri + + mod.compile_constraints = _compile_with_anchors + mod.initial_triangulation = _initial_with_board + _orig_main() + mod.compile_constraints = _orig_compile # restore + mod.initial_triangulation = _orig_initial # restore + + mod.main = _patched_main + return mod + + +def main(): + """Run the patched v4 pipeline with Board marker anchoring.""" + import sys + mod = _monkey_patch_v4() + sys.argv[0] = "3_multiview_bundle_adjustment_v4.py" # cosmetic + mod.main() + + +if __name__ == "__main__": + main() diff --git a/scripts/3b_corner_marker_poses.py b/scripts/3b_corner_marker_poses.py new file mode 100644 index 0000000..f02fd5e --- /dev/null +++ b/scripts/3b_corner_marker_poses.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +""" +3b_corner_marker_poses.py +========================= +Produktiver Pipeline-Schritt: leitet aus den 4 ArUco-Ecken jedes Markers eine +volle Marker-Pose ab (Position + gemessene Normale), statt nur den Center zu +triangulieren. + +Validiert in benchmark/stage0_corner_normals.py: die aus triangulierten Ecken +abgeleitete Normale ist ~1 deg genau (Median), auch fuer Finger-Marker. + +Input: + --evalDir Ordner mit render_*_aruco_detection.json + _camera_pose.json + --robot robot.json (fuer marker_id -> link Zuordnung) +Output: + /aruco_marker_poses.json (pro Marker: position, gemessene normal, + 4 triangulierte Ecken, #Kameras, Kantenlaenge) + +Das Format ist kompatibel mit robot_viewer.html (marker_id, position_m/mm, normal) +und mit 9_evaluateMarker.py (position_m), erweitert um die gemessene Orientierung. +""" +from __future__ import annotations + +import argparse +import glob +import json +import os +import re +import time +from typing import Dict, List, Tuple + +import numpy as np +import cv2 + + +# ------------------------------------------------------------------ +# Loading +# ------------------------------------------------------------------ + +def load_cameras(eval_dir: str) -> Dict[str, dict]: + cams: Dict[str, dict] = {} + for det_path in glob.glob(os.path.join(eval_dir, "*_aruco_detection.json")): + base = os.path.basename(det_path) + m = re.match(r"render_([A-Za-z0-9]+)_aruco_detection\.json", base) + if not m: + continue + cam_id = m.group(1) + pose_path = os.path.join(eval_dir, f"render_{cam_id}_camera_pose.json") + if not os.path.exists(pose_path): + print(f"[WARN] no pose for camera {cam_id}, skipping") + continue + det = json.load(open(det_path, "r", encoding="utf-8")) + pose = json.load(open(pose_path, "r", encoding="utf-8")) + K = np.array(det["camera"]["camera_matrix"], dtype=float).reshape(3, 3) + D = np.array(det["camera"]["distortion_coefficients"], dtype=float).reshape(-1, 1) + w2c = pose["camera_pose"]["world_to_camera"] + R = np.array(w2c["rotation_matrix"], dtype=float).reshape(3, 3) + t = np.array(w2c["translation_m"], dtype=float).reshape(3) + markers: Dict[int, np.ndarray] = {} + for d in det.get("detections", []): + pts = d.get("image_points_px") + if pts is not None: + markers[int(d["marker_id"])] = np.array(pts, dtype=float).reshape(4, 2) + cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers) + return cams + + +def load_marker_links(robot_path: str) -> Dict[int, str]: + robot = json.load(open(robot_path, "r", encoding="utf-8")) + out: Dict[int, str] = {} + for link_name, link in (robot.get("links", {}) or {}).items(): + for mk in link.get("markers", []) or []: + mid = int(mk.get("id", -1)) + if mid >= 0: + out[mid] = link_name + return out + + +# ------------------------------------------------------------------ +# Geometry (validated in stage0) +# ------------------------------------------------------------------ + +def triangulate_multiview(observations) -> np.ndarray: + A = [] + for K, D, R, t, uv in observations: + und = cv2.undistortPoints(np.array([[uv]], dtype=np.float32), K, D).reshape(2) + x, y = float(und[0]), float(und[1]) + P = np.hstack([R, t.reshape(3, 1)]) + A.append(x * P[2] - P[0]) + A.append(y * P[2] - P[1]) + _, _, Vt = np.linalg.svd(np.asarray(A, dtype=float)) + X = Vt[-1] + return np.array([np.nan] * 3) if abs(X[3]) < 1e-12 else X[:3] / X[3] + + +def corner_plane_normal(corners3d: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + center = corners3d.mean(axis=0) + _, _, Vt = np.linalg.svd(corners3d - center) + n = Vt[-1] + # ArUco corners clockwise from the front: outward (camera-facing) normal, + # matching the Blender/robot.json convention, points opposite cross(e01,e02). + cross = np.cross(corners3d[1] - corners3d[0], corners3d[2] - corners3d[0]) + if np.dot(n, cross) > 0: + n = -n + nn = np.linalg.norm(n) + return (n / nn if nn > 1e-12 else n), center + + +# ------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------ + +def main() -> None: + ap = argparse.ArgumentParser(description="Derive marker poses (position + measured normal) from ArUco corners") + ap.add_argument("--evalDir", required=True, help="folder with detection + camera_pose JSONs") + ap.add_argument("--robot", required=True, help="robot.json (for marker->link)") + ap.add_argument("--minCams", type=int, default=2, help="min cameras to triangulate a marker") + ap.add_argument("--out", default=None, help="output path (default /aruco_marker_poses.json)") + args = ap.parse_args() + + cams = load_cameras(args.evalDir) + if len(cams) < 2: + print("[ERROR] need >=2 cameras") + return + links = load_marker_links(args.robot) + print(f"[INFO] Cameras: {sorted(cams.keys())} | marker-link entries: {len(links)}") + + marker_cams: Dict[int, List[str]] = {} + for cid, cam in cams.items(): + for mid in cam["markers"]: + marker_cams.setdefault(mid, []).append(cid) + + markers_out = [] + for mid, cam_ids in sorted(marker_cams.items()): + if len(cam_ids) < args.minCams: + continue + corners3d, ok = [], True + for ci in range(4): + obs = [(cams[c]["K"], cams[c]["D"], cams[c]["R"], cams[c]["t"], cams[c]["markers"][mid][ci]) + for c in cam_ids] + X = triangulate_multiview(obs) + if not np.all(np.isfinite(X)): + ok = False + break + corners3d.append(X) + if not ok: + continue + corners3d = np.array(corners3d) + normal, center = corner_plane_normal(corners3d) + edge_mm = float(np.mean([np.linalg.norm(corners3d[(i + 1) % 4] - corners3d[i]) for i in range(4)]) * 1000.0) + + markers_out.append({ + "marker_id": int(mid), + "link": links.get(mid, "unknown"), + "position_m": [float(v) for v in center], + "position_mm": [float(v * 1000.0) for v in center], + "normal": [float(v) for v in normal], + "corners_m": [[float(v) for v in c] for c in corners3d], + "num_cameras": len(cam_ids), + "edge_length_mm": edge_mm, + }) + + # camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2] + cameras_out = [] + for cid in sorted(cams.keys()): + cam = cams[cid] + C = -cam["R"].T @ cam["t"] + cameras_out.append({ + "camera_id": cid, + "position_m": [float(v) for v in C], + "position_mm": [float(v * 1000.0) for v in C], + "direction": [float(v) for v in cam["R"][2]], + }) + + out_path = args.out or os.path.join(args.evalDir, "aruco_marker_poses.json") + output = { + "schema_version": "1.1", + "stage": "corner_marker_poses", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": {"num_cameras": len(cams), "num_markers": len(markers_out)}, + "cameras": cameras_out, + "markers": markers_out, + } + json.dump(output, open(out_path, "w", encoding="utf-8"), indent=2) + print(f"[INFO] {len(markers_out)} marker poses -> {out_path}") + + +if __name__ == "__main__": + main() diff --git a/scripts/A0_60Arucos_25mm_Seet223.json b/scripts/A0_60Arucos_25mm_Seet223.json new file mode 100644 index 0000000..d8fc77b --- /dev/null +++ b/scripts/A0_60Arucos_25mm_Seet223.json @@ -0,0 +1,83 @@ +{ + "page_format": "A0", + "orientation": "portrait", + "page_size_mm": { + "width": 841.0, + "height": 1189.0 + }, + "seed": 223, + "num_arucos": 60, + "aruco_size_mm": 25.0, + "aruco_dictionary": "DICT_4X4_250", + "aruco_start_id": 46, + "page_border_margin_mm": 50.0, + "forbidden_rectangle_mm": { + "x": 318.5, + "y": 94.5, + "w": 204.0, + "h": 1000.0 + }, + "forbidden_rectangle_margin_mm": 30.0, + "placements": [ + {"id": 46, "position": [536.71, 185.44, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 47, "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 48, "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 49, "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 50, "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 51, "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 52, "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 53, "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 54, "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 55, "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 56, "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 57, "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 58, "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 59, "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 60, "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 61, "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 62, "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 63, "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 64, "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 65, "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 66, "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 67, "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 68, "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 69, "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 70, "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 71, "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 72, "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 73, "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 74, "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 75, "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 76, "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 77, "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 78, "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 79, "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 80, "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 81, "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 82, "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 83, "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 84, "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 85, "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 86, "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 87, "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 88, "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 89, "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 90, "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 91, "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 92, "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 93, "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 94, "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 95, "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 96, "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 97, "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 98, "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 99, "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 100, "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 101, "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 102, "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 103, "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 104, "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 105, "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90} + ] +} diff --git a/scripts/A0_60Arucos_25mm_Seet223.pdf b/scripts/A0_60Arucos_25mm_Seet223.pdf new file mode 100644 index 0000000..e375d4c --- /dev/null +++ b/scripts/A0_60Arucos_25mm_Seet223.pdf @@ -0,0 +1,74 @@ +%PDF-1.3 +% ReportLab Generated PDF document http://www.reportlab.com +1 0 obj +<< +/F1 2 0 R /F2 3 0 R +>> +endobj +2 0 obj +<< +/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font +>> +endobj +3 0 obj +<< +/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font +>> +endobj +4 0 obj +<< +/Contents 8 0 R /MediaBox [ 0 0 2383.937 3370.394 ] /Parent 7 0 R /Resources << +/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ] +>> /Rotate 0 /Trans << + +>> + /Type /Page +>> +endobj +5 0 obj +<< +/PageMode /UseNone /Pages 7 0 R /Type /Catalog +>> +endobj +6 0 obj +<< +/Author (OpenAI) /CreationDate (D:20260530163955+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260530163955+02'00') /Producer (ReportLab PDF Library - www.reportlab.com) + /Subject (A0 poster with random ArUco placement) /Title (A0_60Arucos_25mm_Seet223) /Trapped /False +>> +endobj +7 0 obj +<< +/Count 1 /Kids [ 4 0 R ] /Type /Pages +>> +endobj +8 0 obj +<< +/Filter [ /ASCII85Decode /FlateDecode ] /Length 11520 +>> +stream +Gasba]<3csEJ="0MEeH_[!BE^#&GZIm.1F9A\dd`:nlQca)n(@Js9O+7$Y>pG\:g?1-[?d/$;lR0)hNF"fVO%]K-5=pN?@j>H/s_d+:fa*+:55-]f^of>;l?6`'6reIClPYL0FEH5le;@OULUKAKr[cOP\N"k$IZkH?TZD#Qcbq^[d)"t@g6!%_.]0^"0$Z/G@Of+5=$ALDI"D\q0PXhNhp(H_tQU"7t_4'05Y8Som0#:`%V-Z;!c3Al+2@uh;0YpqN,ZqARYkL4'I>5H[k1/H]ea-N09i*-L1/Zp@L]J!A5RDo/So_V]/9f:H>JuAfW\\?9h.E.SmF>s)eQeF&?P[mYiZ&(RLb4.SS&7M8aJlP8*/ttqiB#YaU>_VXKTDl"7ahbOCl4i%_aZd6bPgcSO)@#3,53bGm?/Hlk=%==4*^$&XGN*A>js)W\t5-]*3plQ]!`2?@=+>T9N&'A@FV;XN-&uY;OP8%lcKA6!k_SVeYf(ZC;Alp"1taZ!k5I%.m4h>G6n[kl7>&"%J]!"4/,*7HDm9Gs\Qo^oHL7kYgNN68JcE0Bk3AW5(l/iFT5bE/?+V7W*Mgo"7_\qE\\UKoHNNg7+IC`L'7&D^5cS.?%?@Un&eQXQ4el&J4p%T-Lb_le;qfG,@CY'OU1jTECQ+69&fs9t/npt4NC^dpb9DqJZE3V.aMN)&2?A;a3A%bL[S6gqUH5UnV"iIK$afrqn`8XPtXbU+iPTlU0FImr,rp5u`%'me#0.T?Pj*2f/+T=b=Q+'@],V*fmRM3`aa%NFA/YJp"W;?h$+5EOc4`C5lXR1kN-^0^GF?HDTXEB%'DWVO([b04nL.c1:SS<)jL.aX,U4:b\u)L05%h6l@Bm`sEM"JQf/fXP#kCAcol<%=lFRlq$B+Qt.Jumpm4b@E@9V$F9F*^#a8bfr$:O]NF:QI;=dI]#TkbdZ!l5V@b140dkX>PoO?DE@b13]f(I2T6&'fX?Hd_B^oZk@XQpFo9Sob+@6#D0A9#e_!X$s/Er/@T*+X?@=%C,QX)Kgs-hTlld9p)Q/H7^+C"#$moWm&FKK>rEgk(ja4"8f[Ai:Y2#KkP+4u]fd;@qJdNHbfSJdDo)o506[_Q@oL-GQlMUY<>()T[6`EW;QS=Acg.%rd7"k9(q0/hgtQ0B]e2#)FTL/I2U$J`=G+Y[_H"nI]3Z;@FWW='5!X;_!OM1rf&/`F+i!".!nht:XS[NNpAqR(Cq*`./^,QViUjGGfEg&;,?Tj5ZX1ZTCH`jVp>Icr>7sH9_YXqOr92!jBido*`n<@DnalBdPrc'E!lq`pNLD7pL:OEt$C+1KdB=f4XYM8ckr2VBHQ[Bs0TLK1h#kli>)9an>cObt1=\q9nUT-kJZ`0!ckR?d/Ms_3933KE&&hn*\#,Ba_0P],<;>1'`Gd"&fk`7pt.]bpPrS#3>FN*1/\X`D0bHP/O6P@G^$mX"jYs$0q9ofuBXNWDrT#+l;R:Ps>35+I,Y,i]mhJNE`>qJN?//GIsl9+7BA_b6[m&Djn[5qgM'cNj`@afJmo*u"dNF4=rbZ_,'XMmYBYh3JgG2>!g`J.O!$\Ge=#hEH0oNH&d4K[fF&0;e,>.n@qI3;Y`JRH_-^_1:PjPbP5B&0NbXpt=lq\8.71E\mDJSoO`C)[rh'+QX59OH6ZOm2p?Sa7(FAnd(_Y;^*@IfM&gR%7`b]ENn9,f"4(H45:DCn+SOJRc#S/\N=(f6r`Fh)iLk+!A2[`N8CE)I.1gc^$>`/g.c?^u`.^17hctN+\N#]o_((%V@j^XMJD1n\c\?K;_:pd&l7leDl,^4KMMSrL-DbR.\6;]2]_\h$)$6%=j-qVs`2`-&*ShL++/?')9ea*:)gOs#$]ZRoPP@34%4Mf*[o)Z2#(UL7\Z`[B,+6SdL<\A!t>aYYa7t"TXQurk*FUccPc_J4V/hlVY%(!)QZ_geF=to5_]``,s&A_Od"Dd["YiZ$snHVSKg0TW)/`b1C;88.Sbc,r&V=Yp@drG<>T0gp'_3nf.IXrmeS+R.Wjk-;7+<&:$r;t@aG4T/'.i_Uq2BN4UoFe7iL$Y\Ga!VT1UR,@_"(X>"g1lZV"Bul[XD*4OVIASk@EO;jlmmkOs&>0o54VR!OFD0.!J7E&CYXtB6/W3XYCBg2$V9NBLKB]2#1cEJ)rJo&9S#`eFi^8dqIq"->kEORTP/!CC/Qq(hVS-^u@!4:/l:-e3&`dmBV0^"d(2.*AGc&Y'I,B/fjsJ?mOCWhpE:WVYL]=$+@cr^dt[e?`?eXh0`o73[WS86A)4,##Pm>U)k+;\$0V\o!.FreVdT)!4-PEB&*X7\dabWHmnfja-G()2U&a=bdq-%DL,ktduUQ%>J4IrA&GD")eRn2bdn=Hh0b;mf0$4#DE_IZIDDr]J6DUIZgIp'nY*]sQUK)@pTEkkT1;stMH;>aDOr;NZoU&lkqE4[Q?%,M7Y\&4WZuM")p`^=L)o5e-OacS`.6;/G\dLuYn=(V7^*XI_62G1*%1uOeM4ro*uK1cAQ[DC3_HI5H3ncF"iILS?s&>H?nF;8*d(5oj092t`C@'I)o5]o"Db8V(>j0?*fl:gXdp'*q*;1H"2UE:LkYeJXm)*QR\pR/5EG@lM.:9=FdZDlq1ZlZSA`ffRhRAm[*UgV9"-7u'Sb.t:Au"tHT\@@`S21]o;pVn+BWECR!$)1\P$kmO.YfeaE2^IYU_Vd?1!teQOa475!Tl2qsF!?fO^:PT49YDqj:'ek+-M+B([17jLG&WELpQ.\a#a2SR?*BnssbAZRLh`hAPJ_<^2"S/i:$\WpLqrb6e]b*ZARucnK7/@0muYkgBmb.4;3O1$bN!Fpebc()iKHTuhCRk)?2MT8//(QV4PU/g8'm8eV9:A^+N)DK7IIEj5b6g[FI=.!$PH:CfqN",(P-5h$c5PJm1_[,fb`Iqudqf<6DPZb2VT[3@F+F,m2%h[?"m[_I#f1$j/Rg1&_1?9*bnHacHj-(#.i(UnHL.1_DG/Mr'eclhYJ&]Iped]u>%[qMgNK!irJlZ.hk9s42na-l!8QXHrH#hE_lZ5so]ljBk0?[Ou@BPIiqM1qmB,mlN/g(1[Qa\f)BL:+MsFu:t.2%q+u/c_."@/02"iH`0hVRBiu36dP!I!KVAC*`W3i8H.tbD86&+pm4*Eb>K!iB"f&U:`dlRW@3-Wi%=`]p;>q]_I]kr?oFuOn*l`jmLA&GCg#2(T:g;/c]>AT]"fJ)W;+hQtj(#8HIO#t1(CmanJ0^JCd=n_s$plLPC$1UP#Yrt9pUU?_LHr;@jm+o:..iaU,MrXVJpH9GugI/aS%E!@gELi:Km#]f";fdPM6([@',6Um3Z9cfL*`#CHTb4A[L09j9Pl&VJ;g/S=5EN!V$.atS\k#Vb5bP1#0^;h]WCpu%f/O8O^"kHF`C7#3EXC;A,Ka;ckm,Dl!.>67?nF3e[Eo31l5-dE"(6sQ7QnL!23I6Qk0j4k+eq0"hD6Fl08'>n0?Gta::!cu?LAd$lU;([3hI9/a(?2J-(Qn)XFsmNTNLeo%X94j`Q#,T"2VR'%WC$A%0UGb0^BVq]FE3@"2VPQU0Q0?cYbddEn5=2nIo$Tbt5gB3Q4j$?C'1A(WN-/U@O]V!kk'.Pg*<)d9d;Y!.aPU+QG4\phDSZ-T%th(F::].iPcM/nqfSN\#LAo+T(@*Mo-2&chPCr!.,5a8lscl!WjZ"Gs+XG'-J()gdJ1nrb?RSGt2eiX>"@',.iencI2ZN7CM"(6rrJGn<+X(A1+G-U;t1jPD/Y&7FiSQ+c7J\&O"qL"1&j!sVSUtDj81<8Yh6`"b'hCp3.98\.'OOs13kH.,f)8VaCJpeBtEE[JRIFSG9RU625C'jT6gX6ha)>U+p@'(^dXJmMu=)ACJ#05u1FpZ_6&F?G?&!3W5o8@!N/]!mb"Db,o@*Jkj&kC&JHR5P6ollm7cemIkoa]XR8OFiA)(Z]KO*";^`MS%IAfq<&or9fgD!>"-M.>kaUnr%3fheIRJAoSE)M+=[Ji*j;ROhQKZuQK(b.Y"@4g+ul<=l63"i6X6Ji14r?NI5^Y,WU@$]SOG1kN-^#hD6Gom?+NcPN]jA__^9&_)l3_L0-D9J$*g_44'+!/=%C7[R.+e)lGqgD-HT/X*FU^aLVBO:OI$/*B21d^!"M:6I!k#&DXP3_WB$-G^L!mfW2<4S7qbZk44pO[e)4NJEk@#RedGBYN@.hnGSVIf`>n[k+#f17bL-%%9TjcX$;?"X2/nSBA_4"Uq_icN[6]_l8CBCgpQXEN?Tjc'.EF_7Wa54lCej:T`X#8+IBLKB][+rr2MmcM,C5&Md2?7TC&ZkdsYiZa6^"Esu\H#`@Uo!8;DMe+MTQfYkl_&t"TW=i8Ep2UOT1J'D4Y)6;I\kC>[*:,bX,9/4\;%_s#]uq*M=E'c7Jj_A(0?4m(E,*g/;\33BMu?rB=o8TOhFbFJdD#,uOr30l`&46Ht&nhgHtV.Y02U"DS:>9g-+'Yce2@`"M8H9;!V4*gUHN&CFkkG,Ze7V3XA@X`OZIT2.9rCC,*>-.&R3'&\Y)5V4JM#Y-s`7:gU,Yfd%K>n.G0irB*4#7ucZ'u\!oSZ\mAnZFA>49j4)]%nVHl(3s6XEn0]Rh2Z[lD#]bA69`#c'C@jL@nh:QnYU`pf4X#0s(86jH\;WR>>*$AmGLN?%_"+G><$i<<5t#$&H/pLq6X#40KV[agtiHFU=Em%D!8*cDd#O0S20q"D`;if(J7:$UD=S69e&W!e?$baNgkGj.Rm,^7]c_GC/JD]729BsIm!aAcSm1&IZ.J.(q',nC@MB?O6pJeHSlPJ4ha#;=EiO?dDe`^oSJEYU5^`@![I^7fni!/B8%I@`,9+*.g$i#hFAGoI_4B!GP!3d(_aum.,^8bm/u/4tu?o!O5=MJG;b'R^VMK4QG$J=.+S/l)WqE,%Kd,3%Pb0EoP_$_s3CF[`Pbb(60Eob,o]c=;^^;F_(G&%B[ZMcltU#=CO-<[0M;kQ;4[H"(6u'Y-#>Lots^RP+[d&1D!Q8?i":r%m!"fc'H>kaKaI$(:)_h>Dk8W:>K,R-oONM>?&.R*ugB78W>,Uk9GOdQNX>D,sdShK63Td]+k.RbHNUc.!Ne`'B1.(^tWfVf&U;kjV$Z-\Q,MLUgXgW!P$YPqhrZ`_6JKrMdWY5&?H%)Yh$lMlRi1'3!M,..oDLbLM)gopc8U6P7.N4PNFNKjJdJ!A4DS2dei@f0Pm$2?]UO.CmX!cfoMUthPbhGck4m+nH>?4V==\Xf;8!?>N:(P*l0bJL0S's"RA5[q,9>&XP1h9XI%\jQ&Z7BRqJ+2%@D8I/R7DIq0CG>=kA1,f21PE3UUS;1gnll@+I2GH-hUVO12QX=kAR7DsNe`[Z4Mb!O2>+Md2Ue4)OkBqMk@EYpte2dWV$Eb'UII$$<[qD5UJVeA]8/pFpi4XH(sAL[&]O7ZK>FL:h&X$'&6lJXQ^n2$s4A+3[7u:m4/K-DqkWnb@k"(oie[`=XTo0?qg`%ma@b`%F[$>A`P0YF`P+N$QMA0HrUqhJ9:rk?4o@DQ&2c8HmOQK/.@,%$egS5NPTNb=lW;]/D[\/e>0%[FUXZq/2.BTUW`&GR@blef64O-Pm/qGX_(cAQdWM['r2fhYACJ=i)/c_5Q!h/b[9/B1MkGq%%.K?foY?[SV=>"?qg`%Y%d)q2Ao*hB$E,d28+YIe(a:Y3L!d)Y(f<*X5+>cKnZ@,<:rft7"p=tQU$dN)Tn8tLsgfVo9MYd>_9?f7^-J,@%!/5GU959M62"%2T+-YY7rV&$TeRK2KI[H)7.PeFhNK;jcpL$T-M6GN=QZPpPpF+/!"A'M.=ebQ>\@6$Q(`70Y0B^Ufi4FEr_tI5Y[;O-7ja+[018c$.ea7n5KYO/V0?qJi1(ACN@mKrkOakYZ>XX=$,jqonH,DJuAfW\\C:"NC++%`qb#2)]Q0%VuEU?6GUDY"[NXr"BJ?!u7L!jh:s?stF`q=7jK\mRUkS4`D9fn*?!nq#.OH>.=XBA\Lsbj/R8@p&Pt8XJ`[2WlU3"G:0A?@Kth,eQ1_?a&\?CSehk6;?L4(X`cm>#d:%!G[$i$g9=p)[Ff"eN5#)6Eg&ZirphqnK!/$ISBG/_J*.lN-1X-c2>%>Z;cH0TB#M]Ac6W0PS\[VV&i9ZY9($>os^Y0\#-KuU>I1e-m?9kJO:l\MPuL4)F!cm>#d9lAeFE[9=q2(iHTB_GSe<]!9h"dhJ^q*%*@<933L`BWpK:e!1ohJ_>)_c)p7M75*\db)4]-$e)\IFr!cnOdZ@Fdc_W/%8'^lLEjC[?FpcVYTW1-jU"-,Cggc'lP?hp!G:-jbaJn>`>?U]\eOGH<9d7L=MMRihAaDnZVPrAo9YD_K/gndZE*l"+bH9Z%WRBnHGQfA/Bk>E_f*kk#5=g*$J=sj?Ln3(*ZJY!fJ)Wr@1=8^ePs&VDK@'pHBdQQ19"?NFJ*3YGGm0=<1c#OXX<9f\ut/f6rbTlZk;Yjo]/'fJ)Wr@Fer+H9F07CE@QJD<4*-.lZ:J$-ie"\d$5n)Do4bER;>J)eQe^6?^h9bB:LeL[I.eh=P1-og3i&$dDM&muh(aLe&hrS`$JscY'oZ:>N*n4oJmG&^E\4YWp+T]YM"F.=8M^@0(-X!Fn4/SnMca>'n_S*\oRK/eRHTsj4$Q'ZOH8QlFjL$5aZDt7F*#fu0nI>beRoa(U8_Gp0qTT[\702H:b2NmpQERPBZ6%[p%sS4mJ#MohDrK?$,EXm!5krh@+_pu(j)026tS&bDgFTcl=pWKW?oof(8J$545P5=j(WXj=d))bCD+","PYh/!bT.!&?Jk7_6/YVP+HBdiY%,MH29(cA]<26duUXZ;$7e$c,=YSJ(lY%C%oAcff;$\2gZU:3YOHdq-@i-q7/WDul#kmT$N*GqC:8F=!LYArpU"D=98Vri8Oc:9"c"\hWK3B$/NTn-Z3+8YSKJ<9h/=+JXl5^U7U0SVl:PCUi!%H[QR_DDJ@\mEHqmUFaW_\tYGr-j!%b(]1_DL0'H0qMh)+!Ae=^Q@:L`N8q:=IaYOoHdAS?1#>$5mmCtQUB$1\&Aan'UIboHVMbt\&Ao[RX&h0+]pVnhK4?P]`>"ggL:Fh;i0bI(d`i/+QMZqZh@9/\J6Rrq/Pi3g]PW8*amRK@L4Bj:JDFN1r1FKm#lnH9g[HRo&hfBX#4A;-/`6<6IRLUa/@H6@+HA)K!iUKfrZSTCGl/,n"S1Sj\],k)Ni4t\U5DT)!sW]PsCuhTu^GBGF:7P7^+5+@:-R(KI'^k?FOFUB5`h6CN:r=pPp@p?.Z@QFk)\@lk.ui4(KqZV=Vo6%pqV&TS4j!SL[e?Ar'Mn90g$2/c+ko0%4(O*oH&(`;cYendstream +endobj +xref +0 9 +0000000000 65535 f +0000000073 00000 n +0000000114 00000 n +0000000221 00000 n +0000000330 00000 n +0000000533 00000 n +0000000601 00000 n +0000000936 00000 n +0000000995 00000 n +trailer +<< +/ID +[<01634a2080dbacde20dfa4977d401202><01634a2080dbacde20dfa4977d401202>] +% ReportLab generated PDF document -- digest (http://www.reportlab.com) + +/Info 6 0 R +/Root 5 0 R +/Size 9 +>> +startxref +12607 +%%EOF diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json new file mode 100644 index 0000000..987bc24 --- /dev/null +++ b/scripts/robot_1781069752019.json @@ -0,0 +1,433 @@ +{ + "coordinateSystem": {"handedness": "right", "x": "right", "y": "backward", "z": "up"}, + "units": {"length": "mm", "rotation": "degree"}, + "vision_config": {"MarkerType": "DICT_4X4_250", "MarkerSize": 0.025}, + "renderingInfo": { + "width": 1280, + "height": 720, + "renderDefaults": {"width": 1280, "height": 720, "dofFStop": 11}, + "cameraPosition__1": [-10, -800, 500], + "cameraPosition__2": [-500, 300, 1200], + "cameraPosition__3": [-200, -900, 200], + "cameraPosition__4": [1200, 200, 300], + "cameraPosition_a": [-300, -800, 500], + "cameraPosition": [-200, 200, 1400], + "cameraPosition_c": [600, -500, 600], + "cameraTarget": [200, -200, 180], + "cameraUpVector": [0, 0, 1], + "lightPosition": [-500, -500, 500], + "lightTarget": [0, 0, 0], + "lightUpVector": [0, 0, 1], + "metric": "mm", + "showSkeleton": true, + "showMarkers": true, + "backgroundColor": [0.7, 0.85, 1.0], + "backgroundStrength": 0.2, + "sunEnergy": 0.35, + "areaEnergy": 120, + "exposure": -1.5, + "lensDirt": true, + "lensDirtStrength": 0.08, + "dofEnabled": true, + "dofFStop": 11.0, + "arucoDust": true, + "arucoDustStrength": 1.6, + "markerOffsetMaxMm": 4.0, + "markerOffsetSeed": 0, + "markerRotationMaxDeg": 3, + "motionBlur": true, + "motionBlurMaxPx": 5.5, + "focalErrorPct": 0.5, + "principalErrorPx": 3.0, + "residualDistortion": [0.02, -0.01], + "localizedBlur": false, + "localizedBlurStrength": 0.15, + "vignette": true, + "vignetteStrength": 0.08, + "sensorNoise": true, + "sensorNoiseStrength": 0.01, + "lensDistortion": true, + "lensDistortionStrength": 0.002, + "materials": { + "wood": {"baseColor": [0.72, 0.52, 0.33], "roughness": 0.85, "metallic": 0.0}, + "plaWhite": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.45, "metallic": 0.0}, + "steel": {"baseColor": [0.72, 0.72, 0.75], "roughness": 0.25, "metallic": 1.0}, + "powderCoatBlue": {"baseColor": [0.15, 0.25, 0.7], "roughness": 0.55, "metallic": 0.0}, + "defaultPlastic": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.4, "metallic": 0.0}, + "skeletonRed": {"baseColor": [0.85, 0.2, 0.2], "roughness": 0.35, "metallic": 0.0}, + "markerBlack": {"baseColor": [0.04, 0.04, 0.04], "roughness": 0.8, "metallic": 0.0} + }, + "skeletonDefaults": {"radius": 4, "color": [0.85, 0.2, 0.2]}, + "markerDefaults": {"size": 25, "thickness": 1, "color": [0.04, 0.04, 0.04]}, + "defaultPosition": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3} + }, + "defaultPosition__": {"x": 10, "y": 4, "z": 20, "a": 10, "b": 2, "c": 9, "e": 1}, + "defaultPosition": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5}, + "recognized": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null}, + "constraint_rules": { + "rigid_distance": {"enabled": true, "mode": "mst", "weight": 1.0}, + "joint_axis_projection": {"enabled": true, "max_pairs": 2, "weight": 0.35}, + "chain_axis_projection": {"enabled": false, "max_depth": 3, "max_pairs": 2, "weight": 0.15}, + "axis_alignment_threshold": 0.95 + }, + "observation_weighting": {"enabled": true, "distance_weight": true, "marker_size_weight": true, "view_angle_weight": true}, + "multiview_calculation": { + "combine_mode": "mean", + "size_ref_px": 50.0, + "border_ref_px": 120.0, + "center_ref_norm": 0.01, + "sharpness_ref": 2500.0, + "homography_ref": 0.18, + "size_factor": 0.3, + "aspect_factor": 0.3, + "border_factor": 0.01, + "center_factor": 0.01, + "sharpness_factor": 0.5, + "homography_factor": 0.2, + "normal_visibility_factor": 0.01, + "spin_factor": 0.3, + "weight_floor": 0.3 + }, + "pose_estimation": { + "method": "hybrid", + "marker_observation": "corner_pose", + "use_normals": true, + "normal_weight": 100.0, + "robust_loss": "huber", + "huber_delta_mm": 8.0, + "max_iterations": 200, + "min_cameras_per_marker": 2, + "finger_block_joints": ["b", "c", "e"], + "per_link_method": {} + }, + "robot_test_poses": { + "4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20}, + "5": {"x": 180, "y": 86, "z": -120, "a": -60, "b": 22, "c": 91, "e": 10}, + "6": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}, + "7": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9}, + "8": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3}, + "9": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8}, + "9a": { + "x": 60, + "y": -2, + "z": 95, + "a": 200, + "b": 60, + "c": 9, + "e": 8, + "rendering": {"width": 1440, "height": 1080, "dofFStop": 11} + }, + "9b": { + "x": 60, + "y": -2, + "z": 95, + "a": 200, + "b": 60, + "c": 9, + "e": 8, + "rendering": {"width": 4896, "height": 3264, "dofFStop": 5.6} + }, + "10": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4}, + "11": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5}, + "12": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6} + }, + "test_camera_positions": { + "a": [-300, -800, 800], + "b": [300, -900, 1200], + "c": [300, -900, 400], + "d": [700, -800, 400], + "e": [1200, -900, 400], + "f": [500, -300, 1400], + "g": [-200, 200, 1400] + }, + "test_camera_targets": { + "a": [210, -100, 180], + "b": [310, -80, 180], + "c": [210, -100, 150], + "d": [210, -100, 150], + "e": [210, -100, 50], + "f": [200, -200, 180], + "g": [200, -200, 180] + }, + "movements": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null}, + "state_pose_params": { + "numbers_of_Elements_to_consider_start": 3, + "numbers_of_Elements_to_consider_final": 5, + "solver_in_between_geometrical": false, + "solver_after_geometrical": false, + "geometric_passes_per_stage": 2, + "revolute_search_coarse_deg": 5.0, + "revolute_search_fine_deg": 1.0, + "root_pose_min_markers": 3, + "use_marker_normals_flip_tiebreak": true, + "normal_flip_weight": 0.05 + }, + "links": { + "Board": { + "parent": null, + "size": [1000, 200, 25], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "skeleton": {"from": [0, 0, 16], "to": [1000, 0, 16], "radius": 4, "color": [0.85, 0.2, 0.2]}, + "markers": [ + { + "id": 46, + "set": "A0", + "position": [536.71, 185.44, -27.3], + "normal": [0, 0, 1], + "spin": 90, + "info": "is placed on a white paper, A0_60Arucos_25mm_Seet223.pdf, with the following marker placements:" + }, + {"id": 47, "set": "A0", "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 48, "set": "A0", "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 49, "set": "A0", "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 50, "set": "A0", "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 51, "set": "A0", "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 52, "set": "A0", "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 53, "set": "A0", "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 54, "set": "A0", "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 55, "set": "A0", "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 56, "set": "A0", "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 57, "set": "A0", "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 58, "set": "A0", "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 59, "set": "A0", "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 60, "set": "A0", "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 61, "set": "A0", "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 62, "set": "A0", "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 63, "set": "A0", "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 64, "set": "A0", "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 65, "set": "A0", "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 66, "set": "A0", "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 67, "set": "A0", "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 68, "set": "A0", "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 69, "set": "A0", "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 70, "set": "A0", "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 71, "set": "A0", "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 72, "set": "A0", "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 73, "set": "A0", "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 74, "set": "A0", "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 75, "set": "A0", "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 76, "set": "A0", "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 77, "set": "A0", "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 78, "set": "A0", "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 79, "set": "A0", "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 80, "set": "A0", "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 81, "set": "A0", "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 82, "set": "A0", "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 83, "set": "A0", "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 84, "set": "A0", "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 85, "set": "A0", "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 86, "set": "A0", "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 87, "set": "A0", "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 88, "set": "A0", "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 89, "set": "A0", "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 90, "set": "A0", "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 91, "set": "A0", "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 92, "set": "A0", "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 93, "set": "A0", "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 94, "set": "A0", "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 95, "set": "A0", "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 96, "set": "A0", "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 97, "set": "A0", "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 98, "set": "A0", "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 99, "set": "A0", "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 100, "set": "A0", "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 101, "set": "A0", "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 102, "set": "A0", "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 103, "set": "A0", "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 104, "set": "A0", "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90} + ], + "model": [ + { + "stlFile": "surfaces/Board.stl", + "originOfModel": [0, 0, 0], + "rotationOfModelDegree": [0, 0, -90], + "material": "wood" + }, + { + "stlFile": "surfaces/BoardRail.stl", + "originOfModel": [0, 0, 0], + "rotationOfModelDegree": [0, 0, -90], + "material": "steel" + } + ] + }, + "Base": { + "parent": "Board", + "size": [150, 200, 150], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Slider", + "type": "linear", + "axis": [1, 0, 0], + "origin": [0, 0, 16], + "rotation": [0, 0, 0], + "variable": "x" + }, + "skeleton": {"from": [0, 108, 45], "to": [110, 108, 45], "radius": 4, "color": [0.2, 0.8, 0.2]}, + "markers": [], + "model": [ + { + "stlFile": "surfaces/Base.stl", + "originOfModel": [-30, 0, -35], + "rotationOfModelDegree": [0, 0, 0], + "material": "plaWhite" + } + ] + }, + "Arm1": { + "parent": "Base", + "size": [70, 250, 70], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Joint1", + "type": "revolute", + "axis": [-1, 0, 0], + "origin": [110, 108, 45], + "rotation": [0, 0, 0], + "variable": "y" + }, + "skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.2, 0.2, 0.9]}, + "markers": [ ], + "model": [ + { + "stlFile": "surfaces/Holm.stl", + "originOfModel__": [-25, 29, -28.5], + "originOfModel": [-29, 25, 28.5], + "rotationOfModelDegree__": [0, 0, 0], + "rotationOfModelDegree": [180, 0, -90], + "material": "powderCoatBlue" + } + ] + }, + "Ellbow": { + "parent": "Arm1", + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Joint2", + "type": "revolute", + "axis": [-1, 0, 0], + "origin": [0, -250, 0], + "rotation": [0, 0, 0], + "variable": "z" + }, + "skeleton": {"from": [0, 0, 0], "to": [90, 0, 0], "radius": 4, "color": [0.9, 0.2, 0.2]}, + "model": [ + { + "stlFile": "surfaces/Ellebogen.stl", + "originOfModel": [90, 0, 0], + "rotationOfModelDegree": [0, -90, -90], + "material": "defaultPlastic" + } + ], + "markers": [ + ] + }, + "Arm2": { + "parent": "Ellbow", + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Joint3", + "type": "revolute", + "axis": [0, -1, 0], + "origin": [90, 0, 0], + "rotation": [0, 0, 0], + "variable": "a" + }, + "skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.95, 0.85, 0.2]}, + "model": [ + { + "stlFile": "surfaces/Unterarm.stl", + "originOfModel": [0, -250, 0], + "rotationOfModelDegree": [180, 0, -90], + "material": "defaultPlastic" + } + ], + "markers": [ + ] + }, + "Hand": { + "parent": "Arm2", + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Joint4", + "type": "revolute", + "axis": [1, 0, 0], + "origin": [0, -250, 0], + "rotation": [0, 0, 0], + "variable": "b" + }, + "skeleton": {"from": [0, 0, 0], "to": [0, -35, 0], "radius": 4, "color": [0.95, 0.55, 0.15]} + }, + "Palm": { + "parent": "Hand", + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Joint3", + "type": "revolute", + "axis": [0, -1, 0], + "origin": [0, 0, 0], + "rotation": [0, 0, 0], + "variable": "c" + }, + "skeleton": {"from": [-50, -35, 0], "to": [50, -35, 0], "radius": 7, "color": [0.95, 0.2, 0.2]} + }, + "FingerA": { + "parent": "Palm", + "size": [80, 60, 20], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Slider", + "type": "linear", + "axis": [1, 0, 0], + "origin": [4, -35, 0], + "rotation": [0, 0, 0], + "variable": "e" + }, + "skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]}, + "markers": [ + ], + "model": [ + { + "stlFile": "surfaces/Finger.stl", + "originOfModel": [24, 0, -9.1], + "rotationOfModelDegree": [90, -90, 0], + "material": "defaultPlastic" + } + ] + }, + "FingerB": { + "parent": "Palm", + "size": [80, 60, 20], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Slider", + "type": "linear", + "axis": [-1, 0, 0], + "origin": [-4, -35, 0], + "rotation": [0, 0, 0], + "variable": "e" + }, + "skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]}, + "markers": [ + ], + "model": [ + { + "stlFile": "surfaces/Finger.stl", + "originOfModel": [-24, 0, 9.1], + "rotationOfModelDegree": [90, 90, 0], + "material": "defaultPlastic" + } + ] + } + } +} diff --git a/server/server.js b/server/server.js index 0ea7090..9215907 100755 --- a/server/server.js +++ b/server/server.js @@ -260,7 +260,14 @@ async function capturePhotos(sessionName) { const savedFiles = []; for (const camId of cameraIds) { - const response = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true); + let response; + // Bei 503 (Kamera kurz busy nach Hires-Grab) einmal nach 2 s neu versuchen + for (let attempt = 1; attempt <= 2; attempt++) { + response = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true); + if (response.status !== 503) break; + if (attempt < 2) await new Promise(r => setTimeout(r, 2000)); + } + if (!response.ok) throw new Error(`getSnapshot(${camId}): HTTP ${response.status}`); const buffer = Buffer.from(await response.arrayBuffer()); const filename = `${camId}_${setNr}.jpg`; await fsPromises.writeFile(path.join(sessionDir, filename), buffer); @@ -371,44 +378,15 @@ app.post('/api/calibration/compute', async (req, res) => { send({ type: 'log', text: `▶ Script: ${calibScriptPath}` }); send({ type: 'log', text: '' }); - // -u = unbuffered (Python gibt jede Zeile sofort aus) - const proc = spawn(PYTHON_BIN, [ - '-u', + const exitCode = await runScript([ calibScriptPath, '--camera', camera, '--input-dir', sessionDir, '--output-dir', sessionDir, - ]); + ], send); - let stdoutBuf = ''; - proc.stdout.on('data', (chunk) => { - stdoutBuf += chunk.toString(); - const lines = stdoutBuf.split('\n'); - stdoutBuf = lines.pop(); - for (const line of lines) send({ type: 'log', text: line }); - }); - - let stderrBuf = ''; - proc.stderr.on('data', (chunk) => { - stderrBuf += chunk.toString(); - const lines = stderrBuf.split('\n'); - stderrBuf = lines.pop(); - for (const line of lines) send({ type: 'log', text: `[stderr] ${line}` }); - }); - - proc.on('error', (err) => { - console.error('calibration/compute spawn error:', err); - send({ type: 'log', text: `Fehler beim Starten: ${err.message}` }); - send({ type: 'done', exitCode: -1 }); - if (!res.writableEnded) res.end(); - }); - - proc.on('close', (code) => { - if (stdoutBuf) send({ type: 'log', text: stdoutBuf }); - if (stderrBuf) send({ type: 'log', text: `[stderr] ${stderrBuf}` }); - send({ type: 'done', exitCode: code ?? -1 }); - if (!res.writableEnded) res.end(); - }); + send({ type: 'done', exitCode }); + if (!res.writableEnded) res.end(); } catch (err) { // Fehler VOR flushHeaders → normaler JSON-Fehler @@ -426,6 +404,174 @@ app.post('/api/calibration/compute', async (req, res) => { } }); +// ── Board-Erkennung ─────────────────────────────────────────────────────────── + +const boardDataDir = path.join(__dirname, '..', 'data', 'board'); +const ROBOT_JSON = process.env.ROBOT_JSON + || path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json'); +const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py'); +const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py'); + +/** + * Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter. + * Gibt den Exit-Code zurück (Promise). + */ +function runScript(args, send) { + return new Promise((resolve) => { + const proc = spawn(PYTHON_BIN, ['-u', ...args]); + + let outBuf = ''; + proc.stdout.on('data', chunk => { + outBuf += chunk.toString(); + const lines = outBuf.split('\n'); + outBuf = lines.pop(); + for (const line of lines) send({ type: 'log', text: line }); + }); + + let errBuf = ''; + proc.stderr.on('data', chunk => { + errBuf += chunk.toString(); + const lines = errBuf.split('\n'); + errBuf = lines.pop(); + for (const line of lines) send({ type: 'log', text: `[stderr] ${line}` }); + }); + + proc.on('error', err => { + send({ type: 'log', text: `Fehler beim Starten: ${err.message}` }); + resolve(-1); + }); + + proc.on('close', code => { + if (outBuf) send({ type: 'log', text: outBuf }); + if (errBuf) send({ type: 'log', text: `[stderr] ${errBuf}` }); + resolve(code ?? -1); + }); + }); +} + +/** + * POST /api/board/run + * 1. Erstellt data/board/{timestamp}/ + * 2. Holt Snapshot jeder Kamera + * 3. Für jede Kamera: Script 1 (ArUco-Erkennung) → Script 2 (Kamera-Pose) + * SSE-Stream während der Ausführung. + */ +app.post('/api/board/run', async (req, res) => { + try { + res.setHeader('Content-Type', 'text/event-stream'); + res.setHeader('Cache-Control', 'no-cache'); + res.setHeader('Connection', 'keep-alive'); + res.flushHeaders(); + + const send = (obj) => { + if (!res.writableEnded) res.write(`data: ${JSON.stringify(obj)}\n\n`); + }; + + // 1. Temp-Verzeichnis + const ts = makeTimestamp(); + const runDir = path.join(boardDataDir, ts); + await fsPromises.mkdir(runDir, { recursive: true }); + send({ type: 'log', text: `▶ Board-Run: ${ts}` }); + send({ type: 'log', text: `▶ Ordner: ${runDir}` }); + send({ type: 'log', text: `▶ Robot-JSON: ${ROBOT_JSON}` }); + send({ type: 'log', text: '' }); + + // 2. Kameras ermitteln + if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert'); + const camData = await new WebcamClient(WEBCAM_URL).getCameras(); + const cameraIds = (camData.cameras ?? []).map(c => c.id); + send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` }); + + // 3. Aktuelle Kalibrierungs-Session für NPZ-Dateien + const calibSession = await findLatestCalibSession(); + if (!calibSession) throw new Error('Keine Kalibrierungs-Session. Bitte zuerst Camera NPZ kalibrieren.'); + send({ type: 'log', text: `▶ NPZ-Session: ${calibSession}` }); + send({ type: 'log', text: '' }); + + // 4. Pro Kamera: Foto → Script 1 → Script 2 + for (const camId of cameraIds) { + send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` }); + + // Snapshot + send({ type: 'log', text: 'Foto aufnehmen …' }); + let snapResp; + for (let attempt = 1; attempt <= 2; attempt++) { + snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true); + if (snapResp.status !== 503) break; + if (attempt < 2) await new Promise(r => setTimeout(r, 2000)); + } + if (!snapResp.ok) { + send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` }); + continue; + } + const imgPath = path.join(runDir, `${camId}.jpg`); + await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer())); + send({ type: 'log', text: `✅ Foto: ${camId}.jpg` }); + + // NPZ prüfen + const npzPath = path.join(calibDataDir, calibSession, `${camId}_calibration.npz`); + try { await fsPromises.access(npzPath); } + catch { + send({ type: 'log', text: `⚠ Keine NPZ (${camId}_calibration.npz) – übersprungen` }); + continue; + } + + // Script 1 – ArUco-Erkennung + send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' }); + const exit1 = await runScript([ + SCRIPT_1, + '-i', imgPath, + '-npz', npzPath, + '-robot', ROBOT_JSON, + '-cameraId', camId, + '-outDir', runDir, + '--saveDebugImage', + ], send); + if (exit1 !== 0) { + send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` }); + continue; + } + + // Script 2 – Kamera-Pose schätzen + const detJson = path.join(runDir, `${camId}_aruco_detection.json`); + try { await fsPromises.access(detJson); } + catch { + send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' }); + continue; + } + + send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' }); + const exit2 = await runScript([ + SCRIPT_2, + '-i', detJson, + '-robot', ROBOT_JSON, + '-outDir', runDir, + ], send); + if (exit2 !== 0) { + send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` }); + } + + send({ type: 'log', text: '' }); + } + + send({ type: 'log', text: `✅ Board-Run abgeschlossen: ${ts}` }); + send({ type: 'done', exitCode: 0, runDir: ts }); + if (!res.writableEnded) res.end(); + + } catch (err) { + console.error('board/run error:', err); + if (!res.headersSent) { + res.status(500).json({ error: String(err) }); + } else { + try { + res.write(`data: ${JSON.stringify({ type: 'log', text: `❌ ${err.message}` })}\n\n`); + res.write(`data: ${JSON.stringify({ type: 'done', exitCode: -1 })}\n\n`); + res.end(); + } catch {} + } + } +}); + /** * POST /api/calibration/upload-npz * Liest {camera}_calibration.npz aus der aktuellen Session und