# Kalibrierung – appRobotHoming

> Stand: 2026-06-15  
> Einmaliger Vorgang — nur nach mechanischen Änderungen wiederholen.  
> Jede Stufe baut auf der vorherigen auf.

---

## Übersicht

```
[1] Camera NPZ  →  [2] Board  →  [3] X-Achse  →  [4] Arm1 Y-Achse  →  [5] Arm-Marker
```

| Schritt | UI-Tab | Ergebnis | Status |
|---------|--------|----------|--------|
| [1] Camera NPZ | Camera NPZ | `data/calibration/{session}/{cam}_calibration.npz` | ✅ |
| [2] Board | Board | `links.Board.markers[].position` in `robot.json` | ✅ |
| [3] X-Achse | Robot X Axis | Alle Marker-Positionen in `robot.json` rotiert | ✅ |
| [4] Arm1 Y-Achse | Arm1 – Y | `links.Arm1.jointToParent.origin[1,2]` in `robot.json` | ✅ |
| [5] Arm-Marker | Marker | `links.*.markers[].{position,spin}` visuell prüfen und korrigieren | ✅ |

---

## [1] Camera NPZ — Kameraparameter

**Ziel:** Intrinsische Parameter (Brennweite, Verzerrungskoeffizienten, Kameramatrix)
für jede Kamera als `.npz`-Datei speichern.

**Ablauf:**
1. ChArUco-Board aus verschiedenen Winkeln fotografieren (mehrere Posen)
2. OpenCV `calibrateCamera()` berechnet Kameraparameter
3. Ergebnis als `.npz` speichern und an Webcam-Service übertragen

**Speichert:** `data/calibration/{session}/{cam}_calibration.npz`

**Wird verwendet von:** Script `1_detect_aruco_observations.py` beim Board-Run und Homing
(Argument `-npz`).

---

## [2] Board — Referenz-Marker-Positionen

**Ziel:** Absolute 3D-Positionen aller Board-ArUco-Marker im Welt-Koordinatensystem
bestimmen und in `robot.json` speichern.

**Ablauf:**
1. Foto mit allen Kameras (Snapshot)
2. Script `1_detect_aruco_observations.py` → `{cam}_aruco_detection.json`
3. Script `2_estimate_camera_from_observations.py` → `{cam}_camera_pose.json`
4. Script `3b_corner_marker_poses.py` → `aruco_marker_poses.json` (Triangulierung)
5. Positionen in `robot.json` unter `links.Board.markers` eintragen / bestätigen

**Aktionen im Board-Tab:**
- **Board erkennen**: startet den vollständigen Foto+Script-Durchlauf (SSE-Stream)
- **Marker zuordnen** (Z-Bereich, Set, Link): Marker manuell kategorisieren
- **Sets justieren** (Kabsch 2D+Z): Zwei Marker-Sets aufeinander ausrichten
- **Marker ID zuordnen / entfernen**: einzelne Marker-Korrekturen

**Speichert:** `links.Board.markers` in `robot.json`

---

## [3] X-Achse — Schieberichtung des Sliders

**Ziel:** X-Achse des Roboters (Slider-Richtung `Base → Arm1`) im Board-Koordinatensystem
ausrichten, sodass `[1, 0, 0]` der realen Bewegungsrichtung entspricht.

**Ablauf:**
1. Roboter auf Position A fahren → Board erkennen
2. Roboter auf Position B fahren (mind. 20 mm Unterschied) → Board erkennen
3. Board-Viewer berechnet den Richtungsvektor der Markerbewegungen
4. „X-Achse übernehmen": alle Marker-Positionen in `robot.json` werden rotiert,
   sodass die gemessene Richtung zur neuen X-Achse `[1, 0, 0]` wird

**Implementierung:** `editRobot.js` → `adoptXAxis()` — rotiert alle `links.*.markers[].position`
um den A0-Schwerpunkt als Ursprung.

**Speichert:** alle `links.*.markers[].position` in `robot.json` (rotiert)

---

## [4] Arm1 — Y-Rotationsachse (Schultergelenk)

**Ziel:** Lage und Richtung der Rotationsachse zwischen `Base` und `Arm1` bestimmen
und als `jointToParent.origin` in `robot.json` speichern.

**Ablauf:**
1. Board erkennen mit Arm in **Position A**
2. Arm1 um ≥ 15° drehen
3. Board erkennen (**Position B**)
4. Arm1 nochmals ≥ 15° drehen
5. Board erkennen (**Position C**)
6. Board-Viewer berechnet automatisch die Rotationsachse (magenta Linie im 3D-Viewer)
7. Aktion **„Joint-Origin Y/Z übernehmen"**: speichert Y/Z des Achspunkts in `robot.json`

**Optional — Aktion „Fixe Marker → Link Base":**  
Marker mit Bewegung < 10 mm über alle drei Positionen sind physisch am Basis-Körper
befestigt. Diese werden in `links.Base.markers` eingetragen.

**Speichert:**
- `links.Arm1.jointToParent.origin[1]` (Y) und `[2]` (Z) in `robot.json`
- Optional: `links.Base.markers` ergänzt

### Alternative/Ergänzung — `pose_estimation.fit_origin_link` (`5_pose_estimation.py`)

🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und
Vergleich zu Verfahren B: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt
„Kalibrier-Switch: Gelenk-Origin").

Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell
gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in
derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme
(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session
nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen
Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button,
nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine
konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / −19 bis
−20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine
frische Messung mit Verfahren B gegengeprüft.

---

### Mathematik: Bestimmung der Rotationsachse

Bei einer Rotation um die Y-Achse bewegt sich jeder erkannte Marker auf einem
**Kreisbogen** im 3D-Raum. Aus den beobachteten Marker-Positionen zu mehreren
Zeitstempeln lassen sich Lage und Richtung der Rotationsachse berechnen.

#### Wie viele Beobachtungen sind nötig?

Entscheidend ist nicht „2 oder 3 Positionen" allein, sondern die Kombination aus
Anzahl Marker und Anzahl Positionen:

| Beobachtungen | Bestimmbar? | Begründung |
|---|---|---|
| 1 Marker, 2 Positionen | **Nein** | Achse liegt *irgendwo* in der Mittelsenkrechten-Ebene der Sehne; Richtung und Lage bleiben unterbestimmt |
| **2 Marker, je 2 Positionen** | **Ja** | **Verfahren A**: Richtung aus d₁ × d₂, Lage aus Schnitt zweier Mittelsenkrechten-Ebenen |
| 1 Marker, 3 Positionen | **Ja** | **Verfahren B**: drei Punkte definieren einen eindeutigen Umkreis → eindeutige Achse |
| N Marker, je ≥ 2 bzw. ≥ 3 Positionen | Ja, überbestimmt | Least-Squares, robust gegenüber Messrauschen, erlaubt Fehlerabschätzung |

Die häufige Kurzbehauptung „zwei Positionen reichen nicht" gilt nur für **einen
einzigen** Marker. Sobald **zwei** (nicht-entartete) Marker vorliegen, genügen
zwei Positionen — siehe Verfahren A.

> Die aktuelle Implementierung (`public/yAxisCompute.js`) verwendet **Verfahren B**.

#### Verfahren A — Zwei Marker, je zwei Positionen

Gegeben: M₁ₐ, M₁ᵦ (Marker 1 zu Zeit a, b) und M₂ₐ, M₂ᵦ (Marker 2 zu Zeit a, b).
Beide Marker sitzen am selben starren Körper, der zwischen a und b um die gesuchte
Achse rotiert. Verschiebungsvektoren (Sehnen der Kreisbögen):

```
d₁ = M₁ᵦ − M₁ₐ
d₂ = M₂ᵦ − M₂ₐ
```

**Schritt 1 – Achsenrichtung.** Jeder Punkt bewegt sich in einer Ebene **senkrecht**
zur Achse; die Sehne liegt in dieser Ebene, also `d₁ ⊥ n` und `d₂ ⊥ n`:

```
n = (d₁ × d₂) / |d₁ × d₂|     ← Einheitsvektor entlang der Rotationsachse
```

**Schritt 2 – Achsenlage.** Der Kreismittelpunkt eines Markers ist von Start- und
Endposition gleich weit entfernt, liegt also in der **Mittelsenkrechten-Ebene** der
Sehne. Da zusätzlich `n ⊥ dᵢ`, liegt die **gesamte Achse** in dieser Ebene.

```
Ebene 1:  d₁ · (x − P₁) = 0,   P₁ = (M₁ₐ + M₁ᵦ) / 2
Ebene 2:  d₂ · (x − P₂) = 0,   P₂ = (M₂ₐ + M₂ᵦ) / 2
```

Die Achse ist die **Schnittgerade** beider Ebenen mit Richtung `n`. Ein Punkt A auf
ihr (Ansatz A = α·d₁ + β·d₂, Komponente entlang n zu 0 gesetzt):

```
c₁ = d₁ · P₁,   c₂ = d₂ · P₂
D  = |d₁|²·|d₂|² − (d₁ · d₂)²        (= |d₁ × d₂|²)

α  = (c₁·|d₂|² − c₂·(d₁ · d₂)) / D
β  = (c₂·|d₁|² − c₁·(d₁ · d₂)) / D

A  = α·d₁ + β·d₂                      ← Referenzpunkt auf der Achse
```

Ergebnis: `r(t) = A + t·n`.

**Entartung:** `D = 0` ⟺ `d₁ ∥ d₂` ⟺ beide Marker liegen **mit der Achse in einer
gemeinsamen Ebene**. Dann sind die Mittelsenkrechten-Ebenen parallel und schneiden
sich nicht eindeutig. Erkennung über `|d₁ × d₂| / (|d₁|·|d₂|) ≈ 0`; auch ein Marker
direkt auf der Achse liefert `dᵢ ≈ 0`. → anderen Marker wählen oder Verfahren B.

#### Verfahren B — Ein Marker, drei Positionen *(implementiert)*

Gegeben P₁, P₂, P₃ desselben Markers zu drei Drehwinkeln. Die drei Punkte liegen
auf einem Kreis, dessen Ebene senkrecht zur Achse steht.

**Schritt 1 — Achsenrichtung** (Normalenvektor der Kreisebene):
```
v₁ = P₂ − P₁
v₂ = P₃ − P₁
n  = normalize(v₁ × v₂)
```

**Schritt 2 — Umkreismittelpunkt** (Punkt auf der Achse), über baryzentrische Gewichte:
```
a² = |P₂ − P₃|²,   b² = |P₁ − P₃|²,   c² = |P₁ − P₂|²

w₁ = a²·(b² + c² − a²)
w₂ = b²·(a² + c² − b²)
w₃ = c²·(a² + b² − c²)

C = (w₁·P₁ + w₂·P₂ + w₃·P₃) / (w₁ + w₂ + w₃)
```

Ergebnis: `r(t) = C + t·n`. **Entartung:** P₁,P₂,P₃ kollinear (zu kleiner Drehwinkel)
→ `|v₁ × v₂| ≈ 0`, Umkreis numerisch schlecht bestimmt.

#### Kombination über N Marker (Least Squares)

Beide Verfahren lassen sich über mehrere Marker mitteln — robuster und mit
Fehlerabschätzung.

**Achsenrichtung gemeinsam.** Aus `dᵢ ⊥ n` (bzw. Ebenen-Normalen) minimiert die beste
Richtung `Σ (dᵢ · n)²` unter `|n| = 1`:
```
M = Σ dᵢ · dᵢᵀ          (3×3-Matrix)
n = Eigenvektor von M zum kleinsten Eigenwert
```
(Verallgemeinert das Kreuzprodukt. Verfahren B alternativ: alle Normalen nᵢ aufs
gleiche Halbraum ausrichten und mitteln.)

**Achsenlage gemeinsam.** Für Verfahren B (Umkreismittelpunkte Cᵢ) ergibt sich der
einfache Schwerpunkt:
```
axisDir   = normalize( mean(nᵢ) )    ← Vorzeichen vorher angleichen
axisPoint = mean(Cᵢ)                 ← Schwerpunkt der Umkreismittelpunkte
```

**Residuum pro Marker** (Verfahren B, Fehler- / Ausreißer-Erkennung):
```
εᵢ = |(Cᵢ − C̄) − ((Cᵢ − C̄)·n̄)·n̄|
```
Große εᵢ deuten auf einen fehlerhaften Marker oder eine nicht-rotatorische
Bewegungskomponente hin.

Die Y/Z-Koordinaten von `axisPoint` geben an, wo die Rotationsachse durch die
YZ-Ebene geht — das ist der gesuchte `jointToParent.origin`.

#### Genauigkeit & Verfahrenswahl

Beide Verfahren sind im **rauschfreien** Fall exakt; die Unterschiede liegen in
Robustheit und Aufwand:

| Kriterium | Verfahren A (2 Marker, 2 Bilder) | Verfahren B (1 Marker, 3 Bilder) |
|---|---|---|
| Mindest-Aufnahmen | 2 Bilder | 3 Bilder |
| Mindest-Marker | 2 (gut separiert) | 1 |
| Eigenständige Fehler-Schätzung | nur über mehrere Marker | pro Marker (Residuum εᵢ) |
| Kritische Entartung | d₁ ∥ d₂ (Marker koplanar mit Achse) | P₁,P₂,P₃ kollinear (kleiner Winkel) |
| Empfindlichkeit | hoch, wenn Sehnen fast parallel oder Drehwinkel klein | hoch, wenn Drehwinkel klein (Bogen fast gerade) |

**Beide** brauchen einen ausreichend großen Drehwinkel: kleine Winkel liefern kurze
Sehnen bzw. fast gerade Bögen, in denen das Messrauschen dominiert.

- **Verfahren A** ist schneller (zwei statt drei Aufnahmen), sinnvoll bei ≥ 2 gut
  getrennten Markern.
- **Verfahren B** ist robuster für die **Fehlerrechnung**: jeder Marker liefert
  unabhängig eine vollständige Achsen-Schätzung, das Residuum εᵢ erlaubt
  Ausreißer-Erkennung, keine Entartung zwischen Markern. Für belastbare Genauigkeit
  daher vorzuziehen — und der Grund, weshalb die Implementierung Verfahren B nutzt.

**Praktische Werte (beide Verfahren):**
- Drehwinkel zwischen den Positionen ≥ 15° (besser ≥ 30°), sonst numerisch instabil.
- 3–4 Marker mit guter räumlicher Verteilung um die Achse.
- Die Rotationswinkel müssen **nicht** bekannt sein – nur die 3D-Koordinaten.

**Marker-Filter (`min_movement_mm = 10`):**  
Marker, die sich zwischen A/B/C weniger als 10 mm bewegen, sind nicht am rotierenden
Teil befestigt und werden aus der Achsenberechnung ausgeschlossen. Ihre Position A
wird für die optionale Base-Link-Zuweisung gespeichert.

---

### Implementierungsnachweis

Die Implementierung in `public/yAxisCompute.js` setzt **Verfahren B** um:

| Schritt | Implementierung | Verifikation |
|---------|----------------|--------------|
| Normalenvektor | `yAxisCompute.js` Z. 51–61 | Kreuzprodukt v₁×v₂, normiert ✓ |
| Umkreismittelpunkt | `yAxisCompute.js` Z. 63–76 | Baryzentrische Gewichte ✓ |
| Mittelung Normalen | `yAxisCompute.js` Z. 153–162 | Vorzeichen-Alignment + mean + renormieren ✓ |
| Mittelung Achspunkte | `yAxisCompute.js` Z. 164–167 | Schwerpunkt der Cᵢ ✓ |
| Origin speichern | `calibration.js` → `setOriginBtn` sendet `axisPoint[1,2]` | `editRobot.js` schreibt `origin[1,2]` ✓ |

**Ergebnis: Implementierung entspricht der beschriebenen Mathematik (Verfahren B) vollständig.**

---

## [5] Arm-Marker — Spin-Verifikation und Korrektur

**Ziel:** Sicherstellen, dass die in `robot.json` eingetragenen Arm-Marker
(Position, Normale, Spin) mit dem realen Aufkleber übereinstimmen — insbesondere
die `spin`-Orientierung, die der Homing-Viewer korrekt darstellen muss.

**Ablauf:**
1. Arm-Marker physisch aufkleben und Position/Normale grob in `robot.json` eintragen
2. Homing-Run starten (Tab „Homing" oder `homing.html`)
3. Kalibrierung → Tab **„Marker"** öffnen:
   - Tabelle zeigt alle Arm-Marker mit aktuellem `spin`-Wert
   - Viewer zeigt 3D-Skeleton mit spin-orientiertem Marker-Quadrat und Orientierungszeiger (→ Ecke 0)
4. Visuelle Prüfung: zeigt die Viewer-Grafik dieselbe Orientierung wie der echte Aufkleber?
5. Falls nicht: Link und Marker-ID wählen → **−90° / +90° / 180°** klicken → Viewer lädt neu
6. Wiederholen bis Modell und Realität übereinstimmen

**Aktionen im Marker-Tab:**
- **Link-Dropdown + ID-Dropdown**: Marker auswählen
- **Spin-Buttons** (−90°, +90°, 180°): Spin in `robot.json` korrigieren und Viewer neu laden
- **Viewer** (boardViewer.html?mode=homing): zeigt Skeleton mit Marker-Quadraten und Orientierungszeigern

**Speichert:** `links.{Link}.markers[].spin` in `robot.json` (normalisiert auf [0, 360))

**Details und Roadmap:** [`doc/Kalibrierung_Marker.md`](Kalibrierung_Marker.md)

---

## Dateistruktur

```
data/
  calibration/
    {session}/
      meta.json
      {cam}_calibration.npz          ← Kameraparameter
      {cam}_aruco_detection.json     ← Marker-Erkennung
      {cam}_camera_pose.json         ← Kamera-Pose
scripts/
  robot_1781069752019.json           ← robot.json (Haupt-Konfiguration)
    links.Board.markers              ← Board-Marker-Positionen
    links.Base.markers               ← fixe Basis-Marker (nach Y-Kalibrierung)
    links.Arm1.jointToParent.origin  ← Schultergelenk-Position (nach Y-Kalibrierung)
    links.Arm1.markers               ← Arm1-Marker (Nutzer eingetragen)
    ...
```