Compare commits
2 Commits
222f0e55f7
...
773e32c51c
| Author | SHA1 | Date | |
|---|---|---|---|
|
773e32c51c | ||
|
|
f7358cea8d |
3437
data/evaluations/Scene8/scene_reconstruction.json
Normal file
3437
data/evaluations/Scene8/scene_reconstruction.json
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
357
doc/callibrate_scene_roadmap.md
Normal file
357
doc/callibrate_scene_roadmap.md
Normal file
@@ -0,0 +1,357 @@
|
|||||||
|
# Roadmap: Szenen-Kalibrierung der Board-/Loose-Marker (`callibrate_scene`)
|
||||||
|
|
||||||
|
Status: **Vorschlag / zur Abstimmung**
|
||||||
|
Ort der Arbeit: `pipeline/` (nicht `approbot-pipeline/`, das bleibt die eingefrorene Kopie)
|
||||||
|
Datum: 2026-06-04
|
||||||
|
|
||||||
|
> Eingearbeitete Entscheidungen:
|
||||||
|
> 1. **Gelenkzustand UNBEKANNT** → wird mitgeschätzt (kein FK-Welt-Anker a priori).
|
||||||
|
> 2. **Set-Definition direkt in `robot.json`** über ein optionales `"set"`-Feld je Marker.
|
||||||
|
> Marker bleiben im bisherigen Format an ihrem Link. Gleiches `set` = ein starr zusammenhängendes
|
||||||
|
> Objekt mit **fixen relativen Bezügen**. **Kein `set` = loser Marker** (fix, aber Lage z.Zt. unbekannt).
|
||||||
|
> 3. **Welt = Roboter (Konvention 2)**, Roboter steht *nicht* bei 0/0/0.
|
||||||
|
> 4. **Primär eine Aufnahme (7 Bilder)**, *ohne* zusätzliche Base-Marker; mehrere Posen als Fallback.
|
||||||
|
> 5. **Ausgabe vorerst `robot.calibrated.json`** (Debugging); später in-place nach `robot.json`.
|
||||||
|
> 6. **Code generisch & `robot.json`-unabhängig.** `robot.json` ist nur ein Beispiel und wird später
|
||||||
|
> gegen ein anderes geprüft. KEINE festen Marker-IDs, Link-/Set-Namen, Achsen oder Gelenk-Variablen
|
||||||
|
> im Code (Auto-Discovery aus den Daten). Daten-spezifisches (z.B. die `set`-Zuordnung) gehört in
|
||||||
|
> `robot.json`, nicht in den Code.
|
||||||
|
> 7. **Aktuelle Marker-Positionen = brauchbare Startwerte.** Die relativen Bezüge innerhalb jedes Sets
|
||||||
|
> gelten als korrekt → direkte Grundlage für den Kabsch-Fit und die BA-Initialisierung.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 0. Machbarkeit — Kurzurteil
|
||||||
|
|
||||||
|
**Ja, machbar** — die anspruchsvollere Variante, weil der Gelenkzustand mitgeschätzt wird und es
|
||||||
|
keinen a-priori Welt-Anker gibt (weder Board noch Arm). Vorgehen:
|
||||||
|
|
||||||
|
> **Erst ankerlos rekonstruieren** (metrisch, aus der bekannten Marker-Größe), **dann den Roboter
|
||||||
|
> in die Rekonstruktion einpassen** — der eingepasste Roboter definiert die Welt — **dann die Sets
|
||||||
|
> einpassen** und die Marker-Positionen aktualisieren.
|
||||||
|
|
||||||
|
Bausteine im Bestand: Per-Marker-PnP (`solve_single_marker_pose`), Eck-Triangulation
|
||||||
|
(`3b_corner_marker_poses.py`), Bündelausgleichung (`3_multiview_bundle_adjustment_v5...`),
|
||||||
|
FK + θ-Schätzung (`pose_estimation.py`, `robot_fk.py`), Kabsch-Fit (`rigid_transform_no_scale`).
|
||||||
|
|
||||||
|
**Ein Beobachtbarkeits-Knackpunkt** für „eine Aufnahme genügt ohne Base-Marker" steht in §7 — er ist
|
||||||
|
beherrschbar, aber bewusst zu entscheiden.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. Problem & Zielbild
|
||||||
|
|
||||||
|
**Realität:** Höhe/Orientierung zwischen Board und Arm sind ungenau. Marker ~20–105 liegen fix,
|
||||||
|
aber unbekannt: teils Board-Platte, teils darunterliegendes A0-Blatt, teils einzeln aufgeklebt.
|
||||||
|
|
||||||
|
**Vertrauenswürdig:** die *interne Geometrie* der Roboter-Links und die *relativen Bezüge innerhalb
|
||||||
|
jedes Sets* (beide in `robot.json` hinterlegt), sowie die *Marker-Kantenlänge* (25 mm → Maßstab).
|
||||||
|
**Unbekannt:** Gelenkwinkel, Kamera-Posen, Platzierung jedes Sets, Lage jedes losen Markers.
|
||||||
|
|
||||||
|
**Ziel:** Nach der Kalibrierung hat jedes Set (als starres Objekt korrekt platziert) und jeder lose
|
||||||
|
Marker (einzeln vermessen) eine korrekte Pose in einem roboter-verankerten Weltsystem.
|
||||||
|
|
||||||
|
### Marker-Klassen (aus dem `set`-Feld abgeleitet)
|
||||||
|
|
||||||
|
| Klasse | Erkennung | bekannt | zu schätzen |
|
||||||
|
|---|---|---|---|
|
||||||
|
| **Arm-Marker** (Roboter) | liegen an Arm-Links (Arm1…Finger) | Lage je Link | — definieren via Fit die Welt |
|
||||||
|
| **Set-Marker** (starr) | `"set": "A0"`, `"set":"Brett"`, … | interne Relativlage (fix) | 6-DoF-Platzierung je Set |
|
||||||
|
| **Lose Marker** | **kein** `set`-Feld | nur „fix vorhanden" | je Marker eigene 6-DoF-Pose |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. Set-Definition: `set`-Feld am Marker (kein Strukturumbau)
|
||||||
|
|
||||||
|
Marker bleiben **wie bisher** in der `markers`-Liste ihres Links. Ein optionales `"set"` gruppiert sie:
|
||||||
|
|
||||||
|
```jsonc
|
||||||
|
"Board": {
|
||||||
|
"parent": null, "mountPosition": [0,0,0], "mountRotation": [0,0,0],
|
||||||
|
"markers": [
|
||||||
|
{ "id": 210, "position": [ 20,-20,0.3], "normal": [0,0,1], "set": "A0" },
|
||||||
|
{ "id": 211, "position": [250,-10,0.3], "normal": [0,0,1], "set": "A0" },
|
||||||
|
{ "id": 215, "position": [250,-90,0.3], "normal": [0,0,1], "set": "Brett" },
|
||||||
|
{ "id": 208, "position": [350,-90,0.3], "normal": [0,0,1], "set": "Brett" },
|
||||||
|
{ "id": 205, "position": [750,-90,0.3], "normal": [0,0,1] } // kein set -> lose
|
||||||
|
]
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- **Gleiches `set` ⇒ ein starres Objekt.** Die relativen Bezüge der Marker im Set gelten als **fix**;
|
||||||
|
die Kalibrierung bestimmt nur die 6-DoF-Platzierung des ganzen Sets und schreibt die daraus
|
||||||
|
resultierenden Positionen zurück (Format unverändert, relative Anordnung erhalten).
|
||||||
|
- **Kein `set` ⇒ loser Marker.** Wird einzeln (Position + Normale + ggf. Spin) vermessen.
|
||||||
|
- **Arm-Marker** brauchen kein `set`: ihr Link ist bereits ein starrer Körper und dient als
|
||||||
|
Roboter-Referenz (sie werden *nicht* kalibriert, sondern definieren die Welt).
|
||||||
|
- **Auto-Discovery** (Projekt-Konvention): Sets ergeben sich aus den `set`-Werten, nichts hartkodiert.
|
||||||
|
|
||||||
|
Hinweis: `robot_fk.py` / `all_markers_world()` bleiben unverändert — das `set`-Feld ist reine
|
||||||
|
Zusatzinfo, die nur der Kalibrier-Treiber auswertet.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. Algorithmus (Gelenkzustand unbekannt)
|
||||||
|
|
||||||
|
### Phase A — Ankerlose, metrische Rekonstruktion
|
||||||
|
1. **Detektion** (Schritt 1) → Ecken je Kamera.
|
||||||
|
2. **Per-Marker-PnP** je Kamera aus der bekannten Marker-Größe (`SOLVEPNP_IPPE_SQUARE`) → volle
|
||||||
|
Marker-Pose *relativ zur Kamera*. Kein Welt-Anker nötig.
|
||||||
|
3. **Relativer Posen-Graph:** gemeinsam gesehene Marker verknüpfen Kamerapaare → Init aller Kamera-
|
||||||
|
und Marker-Posen in einem *beliebigen* Szenen-Frame `S`.
|
||||||
|
4. **Globale Bündelausgleichung** (scipy `least_squares`, Huber): verfeinert alle Kamera- und
|
||||||
|
Marker-Posen über die Reprojektion aller Ecken. Maßstab fix durch Marker-Größe.
|
||||||
|
|
||||||
|
→ konsistente, metrische 3D-Szene (Arm- **und** Set-/Loose-Marker) in `S`.
|
||||||
|
|
||||||
|
### Phase B — Roboter einpassen = Welt definieren
|
||||||
|
5. Arm-Marker per ID → Link zuordnen. **Fit** von Gelenkwinkeln θ **und** der Platzierung der
|
||||||
|
FK-Wurzel in `S`, sodass `FK(θ)` der Arm-Marker die rekonstruierten Arm-Positionen trifft
|
||||||
|
(erweitert `pose_estimation.py` um eine freie Wurzel-Platzierung statt fixer Identität).
|
||||||
|
6. In das Weltsystem rücktransformieren. Welt-Ursprung = FK-Wurzel-Frame (= heutiges „Board"-Frame),
|
||||||
|
Roboter sitzt mit dem fertigen θ darin — *nicht* bei 0/0/0 (§6).
|
||||||
|
|
||||||
|
### Phase C — Set-Fit & Rückschreiben
|
||||||
|
7. **Set-Marker (pro `set`):** Kabsch (`rigid_transform_no_scale`) bildet die **fixe interne Lage**
|
||||||
|
auf die rekonstruierten Welt-Positionen ab → 6-DoF-Set-Platzierung → aktualisierte Positionen
|
||||||
|
(= Platzierung ∘ interne Lage). Auch nicht gesehene Set-Marker erhalten so eine Position, sofern
|
||||||
|
das Set über ≥3 nicht-kollineare Marker bestimmt ist.
|
||||||
|
**Lose Marker:** triangulierte Pose direkt übernehmen.
|
||||||
|
8. **Rückschreiben** nach `robot.calibrated.json` (Marker-Format unverändert, `set`-Felder erhalten)
|
||||||
|
+ `calibration_report.json` (je Set die explizite Verschiebung/Verdrehung + RMS; je Marker Status).
|
||||||
|
Nicht beobachtbare Größen → **`null`** (nie 0).
|
||||||
|
|
||||||
|
### Fallback — mehrere Posen (statische Kameras)
|
||||||
|
Mehrere Gelenkzustände bei festen Kameras: Kamera-Posen + Set-/Loose-Posen + Wurzel-Platzierung sind
|
||||||
|
**geteilte** Unbekannte, je Pose ein eigener θ-Satz. Löst die §7-Schwächen vollständig auf.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. Eingaben & Ausgaben
|
||||||
|
|
||||||
|
**Eingaben:** `robot.json` (Arm-Geometrie + `set`-Felder + fixe interne Set-Lagen);
|
||||||
|
Szenen-Ordner mit `render_*.png` **oder** vorhandene `*_aruco_detection.json`.
|
||||||
|
(Gelenkzustand wird NICHT benötigt.)
|
||||||
|
|
||||||
|
**Ausgaben:** `robot.calibrated.json` (aktualisierte Marker-Positionen, Format wie bisher);
|
||||||
|
`calibration_report.json` (je Set: Verschiebung/Verdrehung, RMS, #Kameras/#Marker, Status;
|
||||||
|
je losem Marker: Pose oder `null`). Optional Viewer-Overlay Soll↔kalibriert.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. Neue / geänderte Dateien
|
||||||
|
|
||||||
|
| Datei | Art | Inhalt |
|
||||||
|
|---|---|---|
|
||||||
|
| `pipeline/calibrate_scene.py` | **neu** | Treiber: Auto-Discovery Kameras+Sets, Phase A→B→C, schreibt `robot.calibrated.json`+Report |
|
||||||
|
| `pipeline/scene_reconstruct.py` | **neu** | Phase A: Per-Marker-PnP, Posen-Graph, globale BA (ankerlos) |
|
||||||
|
| `pipeline/robot_register.py` | **neu** | Phase B: Fit θ + freie Wurzel-Platzierung (nutzt `robot_fk`) |
|
||||||
|
| `pipeline/marker_sets.py` | **neu** | liest `set`-Felder aus `robot.json`; Klassifizierung Arm/Set/Lose |
|
||||||
|
| `3b_corner_marker_poses.py` | **erweitern** | volle Marker-**Rotation** (Normale + Spin) aus 4 Ecken |
|
||||||
|
| `pose_estimation.py` | **erweitern** | optionale freie Wurzel-Platzierung (für Phase B wiederverwendbar) |
|
||||||
|
|
||||||
|
`2_estimate_camera_from_observations.py` / `robot_fk.py`: voraussichtlich **unverändert**.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. Weltursprung (Konvention 2, Roboter nicht bei 0/0/0)
|
||||||
|
|
||||||
|
- Ursprung = FK-Wurzel-Frame (heute „Board"). Der Roboter sitzt mit seinem modellierten Versatz
|
||||||
|
(`Base.jointToParent.origin` + Slider `x`) darin → **nicht** bei 0/0/0. Das deckt den Wunsch ab.
|
||||||
|
- „Welt durch Roboter definiert" wird dadurch realisiert, dass die **Kalibrierung am Roboter
|
||||||
|
verankert** (Fit θ + Wurzel-Platzierung über Arm-Marker), statt den Board-Markern zu vertrauen.
|
||||||
|
Die Board-Positionen werden konsistent *neu* abgeleitet.
|
||||||
|
- Der Kinematik-Baum bleibt unverändert. (Optionaler späterer Umbau „Base = Wurzel" möglich, aber
|
||||||
|
für die Kalibrierung nicht nötig.)
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. Beobachtbarkeit — Einzelaufnahme ohne Base-Marker (wichtig)
|
||||||
|
|
||||||
|
Verifiziert an `robot.json`: `Base`, `Hand`, `Palm` haben **keine** Marker; erster markierter Link
|
||||||
|
ist `Arm1`. Daraus folgt für EINE Pose mit unbekannten Gelenkwinkeln:
|
||||||
|
|
||||||
|
- **Slider `x` und `Joint1 y` sind nicht von der absoluten Roboter-Platzierung trennbar** (2-DoF-
|
||||||
|
Gauge-Freiheit): eine Verschiebung entlang der Schiene ≙ Änderung von `x`; eine Drehung um die
|
||||||
|
`Joint1`-Achse ≙ Änderung von `y`. Die Set-/Loose-Marker erben diese 2 Freiheitsgrade.
|
||||||
|
- **Gut bestimmt aus einer Pose:** `z, a, b, c, e` und damit die gesamte Szene *relativ*.
|
||||||
|
|
||||||
|
Da Base-Marker mechanisch unerwünscht sind, der empfohlene Weg:
|
||||||
|
|
||||||
|
- **Gauge per Konvention fixieren** (Default für Einzelaufnahme): `x`,`y` auf die gefittete
|
||||||
|
Konfiguration / nominale Schienen-Null setzen und die Welt so definieren. Ergebnis ist
|
||||||
|
**in sich konsistent** → für künftige Pose-Schätzung (Board als Anker) voll nutzbar; lediglich
|
||||||
|
der *absolute* Schienen-Nullpunkt und die `Joint1`-Null sind dann Konvention, kein Messwert.
|
||||||
|
- **Mehrere Posen** (Fallback), wenn die absolute Basis-Lage / absolute `x`,`y` wirklich gebraucht
|
||||||
|
werden — das löst die 2 Freiheitsgrade vollständig auf.
|
||||||
|
- *(optional, falls je möglich:* ein einzelner Base-/Schlitten-Marker würde Einzelaufnahme voll
|
||||||
|
beobachtbar machen — derzeit zurückgestellt.)*
|
||||||
|
|
||||||
|
QA: Reprojektions-RMS je Kamera; Set-Fit-Residuum (mm); Co-Visibility-Graph zusammenhängend?;
|
||||||
|
≥3 nicht-kollineare Marker je Set; ≥2 Kameras je losem Marker (sonst Status `partial`/`unobserved`).
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. Validierung (Sim zuerst)
|
||||||
|
|
||||||
|
`pose.json` liefert in der Simulation GT-Gelenkwinkel **und** Kamera-Pos/Targets:
|
||||||
|
1. Bekannte Sets künstlich verschieben/verdrehen → kalibrieren → Rück-Transform gegen Soll.
|
||||||
|
2. Gefittete θ gegen GT-θ; gefittete Kamera-Posen gegen GT.
|
||||||
|
3. Einzelaufnahme- vs. Mehrfach-Posen-Genauigkeit quantifizieren (belegt §7).
|
||||||
|
4. Erst danach `data/recorded/`-Szenen.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 9. Phasen / Meilensteine
|
||||||
|
|
||||||
|
- **P0 — `set`-Felder & Parser:** `set`-Felder in `robot.json` ergänzen; `marker_sets.py`
|
||||||
|
(Arm/Set/Lose-Klassifizierung); FK-Welt-Positionen unverändert verifizieren.
|
||||||
|
- **P1 — Ankerlose Rekonstruktion (Phase A):** Per-Marker-PnP + Posen-Graph + globale BA; gegen
|
||||||
|
GT-Kamera-Posen (Sim) prüfen.
|
||||||
|
- **P2 — Roboter-Registrierung (Phase B):** Fit θ + freie Wurzel-Platzierung; gegen GT-θ; §7-Gauge.
|
||||||
|
- **P3 — Set-Fit & Rückschreiben (Phase C):** Kabsch + Loose → `robot.calibrated.json` + Report;
|
||||||
|
Sim-Validierung mit künstlichem Offset.
|
||||||
|
- **P4 — Mehrfach-Posen-Fallback.**
|
||||||
|
- **P5 — Reale Szenen + Viewer-Overlay; danach in-place nach `robot.json`.**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 10. Verbleibende kleinere Punkte
|
||||||
|
|
||||||
|
1. **Gauge-Konvention für Einzelaufnahme** (§7): `x`,`y` = gefittet, oder Schiene/`Joint1` auf
|
||||||
|
nominal? (Beeinflusst nur den absoluten Nullpunkt, nicht die Set-Relativlagen.)
|
||||||
|
2. **Set-Namensraum:** sind `set`-Namen global eindeutig oder pro Link? (Vorschlag: global, z.B.
|
||||||
|
„A0", „Brett" — ein Set = ein physisches Objekt.)
|
||||||
|
3. **Lose-Marker-Orientierung:** reicht Position + Normale, oder wird der Spin (Drehung um die
|
||||||
|
Normale) gebraucht? (Bestimmt die nötige Genauigkeit von Phase C / 3b.)
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 11. Umsetzungs-Log: Mathematik & Anwendung
|
||||||
|
|
||||||
|
Pro abgeschlossener Phase: *was* gemacht wurde, *welche Mathematik* dahinter steckt, *wie* man es
|
||||||
|
anwendet. (Wird mit jeder Phase fortgeschrieben.)
|
||||||
|
|
||||||
|
### P0 — Marker-Klassifizierung & `set`-Tags — ✅ erledigt (2026-06-04)
|
||||||
|
|
||||||
|
**Was gemacht**
|
||||||
|
- Neu: `pipeline/marker_sets.py` — liest ein beliebiges `robot.json`, klassifiziert jeden Marker in
|
||||||
|
`arm` / `set` / `loose` und gibt einen Report aus. Vollständig generisch (keine festen Namen/IDs).
|
||||||
|
- Daten: `data/robot/robot.json` mit `set`-Tags versehen — `Brett` (9 Board-Oberflächen-Marker,
|
||||||
|
z≈0.3) und `A0` (60 Papier-Marker, z≈−27.3). Chirurgisch eingefügt (kompaktes Custom-Format der
|
||||||
|
Datei bleibt erhalten, nur +1 Zeile), FK numerisch exakt invariant.
|
||||||
|
|
||||||
|
**Mathematik / Logik**
|
||||||
|
|
||||||
|
1) *Statisch (Welt) vs. beweglich (Roboter):* Ein Link `L` ist beweglich, wenn auf dem Pfad
|
||||||
|
`L → Wurzel` mindestens ein Gelenk vom Typ revolute/linear liegt:
|
||||||
|
```
|
||||||
|
movable(L) = ∃ A ∈ chain(L→root): type(jointToParent(A)) ∈ {revolute, linear}
|
||||||
|
```
|
||||||
|
Die Wurzel und nur über `fixed` angebundene Links sind statisch (= Welt). Generisch, da nur
|
||||||
|
Gelenk-Typen geprüft werden — keine Link-Namen.
|
||||||
|
|
||||||
|
2) *Rollen* eines Markers `m` auf Link `L`:
|
||||||
|
```
|
||||||
|
role(m) = arm falls movable(L) → Welt-Referenz, NICHT kalibriert
|
||||||
|
= set(s) falls ¬movable(L) ∧ m.set = s → starres Objekt, 6-DoF kalibrieren
|
||||||
|
= loose falls ¬movable(L) ∧ m hat kein set → einzeln vermessen
|
||||||
|
```
|
||||||
|
|
||||||
|
3) *Modell eines starren Sets `S`* (die eigentliche Kalibriergröße der Phase C):
|
||||||
|
Marker `i` hat eine **bekannte, fixe** lokale Lage `p_iˡᵒᵏ` (die vertrauten relativen Bezüge).
|
||||||
|
Das Set hat eine **unbekannte** starre Platzierung `(R_S, t_S) ∈ SE(3)`:
|
||||||
|
```
|
||||||
|
p_iʷᵉˡᵗ = R_S · p_iˡᵒᵏ + t_S R_S ∈ SO(3) (Verdrehung), t_S ∈ ℝ³ (Verschiebung)
|
||||||
|
```
|
||||||
|
`(R_S, t_S)` = die gesuchten 6 DoF je Set. Schätzung aus gemessenen Welt-Positionen `q_i`
|
||||||
|
per **Kabsch / orthogonalem Procrustes** (ohne Skalierung):
|
||||||
|
```
|
||||||
|
min_{R∈SO(3), t} Σ_i ‖ R·p_iˡᵒᵏ + t − q_i ‖²
|
||||||
|
p̄=mean(pˡᵒᵏ), q̄=mean(q), H = Σ (p_iˡᵒᵏ−p̄)(q_i−q̄)ᵀ, U Σ Vᵀ = svd(H)
|
||||||
|
R = V·diag(1,1,det(V Uᵀ))·Uᵀ, t = q̄ − R·p̄
|
||||||
|
```
|
||||||
|
(vorhanden als `rigid_transform_no_scale`). Weil die aktuellen `robot.json`-Positionen brauchbare
|
||||||
|
Startwerte sind und die relativen Bezüge stimmen, gilt `p_iˡᵒᵏ` = aktuelle Set-Positionen
|
||||||
|
(ggf. zentriert) und `(R_S,t_S) ≈ Identität` als Initialwert.
|
||||||
|
|
||||||
|
4) *Loser Marker:* eigene unbekannte Pose `(R_m, t_m)`, einzeln aus den triangulierten Ecken
|
||||||
|
(Position = Eckmittel, Orientierung aus Eckebene + Eckreihenfolge).
|
||||||
|
|
||||||
|
5) *FK-Invarianz:* `set` ist reine Metadaten; `p^welt = T_Link(θ)·p^lok` bleibt unberührt
|
||||||
|
(verifiziert: max |Δ| = 0).
|
||||||
|
|
||||||
|
**Anwendung**
|
||||||
|
```
|
||||||
|
# Report (Mensch):
|
||||||
|
python pipeline/marker_sets.py -robot data/robot/robot.json
|
||||||
|
# Report (Maschine):
|
||||||
|
python pipeline/marker_sets.py -robot data/robot/robot.json --json
|
||||||
|
```
|
||||||
|
```python
|
||||||
|
from marker_sets import load_robot, classify_markers, get_sets, get_loose_markers, get_arm_markers
|
||||||
|
data = load_robot("data/robot/robot.json")
|
||||||
|
sets = get_sets(data) # {"A0":[MarkerInfo,...], "Brett":[...]}
|
||||||
|
loose = get_loose_markers(data) # [MarkerInfo,...]
|
||||||
|
arm = get_arm_markers(data) # {id: MarkerInfo}
|
||||||
|
```
|
||||||
|
*Neues `robot.json` vorbereiten:* an jeden Marker eines starren Objekts `"set": "<name>"` ergänzen
|
||||||
|
(Name frei wählbar, ein Set = ein physisches Objekt); lose Marker ohne `set`; Arm-Marker (an
|
||||||
|
beweglichen Links) brauchen keinen Eintrag. Der Code liest die Sets dann automatisch.
|
||||||
|
|
||||||
|
### P1 — Ankerlose, metrische Rekonstruktion (Phase A) — ✅ erledigt (2026-06-04)
|
||||||
|
|
||||||
|
**Was gemacht**
|
||||||
|
- Neu: `pipeline/scene_reconstruct.py` — rekonstruiert aus den ArUco-Eckbeobachtungen ALLER Kameras
|
||||||
|
die Kamera- und Marker-Posen in einem gemeinsamen Frame `S`, **ohne** Welt-Anker, **ohne**
|
||||||
|
`robot.json` (nur Marker-Kantenlänge nötig). Vollständig generisch.
|
||||||
|
|
||||||
|
**Mathematik**
|
||||||
|
|
||||||
|
Konventionen: `E_c` = world(S)→Kamera c, `G_m` = Marker-lokal→world(S), also `M_{c←m} = E_c · G_m`.
|
||||||
|
|
||||||
|
1) *Per-Marker-PnP (tentativ):* für jedes (Kamera c, Marker m) löst IPPE_SQUARE die Pose eines
|
||||||
|
Quadrats bekannter Größe. **Problem:** ein planares Quadrat hat eine **2-fache Flip-Ambiguität**
|
||||||
|
→ naive Verkettung liefert ~⅓ gespiegelte Knoten (empirisch verifiziert).
|
||||||
|
|
||||||
|
2) *Flip-robuste Initialisierung* (Referenzkamera `c0`, `E_{c0}=I`), iterativ:
|
||||||
|
- **Kamera-Pose per RANSAC-PnP** gegen die bereits platzierten 3D-Ecken
|
||||||
|
`X_S = G_m · corner_local`. RANSAC verwirft gespiegelte Marker als Ausreißer.
|
||||||
|
- **Marker-Ecken triangulieren** (lineares DLT über alle platzierten Kameras) — Triangulation
|
||||||
|
ist **flip-frei**; daraus Marker-Pose via Kabsch `local→tri`. Korrigiert Flips aus Schritt 1.
|
||||||
|
- Marker mit nur 1 Kamera sind nicht triangulierbar → als `insufficient_views` markiert
|
||||||
|
(unbeobachtbar, **nicht** rekonstruiert; Konvention „unbekannt = null").
|
||||||
|
|
||||||
|
3) *Globale Bündelausgleichung* (`scipy.least_squares`, robuste Huber-Loss, **dünnbesetzte**
|
||||||
|
Jacobi): minimiert die Reprojektion aller Ecken
|
||||||
|
```
|
||||||
|
min_{E_c, G_m} Σ_{(c,m)} Σ_{k=1..4} ρ( ‖ π(K_c, D_c; E_c·G_m·corner_k) − u_{c,m,k} ‖ )
|
||||||
|
```
|
||||||
|
Gauge: Referenzkamera `E_{c0}=I` fix (entfernt die 6-DoF-Starrkörperfreiheit).
|
||||||
|
|
||||||
|
4) *Similarity-Gauge / Skala:* Ankerlose SfM bestimmt die Struktur nur bis auf eine **7-DoF-
|
||||||
|
Ähnlichkeit** (Rotation, Translation, **Skala**). Der absolute Maßstab kommt hier provisorisch aus
|
||||||
|
der angenommenen Markergröße — empirisch ~0.93× gegenüber der echten Welt (konsistent über alle
|
||||||
|
Szenen, also ein systematischer Markergrößen-/Rand-Versatz). **Die echte Skala + Lage fixiert
|
||||||
|
erst Phase B über die bekannte mm-Geometrie des Roboters** (robuster als die Markergröße). Die
|
||||||
|
*Form* ist bereits korrekt.
|
||||||
|
|
||||||
|
**Validierung (Sim, gegen FK-Ground-Truth)**
|
||||||
|
- Reprojektion median **0.7–1.6 px** über Scene5/6/8/10/11/12 — besser als die bestehende,
|
||||||
|
board-verankerte Pipeline (3.2–4.9 px), weil keine falschen Marker-Positionen angenommen werden.
|
||||||
|
- Form (nach Similarity-Ausrichtung): Residuum **median ~3–7 mm** = Sensor-Rauschboden der Renders
|
||||||
|
(`markerOffsetMaxMm: 4` + Rauschen); Übereinstimmung mit der bestehenden Triangulation **1.9 mm**.
|
||||||
|
- Skalenfaktor konsistent **0.92–0.94** (→ Phase B).
|
||||||
|
|
||||||
|
**Anwendung**
|
||||||
|
```
|
||||||
|
python pipeline/scene_reconstruct.py --evalDir data/evaluations/Scene8
|
||||||
|
# -> <evalDir>/scene_reconstruction.json (Kamera- & Marker-Posen in S, Reproj-Statistik,
|
||||||
|
# Liste insufficient_view_markers)
|
||||||
|
```
|
||||||
|
```python
|
||||||
|
import scene_reconstruct as sr
|
||||||
|
res = sr.reconstruct("data/evaluations/Scene8") # dict; res["cameras"], res["markers"]
|
||||||
|
```
|
||||||
|
Voraussetzung: `*_aruco_detection.json` je Kamera (Pipeline-Schritt 1). Marker-Kantenlänge wird
|
||||||
|
aus den Detektionen gelesen (Fallback `--markerSize`).
|
||||||
261
pipeline/marker_sets.py
Normal file
261
pipeline/marker_sets.py
Normal file
@@ -0,0 +1,261 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
marker_sets.py
|
||||||
|
==============
|
||||||
|
Klassifiziert die Marker aus robot.json in drei Rollen für die Szenen-Kalibrierung
|
||||||
|
(siehe doc/callibrate_scene_roadmap.md, Phase P0):
|
||||||
|
|
||||||
|
arm : Marker an BEWEGLICHEN Roboter-Links (Arm1, Ellbow, Arm2, Finger, ...).
|
||||||
|
Ihre Lage je Link ist bekannt -> Welt-Referenz, wird NICHT kalibriert.
|
||||||
|
set : Marker an einem STATISCHEN Link (Welt-Wurzel) MIT "set"-Feld. Gleiches
|
||||||
|
"set" = ein starres Objekt mit fixen relativen Bezügen -> es wird nur die
|
||||||
|
6-DoF-Platzierung des ganzen Sets kalibriert.
|
||||||
|
loose : Marker an einem statischen Link OHNE "set"-Feld. Lose, einzeln zu vermessen.
|
||||||
|
|
||||||
|
Die Set-Zugehörigkeit steht ausschließlich in robot.json (optionales Feld "set" am
|
||||||
|
Marker). Hier wird nichts hartkodiert — die Sets ergeben sich per Auto-Discovery aus
|
||||||
|
den vorhandenen "set"-Werten.
|
||||||
|
|
||||||
|
"Beweglich" = es liegt irgendwo auf der Kette bis zur Wurzel ein revolute/linear-Joint.
|
||||||
|
Damit zählt die Wurzel (Board) und jeder rein über "fixed"-Joints angebundene Link als
|
||||||
|
statisch (Welt), alles ab dem ersten Gelenk als Arm.
|
||||||
|
|
||||||
|
Public API
|
||||||
|
----------
|
||||||
|
data = load_robot("data/robot/robot.json")
|
||||||
|
cls = classify_markers(data) # id -> MarkerInfo
|
||||||
|
sets = get_sets(data) # set_name -> [MarkerInfo, ...]
|
||||||
|
loose = get_loose_markers(data) # [MarkerInfo, ...]
|
||||||
|
arm = get_arm_markers(data) # id -> MarkerInfo
|
||||||
|
rep = set_summary(data) # serialisierbarer Report (für QA / --json)
|
||||||
|
|
||||||
|
CLI
|
||||||
|
---
|
||||||
|
python pipeline/marker_sets.py -robot data/robot/robot.json
|
||||||
|
python pipeline/marker_sets.py -robot data/robot/robot.json --json
|
||||||
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from dataclasses import dataclass, asdict
|
||||||
|
from typing import Any, Dict, List, Optional
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Datentyp
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class MarkerInfo:
|
||||||
|
id: int
|
||||||
|
link: str
|
||||||
|
role: str # "arm" | "set" | "loose"
|
||||||
|
set_name: Optional[str] # nur bei role == "set"
|
||||||
|
position: List[float] # im Link-Frame (mm), wie in robot.json
|
||||||
|
normal: Optional[List[float]]
|
||||||
|
spin: Optional[float]
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Laden
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def load_robot(path: str) -> Dict[str, Any]:
|
||||||
|
with open(path, "r", encoding="utf-8") as f:
|
||||||
|
return json.load(f)
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Link-Topologie: statisch (Welt) vs. beweglich (Arm)
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
_MOVABLE_JOINTS = ("revolute", "linear")
|
||||||
|
|
||||||
|
|
||||||
|
def link_is_movable(links: Dict[str, Any], name: str) -> bool:
|
||||||
|
"""
|
||||||
|
True, wenn auf der Kette von `name` bis zur Wurzel mindestens ein
|
||||||
|
revolute/linear-Joint liegt (Marker dort gehören zum beweglichen Roboter).
|
||||||
|
"""
|
||||||
|
seen = set()
|
||||||
|
cur: Optional[str] = name
|
||||||
|
while cur and cur in links and cur not in seen:
|
||||||
|
seen.add(cur)
|
||||||
|
joint = links[cur].get("jointToParent") or {}
|
||||||
|
if str(joint.get("type", "")).lower() in _MOVABLE_JOINTS:
|
||||||
|
return True
|
||||||
|
cur = links[cur].get("parent")
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def root_links(links: Dict[str, Any]) -> List[str]:
|
||||||
|
return [n for n, l in links.items()
|
||||||
|
if not l.get("parent") or l.get("parent") not in links]
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Klassifizierung
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def _set_name_of(marker: Dict[str, Any]) -> Optional[str]:
|
||||||
|
val = marker.get("set")
|
||||||
|
if val is None:
|
||||||
|
return None
|
||||||
|
s = str(val).strip()
|
||||||
|
return s or None
|
||||||
|
|
||||||
|
|
||||||
|
def classify_markers(robot_data: Dict[str, Any]) -> Dict[int, MarkerInfo]:
|
||||||
|
"""
|
||||||
|
Liefert id -> MarkerInfo über alle Links. Bei doppelten IDs gewinnt das erste
|
||||||
|
Vorkommen (zusätzlich als Warnung in set_summary sichtbar).
|
||||||
|
"""
|
||||||
|
links = robot_data.get("links", {}) or {}
|
||||||
|
out: Dict[int, MarkerInfo] = {}
|
||||||
|
|
||||||
|
for link_name, link in links.items():
|
||||||
|
movable = link_is_movable(links, link_name)
|
||||||
|
for mk in link.get("markers", []) or []:
|
||||||
|
if "id" not in mk or "position" not in mk:
|
||||||
|
continue
|
||||||
|
mid = int(mk["id"])
|
||||||
|
if mid in out:
|
||||||
|
continue # erstes Vorkommen behalten
|
||||||
|
set_name = _set_name_of(mk)
|
||||||
|
if movable:
|
||||||
|
role = "arm"
|
||||||
|
set_name = None
|
||||||
|
elif set_name is not None:
|
||||||
|
role = "set"
|
||||||
|
else:
|
||||||
|
role = "loose"
|
||||||
|
|
||||||
|
normal = mk.get("normal")
|
||||||
|
spin = mk.get("spin")
|
||||||
|
out[mid] = MarkerInfo(
|
||||||
|
id=mid,
|
||||||
|
link=link_name,
|
||||||
|
role=role,
|
||||||
|
set_name=set_name,
|
||||||
|
position=[float(v) for v in mk["position"]],
|
||||||
|
normal=[float(v) for v in normal] if normal is not None else None,
|
||||||
|
spin=float(spin) if spin is not None else None,
|
||||||
|
)
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def get_arm_markers(robot_data: Dict[str, Any]) -> Dict[int, MarkerInfo]:
|
||||||
|
return {m.id: m for m in classify_markers(robot_data).values() if m.role == "arm"}
|
||||||
|
|
||||||
|
|
||||||
|
def get_sets(robot_data: Dict[str, Any]) -> Dict[str, List[MarkerInfo]]:
|
||||||
|
"""set_name -> Liste der Marker (role == 'set'), gruppiert nach 'set'-Wert."""
|
||||||
|
sets: Dict[str, List[MarkerInfo]] = {}
|
||||||
|
for m in classify_markers(robot_data).values():
|
||||||
|
if m.role == "set" and m.set_name is not None:
|
||||||
|
sets.setdefault(m.set_name, []).append(m)
|
||||||
|
return sets
|
||||||
|
|
||||||
|
|
||||||
|
def get_loose_markers(robot_data: Dict[str, Any]) -> List[MarkerInfo]:
|
||||||
|
return [m for m in classify_markers(robot_data).values() if m.role == "loose"]
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# QA / Report
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def _duplicate_ids(robot_data: Dict[str, Any]) -> List[int]:
|
||||||
|
links = robot_data.get("links", {}) or {}
|
||||||
|
seen, dups = set(), set()
|
||||||
|
for link in links.values():
|
||||||
|
for mk in link.get("markers", []) or []:
|
||||||
|
if "id" not in mk:
|
||||||
|
continue
|
||||||
|
mid = int(mk["id"])
|
||||||
|
(dups if mid in seen else seen).add(mid)
|
||||||
|
return sorted(dups)
|
||||||
|
|
||||||
|
|
||||||
|
def set_summary(robot_data: Dict[str, Any]) -> Dict[str, Any]:
|
||||||
|
cls = classify_markers(robot_data)
|
||||||
|
sets = get_sets(robot_data)
|
||||||
|
loose = get_loose_markers(robot_data)
|
||||||
|
arm = [m for m in cls.values() if m.role == "arm"]
|
||||||
|
|
||||||
|
warnings: List[str] = []
|
||||||
|
for mid in _duplicate_ids(robot_data):
|
||||||
|
warnings.append(f"Marker-ID {mid} kommt mehrfach vor (erstes Vorkommen gewertet)")
|
||||||
|
for name, members in sets.items():
|
||||||
|
links_used = sorted({m.link for m in members})
|
||||||
|
if len(members) < 3:
|
||||||
|
warnings.append(
|
||||||
|
f"Set '{name}' hat nur {len(members)} Marker — 6-DoF-Platzierung "
|
||||||
|
f"nicht voll bestimmbar (>=3 nicht-kollineare nötig)")
|
||||||
|
if len(links_used) > 1:
|
||||||
|
warnings.append(f"Set '{name}' verteilt sich über mehrere Links {links_used} "
|
||||||
|
f"— ein Set sollte ein physisches Objekt sein")
|
||||||
|
|
||||||
|
return {
|
||||||
|
"counts": {
|
||||||
|
"total": len(cls),
|
||||||
|
"arm": len(arm),
|
||||||
|
"set": sum(len(v) for v in sets.values()),
|
||||||
|
"loose": len(loose),
|
||||||
|
"num_sets": len(sets),
|
||||||
|
},
|
||||||
|
"root_links": root_links(robot_data.get("links", {}) or {}),
|
||||||
|
"sets": {name: sorted(m.id for m in members) for name, members in sorted(sets.items())},
|
||||||
|
"loose_ids": sorted(m.id for m in loose),
|
||||||
|
"arm_ids": sorted(m.id for m in arm),
|
||||||
|
"warnings": warnings,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# CLI
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
ap = argparse.ArgumentParser(description="Marker aus robot.json in arm/set/loose klassifizieren")
|
||||||
|
ap.add_argument("-robot", "--robot", required=True, help="Pfad zu robot.json")
|
||||||
|
ap.add_argument("--json", action="store_true", help="Report als JSON ausgeben")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
data = load_robot(args.robot)
|
||||||
|
rep = set_summary(data)
|
||||||
|
|
||||||
|
if args.json:
|
||||||
|
print(json.dumps(rep, indent=2, ensure_ascii=False))
|
||||||
|
return
|
||||||
|
|
||||||
|
c = rep["counts"]
|
||||||
|
print(f"robot.json: {args.robot}")
|
||||||
|
print(f"Wurzel-Link(s): {rep['root_links']}")
|
||||||
|
print(f"\nMarker gesamt: {c['total']} | arm: {c['arm']} set: {c['set']} "
|
||||||
|
f"loose: {c['loose']} (Sets: {c['num_sets']})")
|
||||||
|
|
||||||
|
print("\nSets (starr, fixe interne Lage -> 6-DoF kalibrieren):")
|
||||||
|
if rep["sets"]:
|
||||||
|
for name, ids in rep["sets"].items():
|
||||||
|
print(f" {name:10s} ({len(ids):3d}): {ids}")
|
||||||
|
else:
|
||||||
|
print(" (keine)")
|
||||||
|
|
||||||
|
print(f"\nLose Marker (einzeln zu vermessen): {rep['loose_ids'] or '(keine)'}")
|
||||||
|
print(f"Arm-Marker (Welt-Referenz, nicht kalibriert): {rep['arm_ids']}")
|
||||||
|
|
||||||
|
if rep["warnings"]:
|
||||||
|
print("\n[WARN]")
|
||||||
|
for w in rep["warnings"]:
|
||||||
|
print(f" - {w}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
try:
|
||||||
|
main()
|
||||||
|
except Exception as exc: # pragma: no cover
|
||||||
|
print(f"[ERROR] {exc}", file=sys.stderr)
|
||||||
|
sys.exit(1)
|
||||||
424
pipeline/scene_reconstruct.py
Normal file
424
pipeline/scene_reconstruct.py
Normal file
@@ -0,0 +1,424 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
scene_reconstruct.py — Phase A der Szenen-Kalibrierung
|
||||||
|
========================================================
|
||||||
|
Ankerlose, metrische Rekonstruktion ALLER Kamera- und Marker-Posen aus den
|
||||||
|
ArUco-Eckbeobachtungen mehrerer Kameras — OHNE bekannten Welt-Anker, allein aus
|
||||||
|
der bekannten Marker-Kantenlänge.
|
||||||
|
|
||||||
|
Vollständig generisch: kein robot.json nötig, keine festen Marker-IDs/Namen.
|
||||||
|
|
||||||
|
Warum nicht einfach Einzel-Marker-PnP?
|
||||||
|
--------------------------------------
|
||||||
|
Ein planares Quadrat hat bei PnP eine 2-fache Flip-Ambiguität. Verkettet man solche
|
||||||
|
Einzelposen naiv, sind ~1/3 der Knoten gespiegelt -> unbrauchbar. Stattdessen:
|
||||||
|
|
||||||
|
1) je (Kamera, Marker): PnP (IPPE_SQUARE) als TENTATIVE Startpose.
|
||||||
|
2) Referenzkamera c0 (meiste Beobachtungen) definiert den Szenen-Frame S (E_{c0}=I).
|
||||||
|
3) Iteration (flip-robust):
|
||||||
|
a) Kameras per cv2.solvePnPRansac gegen die bereits platzierten 3D-Ecken
|
||||||
|
neu bestimmen -> geflippte Marker fallen als RANSAC-Ausreißer raus.
|
||||||
|
b) Marker-Ecken über alle platzierten Kameras TRIANGULIEREN (DLT, flip-frei)
|
||||||
|
und die Marker-Pose per Kabsch aus den triangulierten Ecken neu setzen.
|
||||||
|
4) globale Bündelausgleichung (least_squares, Huber, dünnbesetzte Jacobi) über
|
||||||
|
die Reprojektion ALLER Ecken. Gauge: c0 = Identität; Maßstab via Markergröße.
|
||||||
|
|
||||||
|
Geometrie-Konventionen
|
||||||
|
----------------------
|
||||||
|
- E_c = world(S) -> camera c (X_cam = E_c X_S)
|
||||||
|
- G_m = marker m local -> world(S) (X_S = G_m X_local)
|
||||||
|
- M_{c<-m} = E_c @ G_m (local -> camera)
|
||||||
|
- Ecken-Reihenfolge wie ArUco/Pipeline: TL, TR, BR, BL.
|
||||||
|
|
||||||
|
Ein-/Ausgabe
|
||||||
|
------------
|
||||||
|
--evalDir : Ordner mit render_*_aruco_detection.json (Eckpunkte + Intrinsik)
|
||||||
|
--out : scene_reconstruction.json (Default: <evalDir>/scene_reconstruction.json)
|
||||||
|
|
||||||
|
Das Ergebnis (Posen in S) ist Eingang für Phase B (robot_register.py).
|
||||||
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import glob
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
import re
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
from typing import Dict, List, Optional, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import cv2
|
||||||
|
|
||||||
|
try:
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
from scipy.sparse import lil_matrix
|
||||||
|
HAVE_SCIPY = True
|
||||||
|
except ImportError: # pragma: no cover
|
||||||
|
HAVE_SCIPY = False
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# SE(3) / Geometrie-Helfer
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def rt_to_T(rvec, tvec) -> np.ndarray:
|
||||||
|
R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=float).reshape(3, 1))
|
||||||
|
T = np.eye(4)
|
||||||
|
T[:3, :3] = R
|
||||||
|
T[:3, 3] = np.asarray(tvec, dtype=float).reshape(3)
|
||||||
|
return T
|
||||||
|
|
||||||
|
|
||||||
|
def T_to_rt(T: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||||
|
rvec, _ = cv2.Rodrigues(np.ascontiguousarray(T[:3, :3]))
|
||||||
|
return rvec.reshape(3), T[:3, 3].copy()
|
||||||
|
|
||||||
|
|
||||||
|
def inv_T(T: np.ndarray) -> np.ndarray:
|
||||||
|
R, t = T[:3, :3], T[:3, 3]
|
||||||
|
Ti = np.eye(4)
|
||||||
|
Ti[:3, :3] = R.T
|
||||||
|
Ti[:3, 3] = -R.T @ t
|
||||||
|
return Ti
|
||||||
|
|
||||||
|
|
||||||
|
def local_corners(size_m: float) -> np.ndarray:
|
||||||
|
"""Marker-Ecken im lokalen Frame (TL, TR, BR, BL), z=0."""
|
||||||
|
h = size_m / 2.0
|
||||||
|
return np.array([[-h, h, 0.0], [h, h, 0.0], [h, -h, 0.0], [-h, -h, 0.0]], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def kabsch(P: np.ndarray, Q: np.ndarray) -> np.ndarray:
|
||||||
|
"""R,t mit Q ≈ R P + t (ohne Skalierung); P,Q: Nx3. Liefert 4x4."""
|
||||||
|
pc, qc = P.mean(0), Q.mean(0)
|
||||||
|
H = (P - pc).T @ (Q - qc)
|
||||||
|
U, _, Vt = np.linalg.svd(H)
|
||||||
|
d = np.sign(np.linalg.det(Vt.T @ U.T))
|
||||||
|
R = Vt.T @ np.diag([1.0, 1.0, d]) @ U.T
|
||||||
|
T = np.eye(4)
|
||||||
|
T[:3, :3] = R
|
||||||
|
T[:3, 3] = qc - R @ pc
|
||||||
|
return T
|
||||||
|
|
||||||
|
|
||||||
|
def triangulate_point(obs: List[Tuple]) -> Optional[np.ndarray]:
|
||||||
|
"""Mehrbild-DLT eines 3D-Punkts. obs: [(K,D,R,t,uv), ...] mit X_cam=R X+t."""
|
||||||
|
A = []
|
||||||
|
for K, D, R, t, uv in obs:
|
||||||
|
und = cv2.undistortPoints(np.array([[uv]], dtype=np.float64), 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 X[:3] / X[3] if abs(X[3]) > 1e-12 else None
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Laden der Detektionen
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class Cam:
|
||||||
|
__slots__ = ("id", "K", "D", "obs")
|
||||||
|
|
||||||
|
def __init__(self, cid, K, D):
|
||||||
|
self.id = cid
|
||||||
|
self.K = K
|
||||||
|
self.D = D
|
||||||
|
self.obs: Dict[int, np.ndarray] = {} # marker_id -> (4,2) px
|
||||||
|
|
||||||
|
|
||||||
|
def load_detections(eval_dir: str, cameras: Optional[List[str]] = None,
|
||||||
|
default_size: Optional[float] = None
|
||||||
|
) -> Tuple[Dict[str, Cam], Dict[int, float]]:
|
||||||
|
cams: Dict[str, Cam] = {}
|
||||||
|
sizes: Dict[int, float] = {}
|
||||||
|
for det_path in sorted(glob.glob(os.path.join(eval_dir, "*_aruco_detection.json"))):
|
||||||
|
m = re.match(r"render_([A-Za-z0-9]+)_aruco_detection\.json", os.path.basename(det_path))
|
||||||
|
if not m:
|
||||||
|
continue
|
||||||
|
cid = m.group(1)
|
||||||
|
if cameras and cid not in cameras:
|
||||||
|
continue
|
||||||
|
det = json.load(open(det_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)
|
||||||
|
cam = Cam(cid, K, D)
|
||||||
|
det_size = (det.get("vision_config", {}) or {}).get("MarkerSize", None)
|
||||||
|
for d in det.get("detections", []):
|
||||||
|
if d.get("type", "aruco") != "aruco":
|
||||||
|
continue
|
||||||
|
pts = d.get("image_points_px")
|
||||||
|
if pts is None:
|
||||||
|
continue
|
||||||
|
mid = int(d["marker_id"])
|
||||||
|
cam.obs[mid] = np.array(pts, dtype=float).reshape(4, 2)
|
||||||
|
s = d.get("marker_size_m", det_size if det_size is not None else default_size)
|
||||||
|
if s is not None:
|
||||||
|
sizes[mid] = float(s)
|
||||||
|
if cam.obs:
|
||||||
|
cams[cid] = cam
|
||||||
|
return cams, sizes
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Schritt 1: Per-Marker-PnP (tentative Startposen)
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def pnp_all(cams: Dict[str, Cam], sizes: Dict[int, float], default_size: float
|
||||||
|
) -> Dict[Tuple[str, int], Dict]:
|
||||||
|
out: Dict[Tuple[str, int], Dict] = {}
|
||||||
|
for cid, cam in cams.items():
|
||||||
|
for mid, corners_px in cam.obs.items():
|
||||||
|
size = sizes.get(mid, default_size)
|
||||||
|
ok, rvec, tvec = cv2.solvePnP(local_corners(size), corners_px, cam.K, cam.D,
|
||||||
|
flags=cv2.SOLVEPNP_IPPE_SQUARE)
|
||||||
|
if not ok:
|
||||||
|
ok, rvec, tvec = cv2.solvePnP(local_corners(size), corners_px, cam.K, cam.D,
|
||||||
|
flags=cv2.SOLVEPNP_ITERATIVE)
|
||||||
|
if ok:
|
||||||
|
out[(cid, mid)] = {"M": rt_to_T(rvec, tvec)}
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Schritt 2+3: flip-robuste Initialisierung
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def initialize_poses(cams, sizes, default_size, pnp, n_iter=6, min_pts_pnp=8,
|
||||||
|
ransac_px=3.0) -> Tuple[Dict[str, np.ndarray], Dict[int, np.ndarray], str]:
|
||||||
|
cam_ids = sorted(cams)
|
||||||
|
all_mk = sorted({m for _, m in pnp})
|
||||||
|
|
||||||
|
# Referenzkamera = meiste Beobachtungen
|
||||||
|
ref = max(cam_ids, key=lambda c: len(cams[c].obs))
|
||||||
|
E: Dict[str, np.ndarray] = {ref: np.eye(4)}
|
||||||
|
G: Dict[int, np.ndarray] = {}
|
||||||
|
for m in cams[ref].obs: # tentative Startmarker aus c0
|
||||||
|
if (ref, m) in pnp:
|
||||||
|
G[m] = pnp[(ref, m)]["M"].copy()
|
||||||
|
|
||||||
|
def corners3D(m):
|
||||||
|
return (G[m][:3, :3] @ local_corners(sizes.get(m, default_size)).T).T + G[m][:3, 3]
|
||||||
|
|
||||||
|
for _ in range(n_iter):
|
||||||
|
# (a) Kameras gegen platzierte 3D-Ecken (RANSAC -> Flip-Ausreißer raus)
|
||||||
|
for c in cam_ids:
|
||||||
|
if c == ref:
|
||||||
|
continue
|
||||||
|
obj, img = [], []
|
||||||
|
for m in cams[c].obs:
|
||||||
|
if m in G:
|
||||||
|
obj.append(corners3D(m))
|
||||||
|
img.append(cams[c].obs[m])
|
||||||
|
if sum(len(o) for o in obj) < min_pts_pnp:
|
||||||
|
continue
|
||||||
|
O = np.vstack(obj).astype(np.float64)
|
||||||
|
I = np.vstack(img).astype(np.float64)
|
||||||
|
ok, rvec, tvec, inl = cv2.solvePnPRansac(
|
||||||
|
O, I, cams[c].K, cams[c].D, reprojectionError=ransac_px,
|
||||||
|
iterationsCount=200, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||||
|
if ok and inl is not None and len(inl) >= 6:
|
||||||
|
E[c] = rt_to_T(rvec, tvec)
|
||||||
|
|
||||||
|
# (b) Marker triangulieren (flip-frei) bzw. einfügen
|
||||||
|
for m in all_mk:
|
||||||
|
placed = [c for c in cam_ids if c in E and (c, m) in pnp]
|
||||||
|
if len(placed) >= 2:
|
||||||
|
tri, ok = [], True
|
||||||
|
for ci in range(4):
|
||||||
|
o = [(cams[c].K, cams[c].D, E[c][:3, :3], E[c][:3, 3], cams[c].obs[m][ci])
|
||||||
|
for c in placed]
|
||||||
|
X = triangulate_point(o)
|
||||||
|
if X is None:
|
||||||
|
ok = False
|
||||||
|
break
|
||||||
|
tri.append(X)
|
||||||
|
if ok:
|
||||||
|
G[m] = kabsch(local_corners(sizes.get(m, default_size)), np.array(tri))
|
||||||
|
elif placed and m not in G:
|
||||||
|
c = placed[0]
|
||||||
|
G[m] = inv_T(E[c]) @ pnp[(c, m)]["M"]
|
||||||
|
|
||||||
|
return E, G, ref
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Schritt 4: Globale Bündelausgleichung (Referenzkamera fix)
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def reproj_stats(obs_pairs, cams, sizes, default_size, E, G) -> Tuple[float, float]:
|
||||||
|
"""(RMS, Median) der Per-Beobachtungs-Reprojektionsfehler (px)."""
|
||||||
|
per = []
|
||||||
|
for (c, m) in obs_pairs:
|
||||||
|
rvec, tvec = T_to_rt(E[c] @ G[m])
|
||||||
|
proj, _ = cv2.projectPoints(local_corners(sizes.get(m, default_size)),
|
||||||
|
rvec, tvec, cams[c].K, cams[c].D)
|
||||||
|
d = proj.reshape(4, 2) - cams[c].obs[m]
|
||||||
|
per.append(float(np.sqrt(np.mean(d * d))))
|
||||||
|
if not per:
|
||||||
|
return 0.0, 0.0
|
||||||
|
a = np.asarray(per)
|
||||||
|
return float(np.sqrt(np.mean(a * a))), float(np.median(a))
|
||||||
|
|
||||||
|
|
||||||
|
def bundle_adjust(pnp, cams, sizes, default_size, E, G, ref,
|
||||||
|
huber_px=2.0, max_nfev=120, verbose=0):
|
||||||
|
if not HAVE_SCIPY:
|
||||||
|
print("[WARN] scipy fehlt -> BA uebersprungen")
|
||||||
|
return E, G
|
||||||
|
cam_opt = [c for c in sorted(E) if c != ref]
|
||||||
|
mk_ids = sorted(G)
|
||||||
|
ci = {c: i for i, c in enumerate(cam_opt)}
|
||||||
|
mi = {m: j for j, m in enumerate(mk_ids)}
|
||||||
|
ncam, nmk = len(cam_opt), len(mk_ids)
|
||||||
|
obs = [(c, m) for (c, m) in pnp if c in E and m in G]
|
||||||
|
|
||||||
|
def pack():
|
||||||
|
x = np.zeros(6 * ncam + 6 * nmk)
|
||||||
|
for c in cam_opt:
|
||||||
|
r, t = T_to_rt(E[c]); x[6*ci[c]:6*ci[c]+3] = r; x[6*ci[c]+3:6*ci[c]+6] = t
|
||||||
|
for m in mk_ids:
|
||||||
|
r, t = T_to_rt(G[m]); o = 6*ncam + 6*mi[m]; x[o:o+3] = r; x[o+3:o+6] = t
|
||||||
|
return x
|
||||||
|
|
||||||
|
def unpack(x):
|
||||||
|
Ee = {ref: np.eye(4)}
|
||||||
|
for c in cam_opt:
|
||||||
|
Ee[c] = rt_to_T(x[6*ci[c]:6*ci[c]+3], x[6*ci[c]+3:6*ci[c]+6])
|
||||||
|
Gg = {}
|
||||||
|
for m in mk_ids:
|
||||||
|
o = 6*ncam + 6*mi[m]
|
||||||
|
Gg[m] = rt_to_T(x[o:o+3], x[o+3:o+6])
|
||||||
|
return Ee, Gg
|
||||||
|
|
||||||
|
def residuals(x):
|
||||||
|
Ee, Gg = unpack(x)
|
||||||
|
res = np.empty(8 * len(obs))
|
||||||
|
for k, (c, m) in enumerate(obs):
|
||||||
|
rvec, tvec = T_to_rt(Ee[c] @ Gg[m])
|
||||||
|
proj, _ = cv2.projectPoints(local_corners(sizes.get(m, default_size)),
|
||||||
|
rvec, tvec, cams[c].K, cams[c].D)
|
||||||
|
res[8*k:8*k+8] = (proj.reshape(4, 2) - cams[c].obs[m]).ravel()
|
||||||
|
return res
|
||||||
|
|
||||||
|
Sp = lil_matrix((8 * len(obs), 6 * ncam + 6 * nmk), dtype=int)
|
||||||
|
for k, (c, m) in enumerate(obs):
|
||||||
|
rows = slice(8*k, 8*k+8)
|
||||||
|
if c in ci:
|
||||||
|
Sp[rows, 6*ci[c]:6*ci[c]+6] = 1
|
||||||
|
o = 6*ncam + 6*mi[m]
|
||||||
|
Sp[rows, o:o+6] = 1
|
||||||
|
|
||||||
|
sol = least_squares(residuals, pack(), jac_sparsity=Sp, method="trf",
|
||||||
|
loss="huber", f_scale=huber_px, max_nfev=max_nfev, verbose=verbose)
|
||||||
|
return unpack(sol.x)
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Hauptlauf
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def reconstruct(eval_dir, cameras=None, default_size=0.025,
|
||||||
|
huber_px=2.0, max_nfev=120, verbose=0) -> Dict:
|
||||||
|
cams, sizes = load_detections(eval_dir, cameras, default_size)
|
||||||
|
if len(cams) < 2:
|
||||||
|
raise RuntimeError(f"brauche >=2 Kameras, gefunden: {sorted(cams)}")
|
||||||
|
pnp = pnp_all(cams, sizes, default_size)
|
||||||
|
E0, G0, ref = initialize_poses(cams, sizes, default_size, pnp)
|
||||||
|
|
||||||
|
# Marker je #platzierter Kameras; nur >=2 sind triangulierbar/zuverlässig.
|
||||||
|
ncams_of = {}
|
||||||
|
for (c, m) in pnp:
|
||||||
|
if c in E0 and m in G0:
|
||||||
|
ncams_of[m] = ncams_of.get(m, 0) + 1
|
||||||
|
weak = sorted(m for m in G0 if ncams_of.get(m, 0) < 2) # Einzelbild -> unbeobachtbar
|
||||||
|
G0 = {m: T for m, T in G0.items() if ncams_of.get(m, 0) >= 2}
|
||||||
|
|
||||||
|
pairs0 = [(c, m) for (c, m) in pnp if c in E0 and m in G0]
|
||||||
|
rms0, med0 = reproj_stats(pairs0, cams, sizes, default_size, E0, G0)
|
||||||
|
E, G = bundle_adjust(pnp, cams, sizes, default_size, E0, G0, ref, huber_px, max_nfev, verbose)
|
||||||
|
pairs = [(c, m) for (c, m) in pnp if c in E and m in G]
|
||||||
|
rms1, med1 = reproj_stats(pairs, cams, sizes, default_size, E, G)
|
||||||
|
dropped = [c for c in cams if c not in E]
|
||||||
|
|
||||||
|
cameras_out = []
|
||||||
|
for c in sorted(E):
|
||||||
|
Ec = E[c]
|
||||||
|
rvec, _ = T_to_rt(Ec)
|
||||||
|
cameras_out.append({
|
||||||
|
"camera_id": c,
|
||||||
|
"world_to_camera": {"rotation_matrix": Ec[:3, :3].tolist(),
|
||||||
|
"translation_m": Ec[:3, 3].tolist(), "rvec_rad": rvec.tolist()},
|
||||||
|
"center_m": (-Ec[:3, :3].T @ Ec[:3, 3]).tolist(),
|
||||||
|
"num_markers": len(cams[c].obs),
|
||||||
|
"is_reference": bool(c == ref),
|
||||||
|
})
|
||||||
|
markers_out = []
|
||||||
|
for m in sorted(G):
|
||||||
|
Gm = G[m]
|
||||||
|
size = sizes.get(m, default_size)
|
||||||
|
corners_S = (Gm[:3, :3] @ local_corners(size).T).T + Gm[:3, 3]
|
||||||
|
rvec, _ = T_to_rt(Gm)
|
||||||
|
markers_out.append({
|
||||||
|
"marker_id": int(m),
|
||||||
|
"pose_in_S": {"rotation_matrix": Gm[:3, :3].tolist(),
|
||||||
|
"translation_m": Gm[:3, 3].tolist(), "rvec_rad": rvec.tolist()},
|
||||||
|
"center_m": Gm[:3, 3].tolist(),
|
||||||
|
"normal": (Gm[:3, :3] @ np.array([0.0, 0.0, 1.0])).tolist(),
|
||||||
|
"corners_m": corners_S.tolist(),
|
||||||
|
"num_cameras": int(ncams_of.get(m, 0)),
|
||||||
|
})
|
||||||
|
|
||||||
|
return {
|
||||||
|
"schema_version": "1.0",
|
||||||
|
"stage": "scene_reconstruct_A",
|
||||||
|
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||||
|
"frame": "arbitrary S (reference camera pose = identity)",
|
||||||
|
"reference_camera": ref,
|
||||||
|
"summary": {"num_cameras": len(E), "num_markers": len(G), "num_observations": len(pairs),
|
||||||
|
"reproj_rms_px_init": rms0, "reproj_median_px_init": med0,
|
||||||
|
"reproj_rms_px_final": rms1, "reproj_median_px_final": med1,
|
||||||
|
"dropped_cameras": dropped,
|
||||||
|
"insufficient_view_markers": weak},
|
||||||
|
"cameras": cameras_out,
|
||||||
|
"markers": markers_out,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
ap = argparse.ArgumentParser(description="Phase A: ankerlose metrische Szenen-Rekonstruktion")
|
||||||
|
ap.add_argument("--evalDir", required=True, help="Ordner mit render_*_aruco_detection.json")
|
||||||
|
ap.add_argument("--cameras", default=None, help="Kommagetrennte Kamera-IDs (Default: alle)")
|
||||||
|
ap.add_argument("--markerSize", type=float, default=0.025, help="Fallback-Kantenlänge (m)")
|
||||||
|
ap.add_argument("--huberPx", type=float, default=2.0)
|
||||||
|
ap.add_argument("--maxNfev", type=int, default=120)
|
||||||
|
ap.add_argument("--out", default=None)
|
||||||
|
ap.add_argument("-v", "--verbose", action="count", default=0)
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
cams = args.cameras.split(",") if args.cameras else None
|
||||||
|
res = reconstruct(args.evalDir, cams, args.markerSize, args.huberPx, args.maxNfev, args.verbose)
|
||||||
|
out = args.out or os.path.join(args.evalDir, "scene_reconstruction.json")
|
||||||
|
json.dump(res, open(out, "w", encoding="utf-8"), indent=2)
|
||||||
|
s = res["summary"]
|
||||||
|
print(f"[INFO] Kameras={s['num_cameras']} Marker(>=2 Views)={s['num_markers']} "
|
||||||
|
f"Obs={s['num_observations']} | RMS {s['reproj_rms_px_init']:.3f}->{s['reproj_rms_px_final']:.3f}px "
|
||||||
|
f"(median {s['reproj_median_px_final']:.3f}px) | ref-Kamera={res['reference_camera']}")
|
||||||
|
if s["dropped_cameras"]:
|
||||||
|
print(f"[WARN] nicht verbundene Kameras verworfen: {s['dropped_cameras']}")
|
||||||
|
if s["insufficient_view_markers"]:
|
||||||
|
print(f"[INFO] {len(s['insufficient_view_markers'])} Marker mit <2 Views (unbeobachtbar, "
|
||||||
|
f"nicht rekonstruiert): {s['insufficient_view_markers']}")
|
||||||
|
print(f"[INFO] -> {out}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
try:
|
||||||
|
main()
|
||||||
|
except Exception as exc:
|
||||||
|
print(f"[ERROR] {exc}", file=sys.stderr)
|
||||||
|
sys.exit(1)
|
||||||
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json
Normal file
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json
Normal file
@@ -0,0 +1,33 @@
|
|||||||
|
{
|
||||||
|
"page_format": "A3",
|
||||||
|
"orientation": "portrait",
|
||||||
|
"page_size_mm": {
|
||||||
|
"width": 297.0,
|
||||||
|
"height": 420.0
|
||||||
|
},
|
||||||
|
"seed": 223,
|
||||||
|
"num_arucos": 10,
|
||||||
|
"aruco_size_mm": 50.0,
|
||||||
|
"aruco_dictionary": "DICT_4X4_250",
|
||||||
|
"aruco_start_id": 46,
|
||||||
|
"page_border_margin_mm": 20.0,
|
||||||
|
"forbidden_rectangle_mm": {
|
||||||
|
"x": 143.5,
|
||||||
|
"y": 205.0,
|
||||||
|
"w": 10.0,
|
||||||
|
"h": 10.0
|
||||||
|
},
|
||||||
|
"forbidden_rectangle_margin_mm": 30.0,
|
||||||
|
"placements": [
|
||||||
|
{"id": 46, "position": [96.26, 119.02, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 47, "position": [158.68, 110.29, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 48, "position": [-43.31, 5.63, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 49, "position": [63.52, -4.25, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 50, "position": [27.77, 149.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 51, "position": [-98.03, 38.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 52, "position": [-120.71, 148.79, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 53, "position": [72.76, 59.77, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 54, "position": [-156.67, 34.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 55, "position": [-110.82, 93.01, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||||
|
]
|
||||||
|
}
|
||||||
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf
Normal file
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf
Normal file
@@ -0,0 +1,74 @@
|
|||||||
|
%PDF-1.3
|
||||||
|
%“Œ‹ž ReportLab Generated PDF document http://www.reportlab.com
|
||||||
|
1 0 obj
|
||||||
|
<<
|
||||||
|
/F1 2 0 R /F2 3 0 R
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
2 0 obj
|
||||||
|
<<
|
||||||
|
/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
3 0 obj
|
||||||
|
<<
|
||||||
|
/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
4 0 obj
|
||||||
|
<<
|
||||||
|
/Contents 8 0 R /MediaBox [ 0 0 841.8898 1190.551 ] /Parent 7 0 R /Resources <<
|
||||||
|
/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ]
|
||||||
|
>> /Rotate 0 /Trans <<
|
||||||
|
|
||||||
|
>>
|
||||||
|
/Type /Page
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
5 0 obj
|
||||||
|
<<
|
||||||
|
/PageMode /UseNone /Pages 7 0 R /Type /Catalog
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
6 0 obj
|
||||||
|
<<
|
||||||
|
/Author (OpenAI) /CreationDate (D:20260609100839+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260609100839+02'00') /Producer (ReportLab PDF Library - www.reportlab.com)
|
||||||
|
/Subject (A0 poster with random ArUco placement) /Title (A3_10Arucos_50mm_Seet223) /Trapped /False
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
7 0 obj
|
||||||
|
<<
|
||||||
|
/Count 1 /Kids [ 4 0 R ] /Type /Pages
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
8 0 obj
|
||||||
|
<<
|
||||||
|
/Filter [ /ASCII85Decode /FlateDecode ] /Length 2256
|
||||||
|
>>
|
||||||
|
stream
|
||||||
|
Gasb^c)r<L%#+HM.C+V0R+R8eZUFpRXJ"GN0h@+g"WLbi='*AObm`^/.uekT^TKc`,iq!cbphBh5QC]L;bGq3dW[or@S)WAM(@-Kpc8<BGdBd2p\,bJT:b>G9Ol;/QtIaGHgCFun\\R[^AH)LDaL5K<a5F@5#HBrhRP!S9Uqq+)0EU[0C8;F^@>8hc6K6Q-e17IqXj](D4N^H9n1io5//-oU((jsCT;e\51G!$RnTZ;p57s"r=ne@2]#of7ERf;T(gFg`c@q3Mg:nHfM!N+P>$DsH3q.!3+"eSYI0L5WMthC6fq'?W/5'@0@#\6[V+To(cr+Jm>A-jSP#LkC\&faD@kR&*@VQ3F(gseG:WuLoc.FaHNRCE#&%so`Eirncr^:LCP^ZGJV@\d\#ea#Yb#dWDS<i2QX1QY$At)ei4K'%ceHsl`k5ka%LB[e!*C=QKO&X3@6KC%I4bki`j$jaC=eVV8,6=O/>>dH*Ak<JH@/?BeFma*r0\TJ%ba#eQ*3'7qR.B&alPF)V(sa3=n-*YcmdI:JQV#'Ei]QQ>:,?d"k'VQ@&n=r-B\F^/o(IQee!"-6Su;1[96Cg4Mjqtcn,8XX\5l:QUPiGQ#;QK_uSBM!YSb@Q+39DR2O0O:nJ.N]<)+8>6?]5CL^I>_h^+!]OhU<XpALd'*-Ao5_Ok,di,u"?Y>gUfIV;Kbl1Kp=d8KARCY8Sok8%=?Q*/1mAplQ`VSF$Ti5[jA:t\helFKE<i[Rf"@=qi$ECNdQr"+ilIKEfb6?2WEf6#u^T3_S(I6TF!N8)<ffa!KlR;fP4G>mgVa;g4kXm!f[kiPWB7PZsXGpt]DRlh:XoVst#(YMJRVcJuDqQXH"MN5Pco&iDG,G>ESjLAM-2-!JT+YocXgZQ*QIt_*@V;T$m(9?GDcVtSIHn7&c_Ek/I:juH@p:mc1*G.ABs/'uXMiSR@"@&nBX&kl@Q=FiH]-RZ2eKO*Yta#<]2a4D"]uj*J9>c1lf/be4,93Z3Fpm_oo.u4Yats4H4D[A]u@(".mqD#(h*q>Qa7NOYq`htWi[Dl+K[B`>%;X`!e@Ga(o'0D!0+%!/!mtb)ZqP^XN'k.)J^dta$%mhB)6B&Wh&p`mKboFfrbEnbOsZE+KUNCLPN+VkN3N,]4+SUrZa3EL7$fJ!7p65)OB/f0d!_:OUJBWC,*?&!N]K-1b<FqF<B*c+ua,C>_$t-J;-^7*JLTR\\#.q,QQEW,4)lf&9_2DoIaoo>O^g>,JY55Y#,;R*<X*'$ioTJ6u5t:>L&R'l_AM_mj`"".C-hN]Lb,Aou5=U7*b\e#i"'Yru5Ce4R[b%Cfr]`&[q?'r\pRfF5Uk-;m7iV3<6.-MqP3K3:G9W7].I%[`r!%&U*CB=P&J$fCdkGhfHj1`iRJkKQXtY#1m(c204)!Y,Mo(So0@%@km8_@KCA))qB+])e=ppN-YO[l3W.-:*<"hJH--pV@5W\okEl!O["aNs/.Z\lSI^dk-R*dPoUm/MEkhs[]cVC5[1tjVZWlfpX9/KT(g\7V=))K8^4^T]01\3h6m<,W<FQWggb3_JOojo,p>ae"L3^]fL$g=`U&luY\PQck]-m+F_B8:Q"ZYqS,o]u%5GIn!8(ilOKbgGUG%S7?C*O&?.K#,XWadI7qXP[f<_d`NM#pjIL1Dil1<G:b%m>XNj[t\9s(h![GJ(7=bqiTLc*3t=,]#[<SDuUD@Q4-F"E!8NEXXA@<W40BZP$HkV:#S4,ZrP)kpPP/qfcO!<%kbW`q]:2a;^u!HbjWd%rQg[oPY`D<P73oV"TRQOpK-6TH4,'MJVEHFNt@klU4_C(Ss;LtJp5Q)^0=<`86%cO*WX.o98]=u7tgY.3Bg(.r,#FDVSi@L[J#fREaIbRf:V7e#qh1k`7]rV/r(U;:ol]bC.pc%>*iZWf)u\m@Y>euOMsrkY^t"i0t:`9]R(0tY-<)R_RtHJ\XQk1s.Xm*6bdG7Gen/r,hM3Q)hZ%_5HImH*6e&@4HRR,'cUloT.UH'c!#K\HB1:%sCHb8\;NDZK*5N3FgfOH$a*>qUuYeu!HS]'Duq0?3W]Mf&9qoe,:u1:6/>jp&(j1?AU(fP3o5=,:_LL/TdL)D\?[hE6R.?]a_pN-Gge]Cc6,lNK,T?@,>+(W]\YXH*P#\i5.^D_^S7`j@5I_VY/M('*ZH&U*D-1q6km$)M6-Dp3).^j04<eZd=Dg0C0e@b0]P_QLG%YW`g&lT%@Lr1hUo08[Yk$ESR)7"&,Me(_W<Reju~>endstream
|
||||||
|
endobj
|
||||||
|
xref
|
||||||
|
0 9
|
||||||
|
0000000000 65535 f
|
||||||
|
0000000073 00000 n
|
||||||
|
0000000114 00000 n
|
||||||
|
0000000221 00000 n
|
||||||
|
0000000330 00000 n
|
||||||
|
0000000533 00000 n
|
||||||
|
0000000601 00000 n
|
||||||
|
0000000936 00000 n
|
||||||
|
0000000995 00000 n
|
||||||
|
trailer
|
||||||
|
<<
|
||||||
|
/ID
|
||||||
|
[<292de92922b99d411f4915f95f3215c2><292de92922b99d411f4915f95f3215c2>]
|
||||||
|
% ReportLab generated PDF document -- digest (http://www.reportlab.com)
|
||||||
|
|
||||||
|
/Info 6 0 R
|
||||||
|
/Root 5 0 R
|
||||||
|
/Size 9
|
||||||
|
>>
|
||||||
|
startxref
|
||||||
|
3342
|
||||||
|
%%EOF
|
||||||
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json
Normal file
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json
Normal file
@@ -0,0 +1,33 @@
|
|||||||
|
{
|
||||||
|
"page_format": "A3",
|
||||||
|
"orientation": "portrait",
|
||||||
|
"page_size_mm": {
|
||||||
|
"width": 297.0,
|
||||||
|
"height": 420.0
|
||||||
|
},
|
||||||
|
"seed": 224,
|
||||||
|
"num_arucos": 10,
|
||||||
|
"aruco_size_mm": 50.0,
|
||||||
|
"aruco_dictionary": "DICT_4X4_250",
|
||||||
|
"aruco_start_id": 46,
|
||||||
|
"page_border_margin_mm": 10.0,
|
||||||
|
"forbidden_rectangle_mm": {
|
||||||
|
"x": 143.5,
|
||||||
|
"y": 205.0,
|
||||||
|
"w": 10.0,
|
||||||
|
"h": 10.0
|
||||||
|
},
|
||||||
|
"forbidden_rectangle_margin_mm": 30.0,
|
||||||
|
"placements": [
|
||||||
|
{"id": 46, "position": [-103.49, -14.02, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 47, "position": [17.33, -11.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 48, "position": [14.91, 182.28, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 49, "position": [-133.58, 94.98, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 50, "position": [82.51, 132.7, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 51, "position": [147.43, 136.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 52, "position": [159.13, -7.87, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 53, "position": [-151.21, 155.25, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 54, "position": [80.2, 56.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 55, "position": [94.41, -7.18, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||||
|
]
|
||||||
|
}
|
||||||
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf
Normal file
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf
Normal file
@@ -0,0 +1,74 @@
|
|||||||
|
%PDF-1.3
|
||||||
|
%“Œ‹ž ReportLab Generated PDF document http://www.reportlab.com
|
||||||
|
1 0 obj
|
||||||
|
<<
|
||||||
|
/F1 2 0 R /F2 3 0 R
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
2 0 obj
|
||||||
|
<<
|
||||||
|
/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
3 0 obj
|
||||||
|
<<
|
||||||
|
/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
4 0 obj
|
||||||
|
<<
|
||||||
|
/Contents 8 0 R /MediaBox [ 0 0 841.8898 1190.551 ] /Parent 7 0 R /Resources <<
|
||||||
|
/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ]
|
||||||
|
>> /Rotate 0 /Trans <<
|
||||||
|
|
||||||
|
>>
|
||||||
|
/Type /Page
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
5 0 obj
|
||||||
|
<<
|
||||||
|
/PageMode /UseNone /Pages 7 0 R /Type /Catalog
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
6 0 obj
|
||||||
|
<<
|
||||||
|
/Author (OpenAI) /CreationDate (D:20260609100923+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260609100923+02'00') /Producer (ReportLab PDF Library - www.reportlab.com)
|
||||||
|
/Subject (A0 poster with random ArUco placement) /Title (A3_10Arucos_50mm_Seet224) /Trapped /False
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
7 0 obj
|
||||||
|
<<
|
||||||
|
/Count 1 /Kids [ 4 0 R ] /Type /Pages
|
||||||
|
>>
|
||||||
|
endobj
|
||||||
|
8 0 obj
|
||||||
|
<<
|
||||||
|
/Filter [ /ASCII85Decode /FlateDecode ] /Length 2250
|
||||||
|
>>
|
||||||
|
stream
|
||||||
|
Gasb_c&UjC%#"*@'L!jj:dg/icS[tN92<UA6')NeAET%I?9_ED,.k)fZS91([ejia<Cs'$fgA9\++!k^a-Yi]nbr7AB0)t2n"1-M`5hbYa%u/BqNCjUqq'TC"!('LN&_BBR?NH@p#kl@dp%:6qqEcB-&p',g[UnbJoaSu;a4ehU0RIA,Frc-0DbM%hrpOOVid%+B4;qBq""25\"5W<3W@E%ID!:i6lW*_ie.,%H)^[!2C[0;g+/JMe5$Z`I<.!)H7dZS4-0:3lcN+#F]S/t7j9gMYc[!98*<Od'up'm,PsA6/6%ZZ=X@]TZDLYp(B<J9M2u/tP_h8?J0es0eQ*fYF"/pf%FhE'PDlcA-WuW8?p0:n&f2:8j:JYh"2L?aJ2M7"le\q=#.`P2f>[l"9gd-nL7tLL%>OEW=N_Lh[pFT-]cp&?4m9C1lc6,l-KdJAF,3uF!FYX=,OD"c#\uFglsimhX_UXuWEi/K^.R#B6+'u28J9RVaHtpKDVlQ)Iin-O(B&5p/\IjcTp8Eg\.-!tQ76h9%^AH#7ZYa(19@4.Ye?j\o7YaFC!6X`Q@fC=\MkoAK\bJrG'cU/m1!IZ>LOn#n85krj+ae8+_G#4cH?.M2Lp/UehR>0di5WrQaD@GP8DUSYVmR'f";$bH&Ghs`^@?Y\R-Sj2h^".#@Vfd#eP.CfUYCN`q>PkP,R9q7/C%Nqb-m(l?UlO7pCCecpR;c,T,9Y(jdnI4*BAJ'4Q;kVlHnj#g99U</@q%6n^]t5%`u.YhL,4RU5'bVH5E6U-?Th!pgseKJd[&CZC/RQ33erEmP=cI@'$qYa.Z#\i5-sDh&&`7Pj>A>Q_d1L6sh-XFjem&<\]`mX,UicSL*Z*"[;V&@,`HD6mI[os-nTjK,cLV:O3];I+,ZjXR8#?-gF"DU"Rg`9J0s.N#3r<%;CZXrBaS6&=%s\IXLEb(1u6Ed)ssjUecd/S8:,aI:sG7\ZCXU).84\0:54L9Q$[&9d4:Yb"5Bo0Z%Y!,FRpXQ9Gdjt13$RpHpr_F(s:XEanMdi5WrQhhR*U).330>Q:D!3FF:J@-@@2QY*GE_4J8Yq?Gt?(N]lR>4+MXViG?_id-FU("<7,cQC#dLo+fLt+A[pBe)V=2"U,L9Qt2S`/FmjaaSi)o.nKJ9AOggiIPM!5$8\@Ei$#]!n&LD\DKK\R'eD]eSi!A(L=URkOMaQ]D<cRU1O#k68Z4\!V;V=g+c:_n6#ON>oZ:%KL9)`2r,5Z!jaL.At*>H5a,]4g=#!>!"_Rk4LSVog-&NBI%I)A%ABt7Y02=WDggh0iP38p+U$"h+U`a@QfLS\LeK<!FX/VbduYu28/>TVUW6/`8h!`<Z@e\2F^tflZt>C?&H/C(u6tk7U9ec\lNDp11_YN*+:F%J2LsoldfOr^$.t2>8s6V5QInI!3hak^fh4&lT%@L>W3U-'"&L92X4F(l(r9XQ:;HLXn1i(j_jo.Bre<52T/Wk\]kJddWL6dN#e^pW\/o(>BJtpCDd9-*Q'r;S@;M6@R/R_%:Dm_^m\NU-=]3>Jk1]#QOpJM5tANPPnfN#^m\MYh3.kIh-/$k253L8A(CX'T+_KG0uU.tMqMt7EAr;_W<1W^G$@>dD%0qJ#Qq0I&@1gH/ubbeT%EqfGu`/\;E8=U;Ok@og;hh"Mkb_b,)2b+O5eq6NR7@r<Bdl16^4ViaTFi:[A]oEkYhPoe?jM1a7g96a?CK"=dL(CE4ssrkM$[3l*qo6FPM"+;"!bD5E0U&533?^XuFq9p!.\EYhjT%He\VP1a'gr%Jlc]kH/(>!3eOlJ@0b:G+V&+CH4e!I/euU,uM#fCPb2);15E37E\Y:\"U?GUFE[!>?]6K>E:99CgGek]jU3\>+R8VgGm<$WYJsom=WGhYCJRT5o[OEDstAa%Pd9q!S6lE)ReK`b5'O=0GXr+`r]\Y2h^#D!7nht/'6rY8iRRdlT"sHjT)NN"2J(MJ2bY;>GTYapWPaTR.VF!eoCamo*4D9Dq]H%F7V!W(:3E<@*KY+g)'pnjUe\*%Bm11'$JdiRH`G?jO`_eCM0i2W<N>$l1^o&6%r/6k.KDr!#=%u\pXQ!23l_Ya(WT<3[(F*cpS]>qo=hC!&84d6e%reW(C9E=&S2:G)hU`^tK>"X;Qq!G8@MsNJ;U/rDnLZ"2LA\R!#qo]B-1O98;4Zs!AOi9`i,!">,A7<D3J0mYQS#Hf?17*XRAi(a9a*U6\HHE2'f?@VaLZH)cG9g])/R!CZ~>endstream
|
||||||
|
endobj
|
||||||
|
xref
|
||||||
|
0 9
|
||||||
|
0000000000 65535 f
|
||||||
|
0000000073 00000 n
|
||||||
|
0000000114 00000 n
|
||||||
|
0000000221 00000 n
|
||||||
|
0000000330 00000 n
|
||||||
|
0000000533 00000 n
|
||||||
|
0000000601 00000 n
|
||||||
|
0000000936 00000 n
|
||||||
|
0000000995 00000 n
|
||||||
|
trailer
|
||||||
|
<<
|
||||||
|
/ID
|
||||||
|
[<544804e2078c10a71a717f7cd03ded29><544804e2078c10a71a717f7cd03ded29>]
|
||||||
|
% ReportLab generated PDF document -- digest (http://www.reportlab.com)
|
||||||
|
|
||||||
|
/Info 6 0 R
|
||||||
|
/Root 5 0 R
|
||||||
|
/Size 9
|
||||||
|
>>
|
||||||
|
startxref
|
||||||
|
3336
|
||||||
|
%%EOF
|
||||||
417
setup/generateTabletopPDF/a3_aruco.py
Normal file
417
setup/generateTabletopPDF/a3_aruco.py
Normal file
@@ -0,0 +1,417 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import random
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import List, Tuple
|
||||||
|
|
||||||
|
from reportlab.pdfgen import canvas
|
||||||
|
from reportlab.lib.units import mm
|
||||||
|
from reportlab.pdfbase import pdfmetrics
|
||||||
|
|
||||||
|
try:
|
||||||
|
import cv2
|
||||||
|
except ImportError as exc:
|
||||||
|
raise SystemExit(
|
||||||
|
"OpenCV ist erforderlich. Installiere es mit: pip install opencv-contrib-python"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
|
||||||
|
# =========================
|
||||||
|
# Header / Parameter
|
||||||
|
# =========================
|
||||||
|
|
||||||
|
PAGE_FORMAT = "A3" # z.B. A0, A1, A2, A3, A4
|
||||||
|
ORIENTATION = "portrait" # "portrait" oder "landscape"
|
||||||
|
|
||||||
|
NUM_ARUCOS = 10
|
||||||
|
ARUCO_SIZE_MM = 50.0
|
||||||
|
ARUCO_START_ID = 46 # erster Marker aus DICT_4X4_250
|
||||||
|
SEED = 224 # Zufalls-Seed für reproduzierbare Verteilung
|
||||||
|
|
||||||
|
PAGE_BORDER_MARGIN_MM = 10.0 # Abstand aller Marker vom Seitenrand
|
||||||
|
|
||||||
|
FORBIDDEN_RECT_W_MM = 10.0
|
||||||
|
FORBIDDEN_RECT_H_MM = 10.0
|
||||||
|
FORBIDDEN_RECT_MARGIN_MM = 30.0 # keine ArUcos innerhalb dieses Abstands
|
||||||
|
|
||||||
|
LINE_WIDTH_MM = 1.0 # Linienstärke des Rechtecks
|
||||||
|
TEXT_FONT = "Times-Roman"
|
||||||
|
TEXT_SIZE_PT = 8
|
||||||
|
TEXT_GAP_MM = 4.0
|
||||||
|
|
||||||
|
OUTPUT_BASENAME = f"{PAGE_FORMAT}_{NUM_ARUCOS}Arucos_{int(ARUCO_SIZE_MM)}mm_Seet{SEED}"
|
||||||
|
|
||||||
|
|
||||||
|
# =========================
|
||||||
|
# DIN-Formate
|
||||||
|
# =========================
|
||||||
|
|
||||||
|
DIN_SIZES_MM = {
|
||||||
|
"A0": (841.0, 1189.0),
|
||||||
|
"A1": (594.0, 841.0),
|
||||||
|
"A2": (420.0, 594.0),
|
||||||
|
"A3": (297.0, 420.0),
|
||||||
|
"A4": (210.0, 297.0),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RectMM:
|
||||||
|
x: float
|
||||||
|
y: float
|
||||||
|
w: float
|
||||||
|
h: float
|
||||||
|
|
||||||
|
def intersects(self, other: "RectMM") -> bool:
|
||||||
|
return not (
|
||||||
|
self.x + self.w <= other.x
|
||||||
|
or other.x + other.w <= self.x
|
||||||
|
or self.y + self.h <= other.y
|
||||||
|
or other.y + other.h <= self.y
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def mm_to_pt(value_mm: float) -> float:
|
||||||
|
return value_mm * mm
|
||||||
|
|
||||||
|
|
||||||
|
def get_page_size_mm(page_format: str, orientation: str) -> Tuple[float, float]:
|
||||||
|
if page_format not in DIN_SIZES_MM:
|
||||||
|
raise ValueError(f"Unbekanntes Format: {page_format}. Unterstützt: {sorted(DIN_SIZES_MM)}")
|
||||||
|
|
||||||
|
w_mm, h_mm = DIN_SIZES_MM[page_format]
|
||||||
|
if orientation.lower() == "portrait":
|
||||||
|
return w_mm, h_mm
|
||||||
|
if orientation.lower() == "landscape":
|
||||||
|
return h_mm, w_mm
|
||||||
|
raise ValueError("ORIENTATION muss 'portrait' oder 'landscape' sein.")
|
||||||
|
|
||||||
|
|
||||||
|
def centered_rect(page_w_mm: float, page_h_mm: float, rect_w_mm: float, rect_h_mm: float) -> RectMM:
|
||||||
|
return RectMM(
|
||||||
|
x=(page_w_mm - rect_w_mm) / 2.0,
|
||||||
|
y=(page_h_mm - rect_h_mm) / 2.0,
|
||||||
|
w=rect_w_mm,
|
||||||
|
h=rect_h_mm,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def expand_rect(rect: RectMM, margin_mm: float) -> RectMM:
|
||||||
|
return RectMM(
|
||||||
|
x=rect.x - margin_mm,
|
||||||
|
y=rect.y - margin_mm,
|
||||||
|
w=rect.w + 2.0 * margin_mm,
|
||||||
|
h=rect.h + 2.0 * margin_mm,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def get_aruco_dictionary():
|
||||||
|
# DICT_4X4_250 hat IDs 0..249
|
||||||
|
return cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
|
||||||
|
|
||||||
|
|
||||||
|
def marker_module_pattern(marker_id: int) -> List[List[int]]:
|
||||||
|
"""
|
||||||
|
Liefert ein 6x6-Raster (inkl. schwarzem Rand).
|
||||||
|
1 = schwarz, 0 = weiss
|
||||||
|
"""
|
||||||
|
aruco_dict = get_aruco_dictionary()
|
||||||
|
img = cv2.aruco.generateImageMarker(aruco_dict, marker_id, 600) # nur zum Abtasten
|
||||||
|
modules = 6
|
||||||
|
cell = img.shape[0] // modules
|
||||||
|
|
||||||
|
pattern: List[List[int]] = []
|
||||||
|
for r in range(modules):
|
||||||
|
row: List[int] = []
|
||||||
|
for c in range(modules):
|
||||||
|
cy = int((r + 0.5) * cell)
|
||||||
|
cx = int((c + 0.5) * cell)
|
||||||
|
pixel = int(img[cy, cx]) # 0 = schwarz, 255 = weiss
|
||||||
|
row.append(1 if pixel < 128 else 0)
|
||||||
|
pattern.append(row)
|
||||||
|
return pattern
|
||||||
|
|
||||||
|
|
||||||
|
def draw_aruco_vector(
|
||||||
|
c: canvas.Canvas,
|
||||||
|
x_mm: float,
|
||||||
|
y_mm: float,
|
||||||
|
size_mm: float,
|
||||||
|
marker_id: int,
|
||||||
|
page_h_mm: float,
|
||||||
|
) -> None:
|
||||||
|
"""
|
||||||
|
Zeichnet den Marker als Vektor-Rechtecke.
|
||||||
|
x_mm, y_mm = linke obere Ecke in mm.
|
||||||
|
"""
|
||||||
|
pattern = marker_module_pattern(marker_id)
|
||||||
|
modules = len(pattern)
|
||||||
|
cell_mm = size_mm / modules
|
||||||
|
|
||||||
|
for r in range(modules):
|
||||||
|
for col in range(modules):
|
||||||
|
if pattern[r][col] == 1:
|
||||||
|
cell_x_mm = x_mm + col * cell_mm
|
||||||
|
#cell_y_mm = y_mm + (modules - 1 - r) * cell_mm
|
||||||
|
cell_y_mm = y_mm + r * cell_mm
|
||||||
|
c.rect(
|
||||||
|
mm_to_pt(cell_x_mm),
|
||||||
|
mm_to_pt(page_h_mm - cell_y_mm - cell_mm),
|
||||||
|
mm_to_pt(cell_mm),
|
||||||
|
mm_to_pt(cell_mm),
|
||||||
|
stroke=0,
|
||||||
|
fill=1,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def draw_aruco_label(
|
||||||
|
c: canvas.Canvas,
|
||||||
|
x_mm: float,
|
||||||
|
y_mm: float,
|
||||||
|
size_mm: float,
|
||||||
|
page_h_mm: float,
|
||||||
|
marker_id: int,
|
||||||
|
) -> None:
|
||||||
|
c.setFont(TEXT_FONT, TEXT_SIZE_PT)
|
||||||
|
text = str(marker_id)
|
||||||
|
text_width_pt = pdfmetrics.stringWidth(text, TEXT_FONT, TEXT_SIZE_PT)
|
||||||
|
text_x_pt = mm_to_pt(x_mm + size_mm / 2.0) - text_width_pt / 2.0
|
||||||
|
font = pdfmetrics.getFont(TEXT_FONT)
|
||||||
|
ascent_mm = (font.face.ascent / 1000.0) * TEXT_SIZE_PT * 0.352777777777778
|
||||||
|
text_baseline_y_mm = y_mm + size_mm + TEXT_GAP_MM + ascent_mm
|
||||||
|
c.drawString(text_x_pt, mm_to_pt(page_h_mm - text_baseline_y_mm), text)
|
||||||
|
|
||||||
|
|
||||||
|
def place_markers(
|
||||||
|
page_w_mm: float,
|
||||||
|
page_h_mm: float,
|
||||||
|
num_markers: int,
|
||||||
|
marker_size_mm: float,
|
||||||
|
border_margin_mm: float,
|
||||||
|
forbidden_area: RectMM,
|
||||||
|
forbidden_margin_mm: float,
|
||||||
|
seed: int,
|
||||||
|
) -> List[dict]:
|
||||||
|
rng = random.Random(seed)
|
||||||
|
placed: List[RectMM] = []
|
||||||
|
result: List[dict] = []
|
||||||
|
|
||||||
|
allowed_x_min = border_margin_mm
|
||||||
|
allowed_y_min = border_margin_mm
|
||||||
|
allowed_x_max = page_w_mm - border_margin_mm - marker_size_mm
|
||||||
|
allowed_y_max = page_h_mm - border_margin_mm - marker_size_mm
|
||||||
|
|
||||||
|
if allowed_x_max < allowed_x_min or allowed_y_max < allowed_y_min:
|
||||||
|
raise RuntimeError("Das Poster ist zu klein für Randabstand und Markergrösse.")
|
||||||
|
|
||||||
|
excluded = expand_rect(forbidden_area, forbidden_margin_mm)
|
||||||
|
|
||||||
|
attempts = 0
|
||||||
|
max_attempts = 200000
|
||||||
|
|
||||||
|
while len(result) < num_markers and attempts < max_attempts:
|
||||||
|
attempts += 1
|
||||||
|
x = rng.uniform(allowed_x_min, allowed_x_max)
|
||||||
|
y = rng.uniform(allowed_y_min, allowed_y_max)
|
||||||
|
candidate = RectMM(x=x, y=y, w=marker_size_mm, h=marker_size_mm)
|
||||||
|
|
||||||
|
if any(candidate.intersects(other) for other in placed):
|
||||||
|
continue
|
||||||
|
if candidate.intersects(excluded):
|
||||||
|
continue
|
||||||
|
|
||||||
|
placed.append(candidate)
|
||||||
|
result.append(
|
||||||
|
{
|
||||||
|
"id": ARUCO_START_ID + len(result),
|
||||||
|
"x_mm": round(x, 2),
|
||||||
|
"y_mm": round(y, 2),
|
||||||
|
"size_mm": marker_size_mm,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
if len(result) < num_markers:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"Nicht alle Marker konnten platziert werden: {len(result)} von {num_markers} "
|
||||||
|
f"(nach {attempts} Versuchen)."
|
||||||
|
)
|
||||||
|
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
page_w_mm, page_h_mm = get_page_size_mm(PAGE_FORMAT, ORIENTATION)
|
||||||
|
|
||||||
|
if ARUCO_START_ID < 0 or ARUCO_START_ID + NUM_ARUCOS > 250:
|
||||||
|
raise ValueError("ARUCO_START_ID + NUM_ARUCOS muss in den Bereich 0..249 von DICT_4X4_250 fallen.")
|
||||||
|
|
||||||
|
forbidden_rect = centered_rect(page_w_mm, page_h_mm, FORBIDDEN_RECT_W_MM, FORBIDDEN_RECT_H_MM)
|
||||||
|
|
||||||
|
# Neues lokales Koordinatensystem
|
||||||
|
origin_x_mm = forbidden_rect.x + forbidden_rect.w - 90.0
|
||||||
|
origin_y_mm = forbidden_rect.y
|
||||||
|
|
||||||
|
placements = place_markers(
|
||||||
|
page_w_mm=page_w_mm,
|
||||||
|
page_h_mm=page_h_mm,
|
||||||
|
num_markers=NUM_ARUCOS,
|
||||||
|
marker_size_mm=ARUCO_SIZE_MM,
|
||||||
|
border_margin_mm=PAGE_BORDER_MARGIN_MM,
|
||||||
|
forbidden_area=forbidden_rect,
|
||||||
|
forbidden_margin_mm=FORBIDDEN_RECT_MARGIN_MM,
|
||||||
|
seed=SEED,
|
||||||
|
)
|
||||||
|
|
||||||
|
pdf_path = Path(f"{OUTPUT_BASENAME}.pdf")
|
||||||
|
json_path = Path(f"{OUTPUT_BASENAME}.json")
|
||||||
|
|
||||||
|
c = canvas.Canvas(str(pdf_path), pagesize=(mm_to_pt(page_w_mm), mm_to_pt(page_h_mm)))
|
||||||
|
c.setTitle(pdf_path.stem)
|
||||||
|
c.setAuthor("OpenAI")
|
||||||
|
c.setSubject("A0 poster with random ArUco placement")
|
||||||
|
|
||||||
|
# Weisser Hintergrund
|
||||||
|
c.setFillColorRGB(1, 1, 1)
|
||||||
|
c.rect(0, 0, mm_to_pt(page_w_mm), mm_to_pt(page_h_mm), stroke=0, fill=1)
|
||||||
|
|
||||||
|
# Rechteck mit 1 mm schwarzer Linie
|
||||||
|
c.setStrokeColorRGB(0, 0, 0)
|
||||||
|
c.setLineWidth(mm_to_pt(LINE_WIDTH_MM))
|
||||||
|
c.rect(
|
||||||
|
mm_to_pt(forbidden_rect.x),
|
||||||
|
mm_to_pt(page_h_mm - forbidden_rect.y - forbidden_rect.h),
|
||||||
|
mm_to_pt(forbidden_rect.w),
|
||||||
|
mm_to_pt(forbidden_rect.h),
|
||||||
|
stroke=1,
|
||||||
|
fill=0,
|
||||||
|
)
|
||||||
|
|
||||||
|
# Koordinatensystem
|
||||||
|
|
||||||
|
ARROW_LEN_MM = 50.0
|
||||||
|
|
||||||
|
# X-Achse (rot, nach unten)
|
||||||
|
c.setStrokeColorRGB(1, 0, 0)
|
||||||
|
c.setLineWidth(mm_to_pt(1.0))
|
||||||
|
|
||||||
|
c.line(
|
||||||
|
mm_to_pt(origin_x_mm),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm),
|
||||||
|
mm_to_pt(origin_x_mm),
|
||||||
|
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)),
|
||||||
|
)
|
||||||
|
|
||||||
|
# Pfeilspitze
|
||||||
|
c.line(
|
||||||
|
mm_to_pt(origin_x_mm),
|
||||||
|
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)),
|
||||||
|
mm_to_pt(origin_x_mm - 4),
|
||||||
|
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM - 4)),
|
||||||
|
)
|
||||||
|
c.line(
|
||||||
|
mm_to_pt(origin_x_mm),
|
||||||
|
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)),
|
||||||
|
mm_to_pt(origin_x_mm + 4),
|
||||||
|
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM - 4)),
|
||||||
|
)
|
||||||
|
|
||||||
|
# Y-Achse (grün, nach rechts)
|
||||||
|
c.setStrokeColorRGB(0, 0.7, 0)
|
||||||
|
|
||||||
|
c.line(
|
||||||
|
mm_to_pt(origin_x_mm),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm),
|
||||||
|
mm_to_pt(origin_x_mm + ARROW_LEN_MM),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm),
|
||||||
|
)
|
||||||
|
|
||||||
|
# Pfeilspitze
|
||||||
|
c.line(
|
||||||
|
mm_to_pt(origin_x_mm + ARROW_LEN_MM),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm),
|
||||||
|
mm_to_pt(origin_x_mm + ARROW_LEN_MM - 4),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm - 4),
|
||||||
|
)
|
||||||
|
c.line(
|
||||||
|
mm_to_pt(origin_x_mm + ARROW_LEN_MM),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm),
|
||||||
|
mm_to_pt(origin_x_mm + ARROW_LEN_MM - 4),
|
||||||
|
mm_to_pt(page_h_mm - origin_y_mm + 4),
|
||||||
|
)
|
||||||
|
|
||||||
|
c.setStrokeColorRGB(0, 0, 0)
|
||||||
|
|
||||||
|
# ArUcos zeichnen
|
||||||
|
c.setFillColorRGB(0, 0, 0)
|
||||||
|
for item in placements:
|
||||||
|
draw_aruco_vector(
|
||||||
|
c=c,
|
||||||
|
x_mm=item["x_mm"],
|
||||||
|
y_mm=item["y_mm"],
|
||||||
|
size_mm=item["size_mm"],
|
||||||
|
marker_id=item["id"],
|
||||||
|
page_h_mm=page_h_mm,
|
||||||
|
)
|
||||||
|
draw_aruco_label(
|
||||||
|
c=c,
|
||||||
|
x_mm=item["x_mm"],
|
||||||
|
y_mm=item["y_mm"],
|
||||||
|
size_mm=item["size_mm"],
|
||||||
|
page_h_mm=page_h_mm,
|
||||||
|
marker_id=item["id"],
|
||||||
|
)
|
||||||
|
|
||||||
|
c.showPage()
|
||||||
|
c.save()
|
||||||
|
|
||||||
|
# JSON mit Positionen
|
||||||
|
with json_path.open("w", encoding="utf-8") as f:
|
||||||
|
meta = {
|
||||||
|
"page_format": PAGE_FORMAT,
|
||||||
|
"orientation": ORIENTATION,
|
||||||
|
"page_size_mm": {"width": page_w_mm, "height": page_h_mm},
|
||||||
|
"seed": SEED,
|
||||||
|
"num_arucos": NUM_ARUCOS,
|
||||||
|
"aruco_size_mm": ARUCO_SIZE_MM,
|
||||||
|
"aruco_dictionary": "DICT_4X4_250",
|
||||||
|
"aruco_start_id": ARUCO_START_ID,
|
||||||
|
"page_border_margin_mm": PAGE_BORDER_MARGIN_MM,
|
||||||
|
"forbidden_rectangle_mm": {
|
||||||
|
"x": round(forbidden_rect.x, 2),
|
||||||
|
"y": round(forbidden_rect.y, 2),
|
||||||
|
"w": forbidden_rect.w,
|
||||||
|
"h": forbidden_rect.h,
|
||||||
|
},
|
||||||
|
"forbidden_rectangle_margin_mm": FORBIDDEN_RECT_MARGIN_MM,
|
||||||
|
}
|
||||||
|
|
||||||
|
f.write(json.dumps(meta, indent=2, ensure_ascii=False)[:-2])
|
||||||
|
f.write(',\n "placements": [\n')
|
||||||
|
|
||||||
|
for index, p in enumerate(placements):
|
||||||
|
item = {
|
||||||
|
"id": p["id"],
|
||||||
|
"position": [
|
||||||
|
round((p["y_mm"] + ARUCO_SIZE_MM / 2) - origin_y_mm, 2),
|
||||||
|
-1*round(origin_x_mm - (p["x_mm"] + ARUCO_SIZE_MM / 2), 2),
|
||||||
|
-27.3,
|
||||||
|
],
|
||||||
|
"normal": [0, 0, 1],
|
||||||
|
"spin": 90,
|
||||||
|
}
|
||||||
|
line = json.dumps(item, ensure_ascii=False)
|
||||||
|
if index < len(placements) - 1:
|
||||||
|
line += ","
|
||||||
|
f.write(f" {line}\n")
|
||||||
|
|
||||||
|
f.write(" ]\n}\n")
|
||||||
|
|
||||||
|
print(f"PDF geschrieben: {pdf_path.resolve()}")
|
||||||
|
print(f"JSON geschrieben: {json_path.resolve()}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Reference in New Issue
Block a user