# Bestimmung der Y-Rotationsachse aus Marker-Positionen

## Problem

Wenn der Roboter um seine Y-Achse rotiert, bewegt sich jeder erkannte Marker auf einem **Kreisbogen** im 3D-Raum. Ziel ist es, Lage und Richtung dieser Rotationsachse aus den beobachteten Marker-Positionen (x, y, z) zu mehreren Zeitstempeln zu berechnen.

---

## Reichen zwei Positionen?

**Nein.** Zwei Punkte P₁, P₂ desselben Markers definieren nur eine Strecke. Die Rotationsachse muss durch die senkrechte Mittelebene dieser Strecke laufen – aber *wo* genau in dieser Ebene und in welcher Richtung bleibt unbestimmt. Das System ist unterbestimmt.

| Beobachtungen | Bestimmbar? | Begründung |
|---|---|---|
| 1 Marker, 2 Positionen | Nein | Achse liegt irgendwo auf einer Halbebene |
| 1 Marker, 3 Positionen | **Ja** | Eindeutiger Umkreis → eindeutige Achse |
| N Marker, je ≥ 3 Positionen | Ja, überbestimmt | Least-Squares, robuster gegenüber Messrauschen |

---

## Mathematik: 1 Marker, 3 Positionen

Gegeben: P₁, P₂, P₃ ∈ ℝ³ (derselbe Marker zu drei verschiedenen Rotationswinkeln).

Da die Rotation starr ist, liegen P₁, P₂, P₃ auf einem **Kreis**, dessen Ebene senkrecht zur Rotationsachse steht.

### Schritt 1 – Achsenrichtung (Normalenvektor der Kreisebene)

```
v₁ = P₂ − P₁
v₂ = P₃ − P₁
n  = (v₁ × v₂) / |v₁ × v₂|     ← Einheitsvektor entlang der Rotationsachse
```

### Schritt 2 – Mittelpunkt des Kreises (Umkreismittelpunkt)

Der Umkreismittelpunkt C liegt in der Ebene der drei Punkte und ist von allen drei Punkten gleich weit entfernt. Er ist ein Punkt **auf der Rotationsachse**.

Berechnung über Baryzentrischen Koordinaten:

```
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₃)
```

### Schritt 3 – Rotationsachse

```
Achse: r(t) = C + t·n,  t ∈ ℝ
```

---

## Mathematik: N Marker, je ≥ 3 Positionen (Least Squares)

Jeder Marker i liefert:
- einen Schätzwert **Cᵢ** (Umkreismittelpunkt, Punkt auf der Achse)
- einen Schätzwert **nᵢ** (Normalenvektor, Achsenrichtung)

### Achsenrichtung (gemittelt / PCA)

Wegen möglichem Vorzeichenambiguität alle nᵢ auf dasselbe Halbraum ausrichten, dann:

```
n̄ = mean(nᵢ) / |mean(nᵢ)|
```

Robuster: **PCA** über die Matrix der nᵢ-Vektoren → erster Hauptkomponentenvektor.

### Achsenposition (Least Squares)

Minimiere die quadratischen Abstände aller Umkreismittelpunkte Cᵢ zur gesuchten Geraden `r(t) = A + t·n̄`:

```
Abstand² von Cᵢ zur Achse = |Cᵢ − A|² − ((Cᵢ − A)·n̄)²
```

Ableitung null setzen liefert:

```
A = C̄ − (C̄·n̄)·n̄ + (C̄·n̄)·n̄ = C̄    (Referenzpunkt = Schwerpunkt der Cᵢ)
```

d. h., der **Schwerpunkt der Umkreismittelpunkte** ist der optimale Referenzpunkt auf der Achse. Die Achsenrichtung n̄ bleibt unverändert.

**Verbleibender Fehler** (Residuum pro Marker):

```
εᵢ = |(Cᵢ − C̄) − ((Cᵢ − C̄)·n̄)·n̄|
```

Große εᵢ deuten auf einen fehlerhaften Marker oder eine nicht-rotatorische Bewegungskomponente hin.

---

## Praktische Empfehlung

- **Mindestens 3 Zeitstempel** pro Marker, verteilt über einen Winkelbereich von ≥ 30° (sonst ist der Kreis numerisch schlecht konditioniert).
- **3–4 Marker** mit je 3 Positionen sind ausreichend für eine stabile Schätzung.
- Die Rotationswinkel müssen nicht bekannt sein – nur die 3D-Koordinaten der Marker.
- Bei sehr kleinen Winkeln (< 10°) ist die Bestimmung numerisch instabil; größere Drehungen bevorzugen.

---

## Zusammenfassung

```
Eingabe:  Marker-Positionen P[marker][timestamp] ∈ ℝ³
           (mind. 3 Timestamps pro Marker, mind. 1 Marker)

Pro Marker:
  1.  n_i = normalize((P2-P1) × (P3-P1))          ← Achsenrichtung
  2.  C_i = Umkreismittelpunkt(P1, P2, P3)         ← Punkt auf Achse

Kombination:
  3.  n̄   = normalize(mean(n_i))                  ← beste Achsenrichtung
  4.  C̄   = mean(C_i)                             ← bester Referenzpunkt

Ergebnis: Rotationsachse  r(t) = C̄ + t·n̄
```