Callibration Board 0

public/calibration.html

@@ -253,19 +253,22 @@
         <div class="sections">
 
           <div class="section full">
-            <h2>Board <span class="status-badge open">offen</span></h2>
+            <h2>Board – ArUco &amp; Kamera-Pose</h2>
-            <div class="placeholder-note">
+            <div class="info-grid" style="margin-top: 14px;">
-              Ziel: Extrinsische Position des Marker-Boards im Kamera-Koordinatensystem bestimmen
+              <span class="info-label">Ablauf</span>
-              (Rotations- und Translationsvektor via <code>solvePnP</code>).<br><br>
+              <span class="info-value" style="font-family: inherit; font-size: 13px; color: var(--muted);">
-              Geplante Aktionen: Kamerabild mit erkannten Markern anzeigen · Pose berechnen ·
+                Foto aufnehmen → ArUco erkennen → Kamera-Pose schätzen
-              Kalibrierungsdatei speichern.<br><br>
+              </span>
-              <em>Aktionen werden ergänzt sobald das Konzept feststeht.</em>
+              <span class="info-label">Schritte</span>
+              <span class="info-value" style="font-family: inherit; font-size: 13px; color: var(--muted);">
+                1_detect_aruco_observations &nbsp;→&nbsp; 2_estimate_camera_from_observations
+              </span>
+              <span class="info-label">Letzter Run</span>
+              <span class="info-value" id="board-last-run">–</span>
             </div>
-            <div class="controls" style="margin-top: 14px;">
+            <div class="controls" style="margin-top: 16px;">
-              <button disabled>Foto aufnehmen</button>
+              <button id="btn-board-run">Board erkennen</button>
-              <button disabled>Marker erkennen</button>
+              <button disabled title="Folgt später">Ergebnis anzeigen</button>
-              <button disabled>Pose berechnen</button>
-              <button disabled>Board-Pose speichern</button>
             </div>
           </div>
 
@@ -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;
+      }
+    });
   </script>
 
 </body>

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

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)

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

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:
+  <evalDir>/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 <evalDir>/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()

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}
+  ]
+}

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

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 exitCode = await runScript([
-    const proc = spawn(PYTHON_BIN, [
-      '-u',
       calibScriptPath,
       '--camera',     camera,
       '--input-dir',  sessionDir,
       '--output-dir', sessionDir,
-    ]);
+    ], send);
 
-    let stdoutBuf = '';
+    send({ type: 'done', exitCode });
-    proc.stdout.on('data', (chunk) => {
+    if (!res.writableEnded) res.end();
-      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();
-    });
 
   } 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<number>).
+ */
+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