docker probleme

README.md

@@ -116,22 +116,13 @@ print(resp.json()["joints"])
 
 ## Deployment (Docker / Portainer)
 
-> **Pflichtschritt zuerst:** `config/robot.json` muss als **Datei** existieren, bevor
+Die `config/robot.json` wird beim Build **mit ins Image gebacken** (`COPY . .`) und
-> der Container startet. Vorlage kopieren und mit der echten Konfiguration füllen:
+liegt dort fest unter `/app/config/robot.json`. Damit läuft der Container
->
+out-of-the-box — **kein Bind-Mount nötig**, auch nicht als Portainer-Stack.
-> ```bash
-> cp config/robot.json.example config/robot.json
-> ```
->
-> ⚠️ Fehlt die Datei, legt Docker am Mount-Pfad ein **leeres Verzeichnis** an.
-> Der Server startet dann zwar, aber jeder `/v1/estimate` liefert **500**
-> (`IsADirectoryError`) und `/v1/config` ebenfalls 500. Genau dann diesen
-> Pflichtschritt nachholen und den Container neu starten.
 
-**Volume** (Pfad muss zur tatsächlichen Datei zeigen, vgl. `docker-compose.yaml`):
+> Voraussetzung: `config/robot.json` muss **zum Build-Zeitpunkt** als Datei
-```yaml
+> vorhanden sein. Falls nicht: `cp config/robot.json.example config/robot.json`
-- ./config/robot.json:/config/robot.json:ro
+> und mit der echten Konfiguration füllen.
-```
 
 **Start & Healthcheck:**
 ```bash
@@ -139,6 +130,15 @@ docker compose up -d
 curl http://<host>:8446/v1/health     # {"status":"ok","version":"1.0.0"}
 ```
 
+**Eigene robot.json ohne Rebuild** (optional): die eingebackene Kopie per Mount
+überlagern — siehe auskommentierter `volumes`-Block in `docker-compose.yaml`.
+
+> ⚠️ **Portainer-Falle:** Ein Bind-Mount auf einen Host-Pfad, der **nicht
+> existiert**, lässt Docker dort ein leeres **Verzeichnis** anlegen. Die Pipeline
+> liest es dann als Datei → `IsADirectoryError` → jeder `/v1/estimate` und
+> `/v1/config` antwortet mit **500**. Deshalb ist der Mount jetzt optional;
+> aktiviere ihn nur, wenn die Datei am Host-Pfad wirklich liegt.
+
 ---
 
 ## Konfiguration

approbot_pipeline.egg-info/PKG-INFO

@@ -0,0 +1,183 @@
+Metadata-Version: 2.4
+Name: approbot-pipeline
+Version: 1.0.0
+Summary: Robot pose estimation from multi-camera ArUco images
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+Requires-Dist: numpy==1.26.4
+Requires-Dist: scipy==1.13.1
+Requires-Dist: opencv-contrib-python-headless==4.10.0.84
+Requires-Dist: fastapi==0.115.0
+Requires-Dist: uvicorn[standard]==0.30.6
+Requires-Dist: python-multipart==0.0.9
+
+# appRobotBodyTrack
+
+3D-Body-Tracking für Roboter aus Mehrkamera-ArUco-Bildern.
+
+**Input**  
+- Bilder: `render_*.png`  
+- Intrinsics: `render_*.npz`  
+- Konfiguration: `robot.json`  
+
+**Output**  
+- Gelenke **R⁷** → `{x, y, z, a, b, c, e}` (mm / Grad)
+
+---
+
+## Interfaces
+
+Eine Logik, drei Zugänge:
+
+- **Python**
+- **CLI**
+- **REST (FastAPI)**
+
+---
+
+## Quickstart
+
+### Python
+
+```python
+from scripts import estimate_from_dir
+
+result = estimate_from_dir("data/Scene8", robot_json="robot.json")
+
+print(result.joints)
+print(result.confidence)
+```
+
+---
+
+### CLI
+
+```bash
+pip install -e .
+
+python -m scripts data/Scene8 --robot robot.json
+python -m scripts data/Scene8 --robot robot.json --cameras a,b,d
+```
+
+---
+
+### REST API
+
+```bash
+docker compose up
+```
+
+**Request:**
+
+```python
+import requests
+
+resp = requests.post(
+    "http://localhost:8446/v1/estimate",
+    files=[
+        ("images", ("render_a.png", open("render_a.png", "rb"))),
+        ("intrinsics", ("render_a.npz", open("render_a.npz", "rb"))),
+    ],
+)
+
+print(resp.json()["joints"])
+```
+
+---
+
+## API
+
+| Endpoint | Methode | Zweck |
+|----------|--------|------|
+| `/v1/estimate` | POST | Bilder → Gelenke |
+| `/v1/health` | GET | Status |
+| `/v1/config` | GET | aktive Konfiguration |
+
+**Maschinenlesbares Schema / interaktive Doku** (automatisch von FastAPI):
+
+| URL | Zweck |
+|-----|-------|
+| `/openapi.json` | OpenAPI-Spezifikation (für Client-Generatoren) |
+| `/docs` | Swagger-UI (interaktiv ausprobieren) |
+| `/redoc` | ReDoc-Ansicht |
+
+**Response:**
+
+```json
+{
+  "joints": {"x": 50.2, "y": -2.1, "z": 94.8, "a": 20.1},
+  "confidence": {"x": "high", "b": "low"},
+  "residual_rms": 1.45,
+  "n_markers": 56,
+  "processing_ms": 1240
+}
+```
+
+---
+
+## Struktur
+
+```
+.
+├── scripts/
+├── config/robot.json
+├── tests/
+└── docker-compose.yaml
+```
+
+---
+
+## Deployment (Docker / Portainer)
+
+Die `config/robot.json` wird beim Build **mit ins Image gebacken** (`COPY . .`) und
+liegt dort fest unter `/app/config/robot.json`. Damit läuft der Container
+out-of-the-box — **kein Bind-Mount nötig**, auch nicht als Portainer-Stack.
+
+> Voraussetzung: `config/robot.json` muss **zum Build-Zeitpunkt** als Datei
+> vorhanden sein. Falls nicht: `cp config/robot.json.example config/robot.json`
+> und mit der echten Konfiguration füllen.
+
+**Start & Healthcheck:**
+```bash
+docker compose up -d
+curl http://<host>:8446/v1/health     # {"status":"ok","version":"1.0.0"}
+```
+
+**Eigene robot.json ohne Rebuild** (optional): die eingebackene Kopie per Mount
+überlagern — siehe auskommentierter `volumes`-Block in `docker-compose.yaml`.
+
+> ⚠️ **Portainer-Falle:** Ein Bind-Mount auf einen Host-Pfad, der **nicht
+> existiert**, lässt Docker dort ein leeres **Verzeichnis** anlegen. Die Pipeline
+> liest es dann als Datei → `IsADirectoryError` → jeder `/v1/estimate` und
+> `/v1/config` antwortet mit **500**. Deshalb ist der Mount jetzt optional;
+> aktiviere ihn nur, wenn die Datei am Host-Pfad wirklich liegt.
+
+---
+
+## Konfiguration
+
+Zentrale Datei: **`robot.json`**
+
+Verwendete Bereiche:
+- `links`
+- `pose_estimation`
+- `vision_config`
+- `movements`
+- `units`
+
+---
+
+## Stack (minimal)
+
+- numpy  
+- scipy  
+- opencv (aruco)  
+- fastapi + uvicorn  
+
+---
+
+## Naming
+
+- **BodyTrack** → Tracking (dynamisch) ✅  
+- **BodyMap** → Modell / Repräsentation  
+- **BodySense** → Wahrnehmung (low-level)

approbot_pipeline.egg-info/SOURCES.txt

@@ -0,0 +1,22 @@
+README.md
+pyproject.toml
+approbot_pipeline.egg-info/PKG-INFO
+approbot_pipeline.egg-info/SOURCES.txt
+approbot_pipeline.egg-info/dependency_links.txt
+approbot_pipeline.egg-info/entry_points.txt
+approbot_pipeline.egg-info/requires.txt
+approbot_pipeline.egg-info/top_level.txt
+scripts/__init__.py
+scripts/__main__.py
+scripts/orchestrator.py
+scripts/api/__init__.py
+scripts/api/__main__.py
+scripts/api/server.py
+scripts/pipeline/1_detect_aruco_observations.py
+scripts/pipeline/2_estimate_camera_from_observations.py
+scripts/pipeline/3_multiview_bundle_adjustment_v4.py
+scripts/pipeline/3b_corner_marker_poses.py
+scripts/pipeline/pose_estimation.py
+scripts/pipeline/robot_fk.py
+tests/test_e2e.py
+tests/test_pipeline.py

approbot_pipeline.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

approbot_pipeline.egg-info/entry_points.txt

@@ -0,0 +1,2 @@
+[console_scripts]
+approbot-pipeline = scripts.__main__:main

approbot_pipeline.egg-info/requires.txt

@@ -0,0 +1,6 @@
+numpy==1.26.4
+scipy==1.13.1
+opencv-contrib-python-headless==4.10.0.84
+fastapi==0.115.0
+uvicorn[standard]==0.30.6
+python-multipart==0.0.9

approbot_pipeline.egg-info/top_level.txt

@@ -0,0 +1 @@
+scripts

build/lib/scripts/__init__.py

@@ -0,0 +1,23 @@
+"""
+scripts
+=======
+Roboter-Pose-Schätzung aus Mehrkamera-ArUco-Bildern.
+
+Zwei Interfaces, gleiche Logik darunter:
+
+  (A) Python-Bibliothek — direkt einbindbar:
+        from scripts import estimate_from_dir, PipelineResult
+        result = estimate_from_dir("path/to/images", robot_json="robot.json")
+        print(result.joints)          # {"x": 50.2, "y": -2.1, ...}
+        print(result.confidence)      # {"x": "high", "b": "low", ...}
+
+  (B) REST-API — läuft als Service im Docker-Container:
+        POST /v1/estimate   (multipart: images + intrinsics)
+        GET  /v1/health
+        GET  /v1/config
+        → JSON mit joints, confidence, residual_rms, processing_ms
+"""
+from .orchestrator import estimate_from_dir, PipelineResult
+
+__version__ = "1.0.0"
+__all__ = ["estimate_from_dir", "PipelineResult", "__version__"]

build/lib/scripts/__main__.py

@@ -0,0 +1,43 @@
+"""Einstiegspunkt: python -m scripts <image_dir> [--robot ...] [--cameras ...]"""
+import argparse
+import json
+import sys
+
+
+def main() -> None:
+    ap = argparse.ArgumentParser(
+        description="appRobotBodyTrack: Bilder + robot.json → Gelenkwinkel im R⁷"
+    )
+    ap.add_argument("image_dir", help="Ordner mit render_*.png/jpg und render_*.npz")
+    ap.add_argument("--robot", default=None, help="Pfad zu robot.json")
+    ap.add_argument("--evalDir", default=None, help="Ausgabeordner (Standard: temporäres Verzeichnis)")
+    ap.add_argument("--cameras", default=None, help="Kamera-IDs, kommagetrennt, z.B. a,b,d")
+    ap.add_argument("--lambdaWeight", type=float, default=100.0)
+    args = ap.parse_args()
+
+    from scripts import estimate_from_dir
+
+    camera_filter = args.cameras.split(",") if args.cameras else None
+    try:
+        result = estimate_from_dir(
+            args.image_dir,
+            robot_json=args.robot,
+            eval_dir=args.evalDir,
+            lambda_weight=args.lambdaWeight,
+            camera_filter=camera_filter,
+        )
+    except Exception as exc:
+        print(f"[ERROR] {exc}", file=sys.stderr)
+        sys.exit(1)
+
+    print(json.dumps({
+        "joints": result.joints,
+        "confidence": result.confidence,
+        "n_markers": result.n_markers,
+        "residual_rms": result.residual_rms,
+        "processing_ms": result.processing_ms,
+    }, indent=2, ensure_ascii=False))
+
+
+if __name__ == "__main__":
+    main()

build/lib/scripts/api/__init__.py

@@ -0,0 +1,12 @@
+"""scripts.api — FastAPI REST-Service."""
+from .server import create_app
+
+
+def start_server(
+    robot_json=None,
+    host: str = "0.0.0.0",
+    port: int = 8446,
+) -> None:
+    import uvicorn
+    app = create_app(robot_json=robot_json)
+    uvicorn.run(app, host=host, port=port)

build/lib/scripts/api/__main__.py

@@ -0,0 +1,17 @@
+"""Einstiegspunkt: python -m scripts.api [--robot ...] [--host ...] [--port ...]"""
+import argparse
+
+from scripts.api import start_server
+
+
+def main() -> None:
+    ap = argparse.ArgumentParser(description="approbot-pipeline REST-API starten")
+    ap.add_argument("--robot", default=None, help="Pfad zu robot.json")
+    ap.add_argument("--host", default="0.0.0.0")
+    ap.add_argument("--port", type=int, default=8446)
+    args = ap.parse_args()
+    start_server(robot_json=args.robot, host=args.host, port=args.port)
+
+
+if __name__ == "__main__":
+    main()

build/lib/scripts/api/server.py

@@ -0,0 +1,97 @@
+"""FastAPI REST-API für appRobotBodyTrack."""
+from __future__ import annotations
+
+import json
+import tempfile
+from pathlib import Path
+from typing import List, Optional
+
+from fastapi import FastAPI, File, HTTPException, UploadFile
+from fastapi.responses import JSONResponse
+
+from scripts import __version__, estimate_from_dir
+
+_robot_json: Optional[Path] = None
+
+
+def create_app(robot_json: str | Path | None = None) -> FastAPI:
+    """App-Fabrik — setzt optionale Server-weite robot.json-Konfig."""
+    global _robot_json
+    if robot_json:
+        _robot_json = Path(robot_json).resolve()
+        # Frühe, klare Warnung statt kryptischem 500 zur Laufzeit.
+        # Häufige Falle: Docker legt für einen fehlenden Bind-Mount-Pfad
+        # ein leeres VERZEICHNIS an — dann ist _robot_json zwar vorhanden,
+        # aber keine Datei.
+        if not _robot_json.is_file():
+            import warnings
+            grund = "ist ein Verzeichnis" if _robot_json.is_dir() else "existiert nicht"
+            warnings.warn(
+                f"robot.json {grund}: {_robot_json}. "
+                f"/v1/config liefert 404, /v1/estimate verlangt einen robot_json-Upload. "
+                f"Bei Docker: liegt die Datei wirklich am gemounteten Host-Pfad?",
+                RuntimeWarning,
+                stacklevel=2,
+            )
+    return _app
+
+
+_app = FastAPI(title="approbot-pipeline", version=__version__)
+
+
+@_app.get("/v1/health")
+def health():
+    return {"status": "ok", "version": __version__}
+
+
+@_app.get("/v1/config")
+def config():
+    if _robot_json is None or not _robot_json.is_file():
+        raise HTTPException(404, "Keine robot.json konfiguriert (Datei fehlt oder ist ein Verzeichnis)")
+    data = json.loads(_robot_json.read_text(encoding="utf-8"))
+    return data.get("pose_estimation", {})
+
+
+@_app.post("/v1/estimate")
+async def estimate(
+    images: List[UploadFile] = File(..., description="Kamerabilder (render_<id>.png)"),
+    intrinsics: List[UploadFile] = File(..., description="Kamera-Intrinsiken (render_<id>.npz)"),
+    robot_json: Optional[UploadFile] = File(default=None, description="robot.json (überschreibt Server-Konfig)"),
+):
+    """Pose-Schätzung aus Kamerabildern.
+
+    Multipart-Upload:
+      images[]    — render_a.png, render_b.png, ...
+      intrinsics[] — render_a.npz, render_b.npz, ... (gleiche Reihenfolge)
+      robot_json  — optional, überschreibt die Server-Konfiguration
+    """
+    with tempfile.TemporaryDirectory(prefix="approbot_req_") as tmp:
+        tmp_path = Path(tmp)
+
+        if robot_json is not None:
+            rj_path = tmp_path / "robot.json"
+            rj_path.write_bytes(await robot_json.read())
+        elif _robot_json and _robot_json.is_file():
+            rj_path = _robot_json
+        else:
+            raise HTTPException(400, "Keine robot.json angegeben (weder Upload noch gültige Server-Konfig)")
+
+        for img in images:
+            (tmp_path / img.filename).write_bytes(await img.read())
+        for npz in intrinsics:
+            (tmp_path / npz.filename).write_bytes(await npz.read())
+
+        try:
+            result = estimate_from_dir(tmp_path, robot_json=rj_path, eval_dir=tmp_path)
+        except FileNotFoundError as exc:
+            raise HTTPException(400, str(exc))
+        except Exception as exc:
+            raise HTTPException(500, str(exc))
+
+    return JSONResponse({
+        "joints": result.joints,
+        "confidence": result.confidence,
+        "residual_rms": result.residual_rms,
+        "n_markers": result.n_markers,
+        "processing_ms": result.processing_ms,
+    })

build/lib/scripts/orchestrator.py

@@ -1,15 +1,18 @@
 """
-pipeline.py
+orchestrator.py
-===========
+===============
 Orchestrator — ruft die Pipeline-Schritte als Subprocess auf und gibt
 ein strukturiertes PipelineResult zurück.
 
+Die einzelnen Schritt-Skripte liegen im Verzeichnis ``scripts/pipeline/``
+und werden per Subprocess über ihren Dateipfad aufgerufen (s. ``SCRIPTS``).
+
 Schritte:
-    1  ArUco-Detektion          (scripts/1_detect_aruco_observations.py)
+    1  ArUco-Detektion          (scripts/pipeline/1_detect_aruco_observations.py)
-    2  Kamera-Posen             (scripts/2_estimate_camera_from_observations.py)
+    2  Kamera-Posen             (scripts/pipeline/2_estimate_camera_from_observations.py)
-    3  Multi-View Triangulation (scripts/3_multiview_bundle_adjustment_v4.py)
+    3  Multi-View Triangulation (scripts/pipeline/3_multiview_bundle_adjustment_v4.py)
-    3b Eck-Marker-Posen         (scripts/3b_corner_marker_poses.py)
+    3b Eck-Marker-Posen         (scripts/pipeline/3b_corner_marker_poses.py)
-    4  Pose-Estimation          (scripts/pose_estimation.py)
+    4  Pose-Estimation          (scripts/pipeline/pose_estimation.py)
 """
 from __future__ import annotations
 

build/lib/scripts/pipeline/1_detect_aruco_observations.py

@@ -0,0 +1,608 @@
+#!/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
+    )
+
+    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
+    )
+
+    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}')
+
+
+# ------------------------------------------------------------
+
+if __name__ == '__main__':
+    main()

build/lib/scripts/pipeline/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)

build/lib/scripts/pipeline/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()

build/lib/scripts/pipeline/pose_estimation.py

@@ -0,0 +1,539 @@
+#!/usr/bin/env python3
+"""
+pose_estimation.py
+==================
+Estimate robot joint angles (x, y, z, a, b, c, e) from triangulated marker
+poses, using the kinematic model in robot.json (via robot_fk.py).
+
+Design
+------
+The estimator is parametrised over JOINT VARIABLES, not links. This handles the
+tricky cases of this robot family generically:
+  * Links with zero own markers (Base/x, Hand/b, Palm/c) — their variable is
+    observable only through descendant markers.
+  * A variable shared by several links (FingerA & FingerB share 'e').
+  * Occluded middle links — global BA reconstructs them from the fingers.
+
+Four switchable methods (robot.json -> pose_estimation.method):
+  sequential_vector : analytic per joint from marker-pair / normal vectors (fast)
+  sequential_fk     : block-wise least squares along the chain (robust, 1 marker ok)
+  global_ba         : all variables jointly, position + normal residuals, robust loss
+  hybrid            : sequential_fk init -> global_ba refine   (default, most stable)
+
+Observation input:
+  marker_observation = "corner_pose"  -> aruco_marker_poses.json (pos + measured normal)
+  marker_observation = "center_point" -> aruco_positions_*.json   (pos only)
+
+Both the engine (estimate_pose) and a CLI (main) live here.
+"""
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import os
+import sys
+import time
+from collections import defaultdict
+from pathlib import Path
+from typing import Any, Dict, List, Optional, Tuple
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).parent))
+from robot_fk import RobotFK, STATE_KEYS  # noqa: E402
+
+try:
+    from scipy.optimize import least_squares
+    HAVE_SCIPY = True
+except ImportError:
+    HAVE_SCIPY = False
+
+
+# ==================================================================
+# Config
+# ==================================================================
+
+DEFAULT_CFG: Dict[str, Any] = {
+    "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": {},
+}
+
+
+def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
+    cfg = dict(DEFAULT_CFG)
+    cfg.update(robot_data.get("pose_estimation", {}) or {})
+    return cfg
+
+
+# ==================================================================
+# Observations
+# ==================================================================
+
+def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
+    """
+    Load marker observations. Accepts aruco_marker_poses.json (with measured
+    normal + num_cameras) or aruco_positions_*.json (position only).
+    Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int}
+    """
+    data = json.load(open(path, "r", encoding="utf-8"))
+    out: Dict[int, Dict[str, Any]] = {}
+    for m in data.get("markers", []):
+        mid = int(m.get("marker_id", m.get("id", -1)))
+        if mid < 0:
+            continue
+        n_cams = int(m.get("num_cameras", 99))
+        if n_cams < min_cams:
+            continue
+        if "position_mm" in m:
+            pos = np.array(m["position_mm"], dtype=float)
+        elif "position_m" in m:
+            pos = np.array(m["position_m"], dtype=float) * 1000.0
+        else:
+            continue
+        nrm = None
+        if use_normals and m.get("normal") is not None:
+            nv = np.array(m["normal"], dtype=float)
+            nn = np.linalg.norm(nv)
+            if nn > 1e-9:
+                nrm = nv / nn
+        out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams}
+    return out
+
+
+# ==================================================================
+# Kinematic chain analysis
+# ==================================================================
+
+def analyze_chain(fk: RobotFK) -> Dict[str, Any]:
+    """
+    Derive, generically from the FK topology:
+      ordered_vars   : movable joint variables, root->tip order, de-duplicated
+      var_links      : variable -> list of links it drives
+      link_markers   : link -> [model marker ids]
+      blocks         : sequential estimation blocks; each block groups the
+                       zero-marker ancestor variables with the next marker-
+                       bearing joint variable, estimated from that link's own
+                       markers (+ siblings sharing the same variable).
+    """
+    links = fk.links
+    topo = fk._topo
+
+    link_markers: Dict[str, List[int]] = {}
+    for ln, ld in links.items():
+        ids = []
+        for mk in ld.get("markers", []) or []:
+            if "id" in mk and "position" in mk:
+                ids.append(int(mk["id"]))
+        link_markers[ln] = ids
+
+    link_var: Dict[str, str] = {}
+    for ln, ld in links.items():
+        j = ld.get("jointToParent", {}) or {}
+        if str(j.get("type", "")).lower() in ("revolute", "linear"):
+            v = str(j.get("variable", "")).lower()
+            if v:
+                link_var[ln] = v
+
+    var_type: Dict[str, str] = {}
+    var_links: Dict[str, List[str]] = defaultdict(list)
+    for ln, v in link_var.items():
+        var_links[v].append(ln)
+        var_type[v] = str(links[ln].get("jointToParent", {}).get("type", "")).lower()
+
+    ordered_vars: List[str] = []
+    for ln in topo:
+        if ln in link_var and link_var[ln] not in ordered_vars:
+            ordered_vars.append(link_var[ln])
+
+    # ---- build blocks ----
+    blocks: List[Dict[str, Any]] = []
+    var_block: Dict[str, int] = {}
+    pending: List[str] = []
+    for ln in topo:
+        if ln not in link_var:
+            continue
+        v = link_var[ln]
+        own = link_markers.get(ln, [])
+        if v in var_block:
+            # shared variable already in a block -> add this link's markers there
+            if own:
+                blocks[var_block[v]]["markers"].extend(own)
+            continue
+        if own:
+            bvars = []
+            for x in pending + [v]:
+                if x not in bvars and x not in var_block:
+                    bvars.append(x)
+            blocks.append({"vars": bvars, "markers": list(own), "anchor": ln})
+            for x in bvars:
+                var_block[x] = len(blocks) - 1
+            pending = []
+        else:
+            if v not in pending:
+                pending.append(v)
+    if pending:
+        blocks.append({"vars": pending, "markers": [], "anchor": None})
+        for x in pending:
+            var_block[x] = len(blocks) - 1
+
+    return {
+        "ordered_vars": ordered_vars,
+        "var_type": var_type,
+        "var_links": dict(var_links),
+        "link_markers": link_markers,
+        "blocks": blocks,
+    }
+
+
+# ==================================================================
+# Residuals
+# ==================================================================
+
+def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, np.ndarray]]:
+    T = fk.compute(state)
+    return fk.all_markers_world(T)  # mid -> {world_mm, normal_world, link, local_mm}
+
+
+def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
+                    marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
+    """Position (mm) + optional normal (scaled) residuals over the given markers."""
+    model = model_markers(fk, state)
+    res: List[float] = []
+    w_n = float(cfg.get("normal_weight", 30.0))
+    use_n = bool(cfg.get("use_normals", True))
+    for mid in marker_ids:
+        if mid not in model or mid not in obs:
+            continue
+        mm = model[mid]
+        dp = np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]
+        res.extend(dp.tolist())
+        if use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
+            dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n
+            res.extend(dn.tolist())
+    return np.asarray(res, dtype=float)
+
+
+def _state_from_vec(var_names: List[str], vec: np.ndarray, base: Dict[str, float]) -> Dict[str, float]:
+    s = dict(base)
+    for name, val in zip(var_names, vec):
+        s[name] = float(val)
+    return s
+
+
+# ==================================================================
+# Method: global bundle adjustment
+# ==================================================================
+
+def estimate_global_ba(fk: RobotFK, obs: Dict[int, Dict[str, Any]], var_names: List[str],
+                       x0: Dict[str, float], cfg: Dict[str, Any]) -> Dict[str, float]:
+    if not HAVE_SCIPY:
+        print("[WARN] scipy missing — global_ba skipped, returning init")
+        return dict(x0)
+    marker_ids = list(obs.keys())
+    base = {k: 0.0 for k in STATE_KEYS}
+    base.update(x0)
+    vec0 = np.array([base.get(v, 0.0) for v in var_names], dtype=float)
+
+    def fun(vec):
+        st = _state_from_vec(var_names, vec, base)
+        return residual_vector(st, fk, obs, marker_ids, cfg)
+
+    loss = cfg.get("robust_loss", "huber")
+    f_scale = float(cfg.get("huber_delta_mm", 8.0))
+    try:
+        sol = least_squares(fun, vec0, loss=loss, f_scale=f_scale,
+                            max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(var_names)))
+        return _state_from_vec(var_names, sol.x, base)
+    except Exception as exc:
+        print(f"[WARN] global_ba failed: {exc}")
+        return dict(base)
+
+
+# ==================================================================
+# Method: sequential block-wise FK fit
+# ==================================================================
+
+def _multistart_values(vtype: str) -> List[float]:
+    # revolute: scan the circle to escape local minima at large angles
+    if vtype == "revolute":
+        return [0.0, 60.0, 120.0, 180.0, 240.0, 300.0]
+    return [0.0]
+
+
+def estimate_sequential_fk(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
+                           cfg: Dict[str, Any]) -> Dict[str, float]:
+    """Estimate block by block along the chain, freezing already-solved variables."""
+    state = {k: 0.0 for k in STATE_KEYS}
+    var_type = chain["var_type"]
+
+    for block in chain["blocks"]:
+        bvars = block["vars"]
+        bmarkers = [m for m in block["markers"] if m in obs]
+        if not bvars:
+            continue
+        if not bmarkers:
+            # unobservable block: leave at 0, flag later
+            continue
+
+        if not HAVE_SCIPY:
+            continue
+
+        base = dict(state)
+
+        def fun(vec, _bvars=bvars, _bm=bmarkers, _base=base):
+            st = _state_from_vec(_bvars, vec, _base)
+            return residual_vector(st, fk, obs, _bm, cfg)
+
+        # multi-start over the first revolute variable in the block
+        starts = [[0.0] * len(bvars)]
+        lead_type = var_type.get(bvars[0], "linear")
+        if lead_type == "revolute":
+            starts = []
+            for a0 in _multistart_values("revolute"):
+                s = [0.0] * len(bvars)
+                s[0] = a0
+                starts.append(s)
+
+        best, best_cost = None, float("inf")
+        for s0 in starts:
+            try:
+                sol = least_squares(fun, np.array(s0, dtype=float),
+                                    loss=cfg.get("robust_loss", "huber"),
+                                    f_scale=float(cfg.get("huber_delta_mm", 8.0)),
+                                    max_nfev=200 * max(1, len(bvars)))
+                if sol.cost < best_cost:
+                    best_cost, best = sol.cost, sol.x
+            except Exception:
+                continue
+        if best is not None:
+            for name, val in zip(bvars, best):
+                state[name] = float(val)
+
+    # wrap revolute angles to (-180, 180]
+    for v, vt in var_type.items():
+        if vt == "revolute":
+            state[v] = (state[v] + 180.0) % 360.0 - 180.0
+    return state
+
+
+# ==================================================================
+# Method: sequential analytic vector  (per revolute joint)
+# ==================================================================
+
+def estimate_sequential_vector(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
+                               cfg: Dict[str, Any]) -> Dict[str, float]:
+    """
+    Analytic angle from marker geometry where possible. For revolute joints with
+    >=2 markers on the link, use the perpendicular marker-pair vector. Falls back
+    to the FK block solver for linear / zero-marker / single-marker cases, so it
+    always returns a full state (still cheaper than full sequential_fk because
+    well-populated joints are solved in closed form).
+    """
+    state = {k: 0.0 for k in STATE_KEYS}
+    var_type = chain["var_type"]
+    link_markers = chain["link_markers"]
+    var_links = chain["var_links"]
+
+    for block in chain["blocks"]:
+        bvars = block["vars"]
+        if len(bvars) == 1 and var_type.get(bvars[0]) == "revolute":
+            v = bvars[0]
+            ln = var_links[v][0]
+            mids = [m for m in link_markers.get(ln, []) if m in obs]
+            if len(mids) >= 2:
+                # model vectors must be expressed in the WORLD frame at angle=0
+                # (the link frame is already rotated by the parents y,z,...), so
+                # use FK marker world positions with this joint set to 0.
+                state_v0 = dict(state)
+                state_v0[v] = 0.0
+                model_v0 = model_markers(fk, state_v0)
+                axis_world = fk.joint_axis_world(ln, state_v0)
+                ang = _angle_from_pairs_world(mids, model_v0, obs, axis_world)
+                if ang is not None:
+                    state[v] = ang
+                    continue
+        # fallback: block FK fit for this single block
+        _fit_single_block(fk, obs, block, var_type, cfg, state)
+
+    for v, vt in var_type.items():
+        if vt == "revolute":
+            state[v] = (state[v] + 180.0) % 360.0 - 180.0
+    return state
+
+
+def _angle_from_pairs_world(mids: List[int], model_v0: Dict[int, Dict[str, np.ndarray]],
+                            obs: Dict[int, Dict[str, Any]], axis_world: np.ndarray) -> Optional[float]:
+    from itertools import combinations
+    a = np.asarray(axis_world, float)
+    a = a / (np.linalg.norm(a) + 1e-12)
+    angs, ws = [], []
+    for i, j in combinations(mids, 2):
+        if i not in model_v0 or j not in model_v0:
+            continue
+        vm = np.asarray(model_v0[j]["world_mm"], float) - np.asarray(model_v0[i]["world_mm"], float)  # world @ angle 0
+        vo = obs[j]["pos_mm"] - obs[i]["pos_mm"]   # observed vector (world, mm)
+        vm_p = vm - np.dot(vm, a) * a
+        vo_p = vo - np.dot(vo, a) * a
+        if np.linalg.norm(vm_p) < 5 or np.linalg.norm(vo_p) < 5:
+            continue
+        ang = math.atan2(float(np.dot(a, np.cross(vm_p, vo_p))), float(np.dot(vm_p, vo_p)))
+        angs.append(ang)
+        ws.append(np.linalg.norm(vm_p) * np.linalg.norm(vo_p))
+    if not angs:
+        return None
+    s = sum(w * math.sin(x) for w, x in zip(ws, angs))
+    c = sum(w * math.cos(x) for w, x in zip(ws, angs))
+    return math.degrees(math.atan2(s, c))
+
+
+def _fit_single_block(fk, obs, block, var_type, cfg, state):
+    if not HAVE_SCIPY:
+        return
+    bvars = block["vars"]
+    bmarkers = [m for m in block["markers"] if m in obs]
+    if not bvars or not bmarkers:
+        return
+    base = dict(state)
+
+    def fun(vec):
+        return residual_vector(_state_from_vec(bvars, vec, base), fk, obs, bmarkers, cfg)
+
+    starts = [[0.0] * len(bvars)]
+    if var_type.get(bvars[0]) == "revolute":
+        starts = [[a0] + [0.0] * (len(bvars) - 1) for a0 in _multistart_values("revolute")]
+    best, best_cost = None, float("inf")
+    for s0 in starts:
+        try:
+            sol = least_squares(fun, np.array(s0, float), loss=cfg.get("robust_loss", "huber"),
+                                f_scale=float(cfg.get("huber_delta_mm", 8.0)), max_nfev=200 * max(1, len(bvars)))
+            if sol.cost < best_cost:
+                best_cost, best = sol.cost, sol.x
+        except Exception:
+            continue
+    if best is not None:
+        for name, val in zip(bvars, best):
+            state[name] = float(val)
+
+
+# ==================================================================
+# Dispatch
+# ==================================================================
+
+def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict[str, Dict[str, Any]]:
+    """
+    Per-variable confidence from how well its estimation block is determined.
+    A block groups coupled variables (e.g. b,c,e on the fingers); confidence is
+    driven by markers-per-variable in that block:
+        high   : >= 2 markers per variable (well over-determined)
+        medium : >= 1 marker  per variable
+        low    : fewer markers than variables (under-determined — distrust!)
+        none   : no markers at all (variable left at 0)
+    """
+    info: Dict[str, Dict[str, Any]] = {}
+    for block in chain["blocks"]:
+        seen = [m for m in block["markers"] if m in obs]
+        nvars = max(1, len(block["vars"]))
+        ratio = len(seen) / nvars
+        if len(seen) == 0:
+            conf = "none"
+        elif ratio >= 2.0:
+            conf = "high"
+        elif ratio >= 1.0:
+            conf = "medium"
+        else:
+            conf = "low"
+        for v in block["vars"]:
+            info[v] = {"observable": len(seen) > 0, "n_markers": len(seen),
+                       "block_vars": len(block["vars"]), "confidence": conf,
+                       "block_anchor": block["anchor"]}
+    return info
+
+
+def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any]) -> Dict[str, Any]:
+    chain = analyze_chain(fk)
+    var_names = chain["ordered_vars"]
+    method = str(cfg.get("method", "hybrid")).lower()
+    obsv = observability(chain, obs)
+
+    if method == "sequential_vector":
+        state = estimate_sequential_vector(fk, obs, chain, cfg)
+    elif method == "sequential_fk":
+        state = estimate_sequential_fk(fk, obs, chain, cfg)
+    elif method == "global_ba":
+        init = estimate_sequential_fk(fk, obs, chain, cfg)  # cheap robust init
+        state = estimate_global_ba(fk, obs, var_names, init, cfg)
+    else:  # hybrid (default)
+        init = estimate_sequential_fk(fk, obs, chain, cfg)
+        state = estimate_global_ba(fk, obs, var_names, init, cfg)
+
+    # final residual stats over all observed markers
+    final_res = residual_vector(state, fk, obs, list(obs.keys()), cfg)
+    rms = float(np.sqrt(np.mean(final_res ** 2))) if final_res.size else 0.0
+
+    return {"state": state, "method": method, "observability": obsv,
+            "residual_rms": rms, "num_markers": len(obs)}
+
+
+# ==================================================================
+# CLI
+# ==================================================================
+
+def main() -> None:
+    ap = argparse.ArgumentParser(description="Estimate robot joint angles from marker poses")
+    ap.add_argument("markers", help="aruco_marker_poses.json (corner_pose) or aruco_positions_*.json (center)")
+    ap.add_argument("-robot", "--robot", required=True)
+    ap.add_argument("-out", "--out", default=None)
+    ap.add_argument("--method", default=None, help="override robot.json method")
+    args = ap.parse_args()
+
+    robot_data = json.load(open(args.robot, "r", encoding="utf-8"))
+    cfg = load_pose_cfg(robot_data)
+    if args.method:
+        cfg["method"] = args.method
+
+    fk = RobotFK(robot_data)
+    obs = load_observations(args.markers, cfg.get("use_normals", True),
+                            int(cfg.get("min_cameras_per_marker", 2)))
+    print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}")
+
+    result = estimate_pose(fk, obs, cfg)
+    st = result["state"]
+
+    print("\nEstimated joint values:")
+    for v in ["x", "y", "z", "a", "b", "c", "e"]:
+        ob = result["observability"].get(v, {})
+        unit = "mm" if v in ("x", "e") else "deg"
+        conf = ob.get("confidence", "?")
+        tag = "" if ob.get("observable", False) else "  [UNOBSERVABLE -> 0]"
+        print(f"  {v}: {st.get(v, 0.0):8.2f} {unit}   (markers={ob.get('n_markers','?')}, conf={conf}){tag}")
+    print(f"\n[INFO] residual RMS over {result['num_markers']} markers: {result['residual_rms']:.3f}")
+
+    out = {
+        "schema_version": "1.0",
+        "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
+        "method": result["method"],
+        "movements": {v: {"value": st.get(v, 0.0),
+                          "unit": "mm" if v in ("x", "e") else "deg",
+                          "observable": result["observability"].get(v, {}).get("observable", False),
+                          "confidence": result["observability"].get(v, {}).get("confidence", "none"),
+                          "n_markers": result["observability"].get(v, {}).get("n_markers", 0)}
+                      for v in ["x", "y", "z", "a", "b", "c", "e"]},
+        "residual_rms": result["residual_rms"],
+        "num_markers": result["num_markers"],
+    }
+    out_path = args.out or os.path.join(os.path.dirname(args.markers), "robot_state.json")
+    json.dump(out, open(out_path, "w", encoding="utf-8"), indent=2)
+    print(f"[INFO] wrote {out_path}")
+
+
+if __name__ == "__main__":
+    main()

build/lib/scripts/pipeline/robot_fk.py

@@ -0,0 +1,310 @@
+#!/usr/bin/env python3
+"""
+robot_fk.py
+-----------
+Minimal forward kinematics engine for the robot.json format.
+
+Matches the Blender hierarchy used by render_robot.py exactly:
+  world_T_link = world_T_parent
+                 @ Translate(mountPosition) @ Rotate_xyz(mountRotation)
+                 @ Translate(jointOrigin)   @ Rotate_xyz(joint.rotation)
+                 @ T_motion
+
+  T_motion =  Rotate(axis, value_deg)   for revolute joints
+              Translate(axis * value_mm) for linear joints
+
+Units throughout: millimetres, degrees.
+
+Public API
+----------
+fk = RobotFK.from_file("robot.json")
+
+transforms = fk.compute({"x": 180, "y": 86, "z": -120,
+                          "a": -60, "b": 22, "c": 91, "e": 10})
+# → dict  link_name -> 4×4 np.ndarray (world frame, mm)
+
+p_world = fk.marker_world(transforms, "Arm1", [0, -160, 35])
+# → np.ndarray shape (3,), in mm
+
+all_m = fk.all_markers_world(transforms)
+# → dict  marker_id -> {"world_mm", "link", "local_mm"}
+
+# Cumulative x-offset for a link at all-zero state
+# (useful for 4a: x_slider = world_x - local_x - link_x_at_zero)
+x0 = fk.link_x_at_zero_state("Arm1")   # → float mm
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Dict, List, Optional, Sequence, Tuple
+
+import numpy as np
+
+STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
+
+
+# ──────────────────────────────────────────────────────────────
+# Low-level matrix helpers
+# ──────────────────────────────────────────────────────────────
+
+def _rot_axis_angle(axis: Sequence[float], angle_deg: float) -> np.ndarray:
+    """3×3 rotation matrix via Rodrigues (axis need not be normalised)."""
+    ax = np.asarray(axis, dtype=float)
+    n = float(np.linalg.norm(ax))
+    if n < 1e-12:
+        return np.eye(3)
+    ax = ax / n
+    c = math.cos(math.radians(angle_deg))
+    s = math.sin(math.radians(angle_deg))
+    t = 1.0 - c
+    x, y, z = ax
+    return np.array([
+        [t*x*x + c,   t*x*y - s*z, t*x*z + s*y],
+        [t*x*y + s*z, t*y*y + c,   t*y*z - s*x],
+        [t*x*z - s*y, t*y*z + s*x, t*z*z + c  ],
+    ])
+
+
+def _rot_xyz_euler(rx: float, ry: float, rz: float) -> np.ndarray:
+    """XYZ Euler angles (degrees) → 3×3 — matches Blender XYZ Euler mode."""
+    return (_rot_axis_angle([0, 0, 1], rz)
+            @ _rot_axis_angle([0, 1, 0], ry)
+            @ _rot_axis_angle([1, 0, 0], rx))
+
+
+def make_T(R: np.ndarray, t: Sequence[float]) -> np.ndarray:
+    """4×4 homogeneous transform."""
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = np.asarray(t, dtype=float)
+    return T
+
+
+def transform_point(T: np.ndarray, p: Sequence[float]) -> np.ndarray:
+    """Apply 4×4 transform to a 3-D point."""
+    h = np.array([p[0], p[1], p[2], 1.0])
+    return (T @ h)[:3]
+
+
+# ──────────────────────────────────────────────────────────────
+# FK engine
+# ──────────────────────────────────────────────────────────────
+
+class RobotFK:
+    """Forward kinematics for the robot.json format."""
+
+    def __init__(self, robot_data: Dict[str, Any]):
+        self.robot = robot_data
+        self.links: Dict[str, Any] = robot_data.get("links", {})
+        self._topo: List[str] = self._topological_sort()
+
+    # ── construction ─────────────────────────────────────────
+
+    @classmethod
+    def from_file(cls, path) -> "RobotFK":
+        with open(path, "r", encoding="utf-8") as f:
+            return cls(json.load(f))
+
+    def _topological_sort(self) -> List[str]:
+        parent_map = {n: d.get("parent") for n, d in self.links.items()}
+        visited, order = set(), []
+
+        def visit(name: str) -> None:
+            if name in visited:
+                return
+            visited.add(name)
+            p = parent_map.get(name)
+            if p and p in self.links:
+                visit(p)
+            order.append(name)
+
+        for name in self.links:
+            visit(name)
+        return order
+
+    # ── core computation ──────────────────────────────────────
+
+    def compute(self, joint_values: Dict[str, float]) -> Dict[str, np.ndarray]:
+        """
+        Compute link world transforms for the given joint state.
+
+        Parameters
+        ----------
+        joint_values : dict  variable_name -> value
+            Linear joints (x, e): mm
+            Revolute joints (y, z, a, b, c): degrees
+
+        Returns
+        -------
+        dict  link_name -> 4×4 np.ndarray (world frame, mm)
+        """
+        state = {k: 0.0 for k in STATE_KEYS}
+        for k, v in joint_values.items():
+            if k in state:
+                state[k] = float(v)
+
+        transforms: Dict[str, np.ndarray] = {}
+
+        for link_name in self._topo:
+            d = self.links[link_name]
+            parent = d.get("parent")
+            T_parent = transforms.get(parent, np.eye(4)) if parent else np.eye(4)
+
+            # 1 · Mount (static in parent frame)
+            mp  = d.get("mountPosition", [0, 0, 0])
+            mr  = d.get("mountRotation", [0, 0, 0])
+            T_m = make_T(_rot_xyz_euler(*mr), mp)
+
+            # 2 · Joint origin (pivot point in mount frame)
+            ji  = d.get("jointToParent", {}) or {}
+            jp  = ji.get("origin", [0, 0, 0])
+            jr  = ji.get("rotation", [0, 0, 0])
+            T_j = make_T(_rot_xyz_euler(*jr), jp)
+
+            # 3 · Motion
+            jtype = str(ji.get("type", "fixed")).lower()
+            var   = str(ji.get("variable", "")).lower()
+            axis  = np.asarray(ji.get("axis", [1, 0, 0]), dtype=float)
+            val   = state.get(var, 0.0)
+
+            if jtype == "revolute":
+                T_mot = make_T(_rot_axis_angle(axis, val), [0, 0, 0])
+            elif jtype == "linear":
+                n = float(np.linalg.norm(axis))
+                u = axis / n if n > 1e-12 else np.array([1.0, 0, 0])
+                T_mot = make_T(np.eye(3), u * val)
+            else:
+                T_mot = np.eye(4)
+
+            transforms[link_name] = T_parent @ T_m @ T_j @ T_mot
+
+        return transforms
+
+    # ── marker helpers ────────────────────────────────────────
+
+    @staticmethod
+    def marker_world(transforms: Dict[str, np.ndarray],
+                     link_name: str,
+                     local_pos: Sequence[float]) -> np.ndarray:
+        """Transform a local marker position → world (mm)."""
+        return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
+
+    def all_markers_world(self,
+                          transforms: Dict[str, np.ndarray]
+                          ) -> Dict[int, Dict[str, Any]]:
+        """
+        Returns
+        -------
+        dict  marker_id -> {world_mm, local_mm, link, normal_world}
+        """
+        result: Dict[int, Dict[str, Any]] = {}
+        for lname, ldata in self.links.items():
+            T = transforms.get(lname, np.eye(4))
+            R = T[:3, :3]
+            for m in ldata.get("markers", []):
+                mid = int(m.get("id", -1))
+                if mid < 0 or "position" not in m:
+                    continue
+                loc = np.array(m["position"], dtype=float)
+                nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
+                result[mid] = {
+                    "world_mm":    transform_point(T, loc),
+                    "local_mm":    loc,
+                    "link":        lname,
+                    "normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
+                }
+        return result
+
+    # ── x-axis invariant helpers (used by 4a) ────────────────
+
+    def link_x_at_zero_state(self, link_name: str) -> float:
+        """
+        Return the world x-coordinate of the link-frame origin
+        when ALL joint variables are zero.
+
+        For any link reachable via only x-axis rotations (Arm1, Ellbow, Arm2),
+        this value is constant regardless of the actual revolute angles.
+        Adding the slider value x_mm gives the true link origin x:
+            link_origin_world_x = x_mm + link_x_at_zero_state(link_name)
+
+        And for any marker in that link:
+            marker_world_x = x_mm + link_x_at_zero_state(link_name) + marker_local_x
+        """
+        T = self.compute({k: 0.0 for k in STATE_KEYS})
+        return float(T[link_name][0, 3])
+
+    def joint_origin_world(self,
+                            link_name: str,
+                            joint_state: Dict[str, float]) -> np.ndarray:
+        """
+        World position of the pivot that link_name rotates / slides around.
+        """
+        d = self.links[link_name]
+        parent = d.get("parent")
+        T_all = self.compute(joint_state)
+        T_parent = T_all.get(parent, np.eye(4)) if parent else np.eye(4)
+
+        mp  = d.get("mountPosition", [0, 0, 0])
+        mr  = d.get("mountRotation", [0, 0, 0])
+        T_m = make_T(_rot_xyz_euler(*mr), mp)
+
+        ji  = d.get("jointToParent", {}) or {}
+        jp  = ji.get("origin", [0, 0, 0])
+        jr  = ji.get("rotation", [0, 0, 0])
+        T_j = make_T(_rot_xyz_euler(*jr), jp)
+
+        return transform_point(T_parent @ T_m @ T_j, [0, 0, 0])
+
+    def joint_axis_world(self,
+                          link_name: str,
+                          joint_state: Dict[str, float]) -> np.ndarray:
+        """
+        Joint axis of link_name expressed in world frame.
+        """
+        d = self.links[link_name]
+        parent = d.get("parent")
+        T_all = self.compute(joint_state)
+        T_parent = T_all.get(parent, np.eye(4)) if parent else np.eye(4)
+
+        mp  = d.get("mountPosition", [0, 0, 0])
+        mr  = d.get("mountRotation", [0, 0, 0])
+        T_m = make_T(_rot_xyz_euler(*mr), mp)
+
+        ji  = d.get("jointToParent", {}) or {}
+        jp  = ji.get("origin", [0, 0, 0])
+        jr  = ji.get("rotation", [0, 0, 0])
+        T_j = make_T(_rot_xyz_euler(*jr), jp)
+
+        R_to_pivot = (T_parent @ T_m @ T_j)[:3, :3]
+        axis_local = np.asarray(ji.get("axis", [1, 0, 0]), dtype=float)
+        world = R_to_pivot @ axis_local
+        n = float(np.linalg.norm(world))
+        return world / n if n > 1e-12 else world
+
+    # ── utility ───────────────────────────────────────────────
+
+    def chain(self, link_name: str) -> List[str]:
+        """Return chain from root to link_name (inclusive)."""
+        out, cur = [], link_name
+        while cur:
+            out.append(cur)
+            cur = self.links.get(cur, {}).get("parent")
+        return list(reversed(out))
+
+    def board_marker_world_positions(self, length_scale: float = 1.0) -> Dict[int, np.ndarray]:
+        """
+        Return known world positions for all Board markers (in mm).
+        Board is the root, so its marker positions ARE world positions.
+        length_scale: 1/1000 if robot.json uses mm.
+        """
+        board = self.links.get("Board", {})
+        result: Dict[int, np.ndarray] = {}
+        for m in board.get("markers", []):
+            mid = int(m.get("id", -1))
+            if mid >= 0 and "position" in m:
+                p = np.array(m["position"], dtype=float) * length_scale
+                result[mid] = p
+        return result

deploy.sh

@@ -0,0 +1,53 @@
+#!/usr/bin/env bash
+# ---------------------------------------------------------------------------
+# appRobotBodyTracker — Build & Start ohne Portainer (per docker compose).
+#
+#   ./deploy.sh           bauen + starten (Default)
+#   ./deploy.sh logs      Live-Logs
+#   ./deploy.sh stop      stoppen + entfernen
+#   ./deploy.sh status    Status
+#
+# Die robot.json ist im Image eingebacken (/app/config/robot.json) -> kein
+# Volume noetig. Das Skript repariert vorher automatisch den haeufigen Fall,
+# dass config/robot.json versehentlich ein (leeres) Verzeichnis ist.
+# ---------------------------------------------------------------------------
+set -euo pipefail
+cd "$(dirname "$0")"
+
+PORT=8446
+NAME=appRobotBodyTracker
+
+case "${1:-up}" in
+  logs)   exec docker compose logs -f ;;
+  stop)   exec docker compose down ;;
+  status) exec docker compose ps ;;
+esac
+
+# --- robot.json muss eine DATEI sein (kein Phantom-Verzeichnis vom Bind-Mount) ---
+if [ -d config/robot.json ]; then
+  echo ">> config/robot.json ist ein Verzeichnis -> repariere aus tests/data/robot.json"
+  rm -rf config/robot.json
+  cp tests/data/robot.json config/robot.json
+fi
+if [ ! -f config/robot.json ]; then
+  echo "FEHLER: config/robot.json fehlt."
+  echo "Fix:    cp tests/data/robot.json config/robot.json"
+  exit 1
+fi
+
+echo ">> Build + Start ($NAME, Port $PORT)"
+docker compose up -d --build
+
+echo ">> Warte auf /v1/health ..."
+for _ in $(seq 1 30); do
+  if curl -fsS "http://localhost:${PORT}/v1/health" >/dev/null 2>&1; then
+    echo ">> OK   $(curl -fsS "http://localhost:${PORT}/v1/health")"
+    ip="$(hostname -I 2>/dev/null | awk '{print $1}')"
+    echo ">> API erreichbar unter: http://${ip:-<host>}:${PORT}"
+    echo ">> Logs: ./deploy.sh logs   |   Stop: ./deploy.sh stop"
+    exit 0
+  fi
+  sleep 1
+done
+echo ">> WARN: Health nicht erreicht. Logs ansehen: ./deploy.sh logs"
+exit 1

docker-compose.portainer.yaml

@@ -0,0 +1,23 @@
+# Run-only Stack fuer Portainer.
+#
+# Portainer baut Web-Editor-/Upload-Stacks in einem leeren internen Verzeichnis
+# (context "." enthaelt dort NUR die Compose-Datei) -> ein "build:"-Block mit
+# "COPY . ." schlaegt fehl ("pip install ." exit 1). Deshalb hier KEIN build:,
+# sondern nur das fertige Image, das vorher per CLI auf dem Host gebaut wurde:
+#
+#     cd ~/Documents/appRobotBodyTracker
+#     docker compose build          # erzeugt approbot/pose-pipeline:1.0.0 lokal
+#
+# Danach diesen Stack in Portainer deployen. WICHTIG: "Re-pull image" / "Always
+# pull" NICHT aktivieren - das Image liegt lokal, nicht in einer Registry.
+#
+# Die robot.json ist im Image eingebacken (/app/config/robot.json), daher kein
+# Volume noetig.
+
+services:
+  pipeline:
+    image: approbot/pose-pipeline:1.0.0
+    container_name: appRobotBodyTracker
+    restart: unless-stopped
+    ports:
+      - "8446:8446"

docker-compose.yaml

@@ -7,19 +7,26 @@ services:
         WORKDIR /app
         COPY . .
         RUN pip install --no-cache-dir .
-        ENV ROBOT_JSON=/config/robot.json
+        # robot.json wird per "COPY . ." mit ins Image gebacken und liegt damit
+        # IMMER unter /app/config/robot.json. Kein Bind-Mount noetig -> laeuft
+        # out-of-the-box auch als Portainer-Stack (dort gibt es kein lokales
+        # ./config, ein fehlender Mount-Pfad wuerde sonst ein leeres Verzeichnis
+        # erzeugen und jeden /v1/estimate mit 500 (IsADirectoryError) abbrechen).
+        ENV ROBOT_JSON=/app/config/robot.json
         EXPOSE 8446
         HEALTHCHECK --interval=30s --timeout=10s --start-period=15s --retries=3 \
           CMD python -c "import urllib.request; urllib.request.urlopen('http://localhost:8446/v1/health')"
         CMD ["python", "-m", "scripts.api", \
-             "--robot", "/config/robot.json", \
+             "--robot", "/app/config/robot.json", \
              "--host", "0.0.0.0", "--port", "8446"]
     image: approbot/pose-pipeline:1.0.0
     container_name: appRobotBodyTracker
     restart: unless-stopped
     ports:
       - "8446:8446"
-    volumes:
+    # Optional: eigene robot.json ohne Rebuild einspielen, indem die eingebackene
-      - ./config/robot.json:/config/robot.json:ro
+    # Kopie ueberlagert wird. NUR aktivieren, wenn die Host-Datei WIRKLICH existiert
-    environment:
+    # (sonst legt Docker dort ein leeres Verzeichnis an -> 500). Im Portainer-Stack
-      - ROBOT_JSON=/config/robot.json
+    # in der Regel nicht noetig, da die robot.json bereits im Image enthalten ist.
+    # volumes:
+    #   - /home/chk/Documents/appRobotBodyTracker/config/robot.json:/app/config/robot.json:ro

pyproject.toml

@@ -23,3 +23,10 @@ approbot-pipeline = "scripts.__main__:main"
 [tool.setuptools.packages.find]
 where = ["."]
 include = ["scripts*"]
+
+# Die Pipeline-Schritte in scripts/pipeline/ sind KEIN Python-Paket
+# (kein __init__.py, teils Ziffern-Präfixe wie 1_detect_*.py). Sie werden
+# vom Orchestrator per Subprocess über ihren Dateipfad aufgerufen und müssen
+# deshalb explizit als Paket-Daten ins Wheel aufgenommen werden.
+[tool.setuptools.package-data]
+scripts = ["pipeline/*.py"]

scripts/__init__.py

@@ -17,7 +17,7 @@ Zwei Interfaces, gleiche Logik darunter:
         GET  /v1/config
         → JSON mit joints, confidence, residual_rms, processing_ms
 """
-from .pipeline import estimate_from_dir, PipelineResult
+from .orchestrator import estimate_from_dir, PipelineResult
 
 __version__ = "1.0.0"
 __all__ = ["estimate_from_dir", "PipelineResult", "__version__"]

scripts/orchestrator.py

@@ -0,0 +1,183 @@
+"""
+orchestrator.py
+===============
+Orchestrator — ruft die Pipeline-Schritte als Subprocess auf und gibt
+ein strukturiertes PipelineResult zurück.
+
+Die einzelnen Schritt-Skripte liegen im Verzeichnis ``scripts/pipeline/``
+und werden per Subprocess über ihren Dateipfad aufgerufen (s. ``SCRIPTS``).
+
+Schritte:
+    1  ArUco-Detektion          (scripts/pipeline/1_detect_aruco_observations.py)
+    2  Kamera-Posen             (scripts/pipeline/2_estimate_camera_from_observations.py)
+    3  Multi-View Triangulation (scripts/pipeline/3_multiview_bundle_adjustment_v4.py)
+    3b Eck-Marker-Posen         (scripts/pipeline/3b_corner_marker_poses.py)
+    4  Pose-Estimation          (scripts/pipeline/pose_estimation.py)
+"""
+from __future__ import annotations
+
+import glob
+import json
+import os
+import re
+import subprocess
+import sys
+import tempfile
+import time
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Dict, List, Optional
+
+SCRIPTS = Path(__file__).parent / "pipeline"
+PY = sys.executable
+
+
+@dataclass
+class PipelineResult:
+    """Ergebnis der Pose-Schätzung."""
+    joints: Dict[str, float]          # x,y,z,a,b,c,e → Wert (mm oder °)
+    confidence: Dict[str, str]        # x,y,z,a,b,c,e → high|medium|low|none
+    n_markers: int                    # Anzahl triangulierter Marker
+    residual_rms: float               # Residuum der Schätzung (mm)
+    processing_ms: float              # Laufzeit der Pipeline
+    robot_state_path: Optional[Path] = None   # Pfad zur erzeugten robot_state.json
+    errors: List[str] = field(default_factory=list)
+
+
+def _run(cmd: list, step: str) -> None:
+    """Subprocess-Aufruf mit Fehlerbehandlung."""
+    r = subprocess.run(cmd, capture_output=True, text=True)
+    if r.returncode != 0:
+        raise RuntimeError(f"[{step}] exit {r.returncode}:\n{r.stderr.strip()[-800:]}")
+
+
+def _cam_id(path: str) -> Optional[str]:
+    m = re.match(r"render_([A-Za-z0-9]+)\.(png|jpg|jpeg)", os.path.basename(path), re.I)
+    return m.group(1) if m else None
+
+
+def estimate_from_dir(
+    image_dir: str | Path,
+    robot_json: str | Path | None = None,
+    eval_dir: str | Path | None = None,
+    lambda_weight: float = 100.0,
+    camera_filter: Optional[List[str]] = None,
+) -> PipelineResult:
+    """
+    Pose-Schätzung aus einem Bildordner.
+
+    Parameters
+    ----------
+    image_dir       Ordner mit render_*.png und render_*.npz
+    robot_json      Pfad zu robot.json (Default: ROBOT_JSON env oder Exception)
+    eval_dir        Ausgabeordner (Default: temporäres Verzeichnis)
+    lambda_weight   Constraint-Gewicht für Bundle Adjustment
+    camera_filter   Liste von Kamera-IDs; None = alle
+
+    Returns
+    -------
+    PipelineResult
+    """
+    t0 = time.time()
+    image_dir = Path(image_dir).resolve()
+
+    # robot.json bestimmen
+    if robot_json is None:
+        robot_json = os.environ.get("ROBOT_JSON")
+        if not robot_json:
+            raise ValueError("robot_json muss angegeben werden oder ROBOT_JSON env gesetzt sein")
+    robot_json = Path(robot_json).resolve()
+
+    # Ausgabeordner
+    _tmp = None
+    if eval_dir is None:
+        _tmp = tempfile.mkdtemp(prefix="approbot_")
+        eval_dir = Path(_tmp)
+    else:
+        eval_dir = Path(eval_dir).resolve()
+        eval_dir.mkdir(parents=True, exist_ok=True)
+
+    errors = []
+
+    try:
+        # Bilder sammeln
+        imgs = sorted(
+            glob.glob(str(image_dir / "render_*.png")) +
+            glob.glob(str(image_dir / "render_*.PNG")) +
+            glob.glob(str(image_dir / "render_*.jpg")) +
+            glob.glob(str(image_dir / "render_*.jpeg"))
+        )
+        if camera_filter:
+            imgs = [i for i in imgs if _cam_id(i) in set(camera_filter)]
+        if not imgs:
+            raise FileNotFoundError(f"Keine render_*.png/jpg in {image_dir}")
+
+        # ── Schritt 1: ArUco-Detektion ──────────────────────────────
+        for img in imgs:
+            cid = _cam_id(img)
+            if not cid:
+                continue
+            npz = image_dir / f"render_{cid}.npz"
+            if not npz.exists():
+                npz_candidates = list(image_dir.glob("*.npz"))
+                if not npz_candidates:
+                    raise FileNotFoundError(f"Keine .npz-Intrinsik in {image_dir}")
+                npz = npz_candidates[0]
+            _run([PY, str(SCRIPTS / "1_detect_aruco_observations.py"),
+                  "-i", str(img), "-npz", str(npz),
+                  "-outDir", str(eval_dir), "-robot", str(robot_json), "-cameraId", cid],
+                 "Schritt 1")
+
+        # ── Schritt 2: Kamera-Posen ──────────────────────────────────
+        dets = sorted(glob.glob(str(eval_dir / "*_aruco_detection.json")))
+        if not dets:
+            raise RuntimeError("Keine ArUco-Detektionen erzeugt (Schritt 1)")
+        for d in dets:
+            _run([PY, str(SCRIPTS / "2_estimate_camera_from_observations.py"),
+                  "-i", d, "-robot", str(robot_json), "-outDir", str(eval_dir)],
+                 "Schritt 2")
+
+        # ── Schritt 3: Multi-View Triangulation ──────────────────────
+        poses = sorted(glob.glob(str(eval_dir / "*_camera_pose.json")))
+        if not poses:
+            raise RuntimeError("Keine Kamera-Posen erzeugt (Schritt 2)")
+        det_args = sum([["-det", d] for d in dets], [])
+        pose_args = sum([["-pose", p] for p in poses], [])
+        _run([PY, str(SCRIPTS / "3_multiview_bundle_adjustment_v4.py"),
+              "-robot", str(robot_json), "-lambdaWeight", str(lambda_weight)]
+             + det_args + pose_args, "Schritt 3")
+
+        # ── Schritt 3b: Eck-Marker-Posen ─────────────────────────────
+        _run([PY, str(SCRIPTS / "3b_corner_marker_poses.py"),
+              "--evalDir", str(eval_dir), "--robot", str(robot_json)], "Schritt 3b")
+
+        # ── Schritt 4: Pose-Estimation ────────────────────────────────
+        marker_poses = eval_dir / "aruco_marker_poses.json"
+        state_out = eval_dir / "robot_state.json"
+        _run([PY, str(SCRIPTS / "pose_estimation.py"),
+              str(marker_poses), "-robot", str(robot_json), "-out", str(state_out)],
+             "Schritt 4")
+
+        # Ergebnis lesen
+        state = json.load(open(state_out, "r", encoding="utf-8"))
+        mv = state.get("movements", {})
+        joints = {k: float(v.get("value", 0.0)) for k, v in mv.items() if isinstance(v, dict)}
+        confidence = {k: str(v.get("confidence", "none")) for k, v in mv.items() if isinstance(v, dict)}
+
+        return PipelineResult(
+            joints=joints,
+            confidence=confidence,
+            n_markers=int(state.get("num_markers", 0)),
+            residual_rms=float(state.get("residual_rms", 0.0)),
+            processing_ms=round((time.time() - t0) * 1000),
+            robot_state_path=state_out,
+            errors=errors,
+        )
+
+    except Exception as exc:
+        errors.append(str(exc))
+        raise
+    finally:
+        # Temporäres Verzeichnis bleibt absichtlich erhalten für Debugging;
+        # Aufrufer kann es über result.robot_state_path.parent aufräumen.
+        pass