Multipoint Schritt 3
This commit is contained in:
chk
@@ -1,11 +1,16 @@
|
||||
# Homing 5 – Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung
|
||||
|
||||
> Reine Wegbeschreibung — **nichts davon ist umgesetzt**. Ergänzt
|
||||
> [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche Verbesserungen, die
|
||||
> alle an derselben Stelle ansetzen: *was* ein Marker als Messung beiträgt und
|
||||
> *wie stark* sie zählt. Stand: 2026-06-16, basiert auf Code-Durchsicht
|
||||
> (`scripts/1_detect_aruco_observations.py`, `3b_corner_marker_poses.py`,
|
||||
> `5_pose_estimation.py`), keine Code-Änderung.
|
||||
> Ergänzt [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche
|
||||
> Verbesserungen, die alle an derselben Stelle ansetzen: *was* ein Marker als
|
||||
> Messung beiträgt und *wie stark* sie zählt.
|
||||
>
|
||||
> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2)
|
||||
> umgesetzt. **Mehrpunkt-Eckresiduen** (Doc-Punkt 1 / Schritte 3+4) umgesetzt
|
||||
> als Opt-in-Modus `marker_observation="corner_points"` — 12 Eck-Residuen für
|
||||
> Roboter-Links, 1 Center-Residuum für die (spin-unkalibrierten) Board/Rail-
|
||||
> Marker auf dem Root-Link. Endgültiges Tuning/Validierung gegen Simulation
|
||||
> steht aus (siehe Notiz unten). **Einzelkamera-Einbindung** (Doc-Punkt 2 /
|
||||
> Schritte 5+6) weiterhin **offen** — siehe Status-Spalte unten.
|
||||
|
||||
---
|
||||
|
||||
@@ -216,15 +221,21 @@ Reihenfolge folgt der Risiko-Einschätzung oben: erst risikoarmes Durchreichen,
|
||||
dann die beiden strukturellen Erweiterungen. Jeder Schritt ist einzeln
|
||||
umsetzbar und (wo möglich) einzeln testbar, bevor der nächste beginnt.
|
||||
|
||||
| # | Schritt | Testbar an | Bricht Bestehendes? |
|
||||
|---|---|---|---|
|
||||
| 1 | **(Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es |
|
||||
| 2 | **(Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden |
|
||||
| 3 | **(Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode |
|
||||
| 4 | **(Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden |
|
||||
| 5 | **(Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
|
||||
| 6 | **(Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) |
|
||||
| 7 | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten |
|
||||
> **Status-Legende:** ✅ erledigt · ⬜ **offen**
|
||||
>
|
||||
> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier
|
||||
> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 ·
|
||||
> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt).
|
||||
|
||||
| # | Status | Schritt | Testbar an | Bricht Bestehendes? |
|
||||
|---|---|---|---|---|
|
||||
| 1 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es |
|
||||
| 2 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden |
|
||||
| 3 | ✅ erledigt (2026-06-25) | **(Doc-Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode |
|
||||
| 4 | 🟡 Code erledigt (2026-06-25), Tuning offen | **(Doc-Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden |
|
||||
| 5 | ⬜ **offen** (Vorarbeit/Guards ✅) | **(Doc-Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
|
||||
| 6 | ⬜ **offen** | **(Doc-Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) |
|
||||
| 7 | ⬜ **offen** | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten |
|
||||
|
||||
### Recherche zu Schritt 5: wer liest `aruco_marker_poses.json`? (2026-06-16)
|
||||
|
||||
@@ -259,6 +270,34 @@ Homing-Seite selbst (`editRobot.js`, `homing.js`) ist bereits robust.
|
||||
|
||||
Alle anderen Konsumenten (`homing.js`, `editRobot.js` → `assignByZRange`/`alignSetToMeasured`, `scripts/9_evaluateMarker.py`) waren schon robust oder sind Offline-Benchmark-Code ohne Produktionsrelevanz.
|
||||
|
||||
### Umsetzung Schritt 3+4 (Doc-Punkt 1) — Befunde (2026-06-25)
|
||||
|
||||
- **Schritt 3** (`robot_fk.py`): `marker_corners_local/_world` + `all_markers_world`
|
||||
liefern jetzt `corners_world` (4×3, in `corners_m`-Reihenfolge). Orientierung =
|
||||
Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale (exakt wie
|
||||
boardViewer.html). Verifiziert in `test/test_robot_fk_corners.py`:
|
||||
Selbst-Konsistenz (Center/Kanten/planar/Normalen-Rückgewinnung) **und** gegen
|
||||
echte triangulierte Roboter-Ecken am Seed-Pose (~1 mm RMS, Identitäts-Paarung
|
||||
schlägt jede Umordnung). Eckreihenfolge = `(+h,+h),(+h,-h),(-h,-h),(-h,+h)`
|
||||
(= boardViewer-Zeiger (1,1,0)).
|
||||
- **Wichtiger Konventions-Befund:** Die `spin`-Werte sind nur für die
|
||||
**Roboter-Marker** kalibriert/visuell verifiziert, **nicht** für die Board/
|
||||
Rail-Marker (Set A0/rail, alle auf dem Root-Link `Board`). Deren Eckreihenfolge
|
||||
liegt ~90° daneben. Lösung: `corner_points` nutzt 12 Eck-Residuen nur für
|
||||
Roboter-Links (`corner_point_links`, Default = alle Nicht-Root-Links) und 1
|
||||
Center-Residuum für die Board/Rail-Marker. Da `Board` Root ist (Residuum
|
||||
konstant bzgl. der Gelenke), kostet das nichts an Information.
|
||||
- **Datenbefund (nicht Code):** 6 Marker des Board-Sets **A0 sind auf `Arm1`
|
||||
fehlzugeordnet** (`[55,56,57,77,78,99]`), ~230 mm neben dem Modell. Sie
|
||||
destabilisieren `corner_points` (12 statt 3 Residuen → falsches Minimum) und
|
||||
ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert
|
||||
`corner_points` ≈ `corner_pose`. → Marker-Zuordnung korrigieren (separate
|
||||
Kalibrier-Aufgabe).
|
||||
- **Offen (Schritt 4 Tuning):** `huber_delta_mm` ist auf 6 Residuen/Marker
|
||||
kalibriert; mit 12 verschiebt sich die RMS-Größenordnung. Sauberes A/B + Tuning
|
||||
gegen appRobotRendering-Simulations-GT (klare Daten) steht aus, bevor der Modus
|
||||
produktiv als Default taugt. CLI: `--marker-observation corner_points`.
|
||||
|
||||
## Offene Punkte
|
||||
|
||||
- [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen.
|
||||
|
||||
@@ -20,9 +20,15 @@ Four switchable methods (robot.json -> pose_estimation.method):
|
||||
global_ba : all variables jointly, position + normal residuals, robust loss
|
||||
hybrid : sequential_fk init -> global_ba refine (default, most stable)
|
||||
|
||||
Observation input:
|
||||
marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal)
|
||||
marker_observation = "center_point" -> aruco_positions_*.json (pos only)
|
||||
Observation input (robot.json -> pose_estimation.marker_observation):
|
||||
"corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
|
||||
"corner_points" -> aruco_marker_poses.json: 12 corner residuals for
|
||||
robot-link markers (4 triangulated corners vs FK
|
||||
corners; no separate normal), 1 center residual for
|
||||
root-link (Board: floor/rail) markers whose spin is
|
||||
uncalibrated. Robust loss acts per corner. Opt-in;
|
||||
needs `corners_m`. Links via corner_point_links.
|
||||
"center_point" -> aruco_positions_*.json: position only
|
||||
|
||||
Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
|
||||
--from-state <json> seed/init state (flat {var: value}, or the
|
||||
@@ -89,6 +95,14 @@ DEFAULT_CFG: Dict[str, Any] = {
|
||||
"min_cameras_per_marker": 2,
|
||||
"finger_block_joints": ["b", "c", "e"],
|
||||
"per_link_method": {},
|
||||
# Nur im marker_observation="corner_points"-Modus relevant: welche Links die
|
||||
# 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
|
||||
# der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
|
||||
# Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
|
||||
# Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
|
||||
# zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
|
||||
# z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
|
||||
"corner_point_links": None,
|
||||
# One switch: if set to a link name (e.g. "Arm1"), that link's
|
||||
# jointToParent.origin Y/Z is fit together with the normal pose (same
|
||||
# global_ba solve) and the result is written back into robot.json
|
||||
@@ -111,8 +125,12 @@ def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
|
||||
def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Load marker observations. Accepts aruco_marker_poses.json (with measured
|
||||
normal + num_cameras) or aruco_positions_*.json (position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int}
|
||||
normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
|
||||
(position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
|
||||
link:str, n_cams:int, weight:float}
|
||||
corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
|
||||
"corner_points"-Modus in residual_vector().
|
||||
"""
|
||||
data = json.load(open(path, "r", encoding="utf-8"))
|
||||
out: Dict[int, Dict[str, Any]] = {}
|
||||
@@ -135,7 +153,14 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i
|
||||
nn = np.linalg.norm(nv)
|
||||
if nn > 1e-9:
|
||||
nrm = nv / nn
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams,
|
||||
corners_mm = None
|
||||
cm = m.get("corners_m")
|
||||
if cm is not None:
|
||||
arr = np.array(cm, dtype=float)
|
||||
if arr.shape == (4, 3):
|
||||
corners_mm = arr * 1000.0
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
|
||||
"link": m.get("link", "?"), "n_cams": n_cams,
|
||||
"weight": float(m.get("weight", 1.0))}
|
||||
return out
|
||||
|
||||
@@ -268,14 +293,64 @@ def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, n
|
||||
return fk.all_markers_world(T) # mid -> {world_mm, normal_world, link, local_mm}
|
||||
|
||||
|
||||
def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
|
||||
"""
|
||||
Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
|
||||
Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
|
||||
(der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
|
||||
ein Center-Residuum).
|
||||
"""
|
||||
explicit = cfg.get("corner_point_links")
|
||||
if isinstance(explicit, list) and explicit:
|
||||
return set(explicit)
|
||||
links = fk.links
|
||||
roots = {ln for ln, ld in links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in links}
|
||||
return set(links.keys()) - roots
|
||||
|
||||
|
||||
def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
|
||||
marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
|
||||
"""Position (mm) + optional normal (scaled) residuals over the given markers."""
|
||||
"""
|
||||
Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
|
||||
|
||||
"corner_pose" (Default): 3 Position (mm) + optional 3 Normale
|
||||
(×normal_weight) je Marker — wie bisher.
|
||||
"corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) je Marker auf
|
||||
einem Roboter-Link (corner_point_links), KEINE
|
||||
separate Normale (Orientierung steckt in den
|
||||
Ecken). Mehr unabhängige Messpunkte, robuster
|
||||
Verlust greift auf Eck-Ebene. Marker auf dem
|
||||
Root-Link (Board: Boden-/Rail-Marker mit
|
||||
unkalibriertem Spin) oder ohne `corners_mm`
|
||||
nutzen EIN Center-Residuum (3, "ein Punkt pro
|
||||
Marker") — gleiche mm-Skala → ein huber_delta_mm.
|
||||
"""
|
||||
model = model_markers(fk, state)
|
||||
res: List[float] = []
|
||||
use_mw = bool(cfg.get("use_marker_weight", False))
|
||||
obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
|
||||
|
||||
if obs_mode == "corner_points":
|
||||
corner_links = _resolve_corner_links(fk, cfg)
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
|
||||
mm = model[mid]
|
||||
oc = obs[mid].get("corners_mm")
|
||||
mc = mm.get("corners_world")
|
||||
if mm.get("link") in corner_links and oc is not None and mc is not None:
|
||||
dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw # (4,3)
|
||||
res.extend(dc.reshape(-1).tolist()) # 12 Werte
|
||||
else:
|
||||
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
|
||||
res.extend(dp.tolist()) # Center (1 Punkt)
|
||||
return np.asarray(res, dtype=float)
|
||||
|
||||
# Default: Center (mm) + optionale Normale (skaliert)
|
||||
w_n = float(cfg.get("normal_weight", 30.0))
|
||||
use_n = bool(cfg.get("use_normals", True))
|
||||
use_mw = bool(cfg.get("use_marker_weight", False))
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
@@ -646,6 +721,9 @@ def main() -> None:
|
||||
ap.add_argument("-robot", "--robot", required=True)
|
||||
ap.add_argument("-out", "--out", default=None)
|
||||
ap.add_argument("--method", default=None, help="override robot.json method")
|
||||
ap.add_argument("--marker-observation", default=None, dest="marker_observation",
|
||||
help="override robot.json marker_observation "
|
||||
"(corner_pose | corner_points | center_point)")
|
||||
ap.add_argument("--from-state", default=None, metavar="JSON",
|
||||
help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
|
||||
"written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
|
||||
@@ -656,6 +734,8 @@ def main() -> None:
|
||||
cfg = load_pose_cfg(robot_data)
|
||||
if args.method:
|
||||
cfg["method"] = args.method
|
||||
if args.marker_observation:
|
||||
cfg["marker_observation"] = args.marker_observation
|
||||
|
||||
fk = RobotFK(robot_data)
|
||||
obs = load_observations(args.markers, cfg.get("use_normals", True),
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -192,14 +192,109 @@ class RobotFK:
|
||||
"""Transform a local marker position → world (mm)."""
|
||||
return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
|
||||
|
||||
# ── Marker-Eckpunkte (Mehrpunkt-Residuen, doc/Homing_5_Pose_MultiPoint_Weighted.md Schritt 3) ──
|
||||
|
||||
@staticmethod
|
||||
def _shortest_arc_R(normal: Sequence[float]) -> np.ndarray:
|
||||
"""
|
||||
Rotationsmatrix [0,0,1] → normal (kürzester Bogen).
|
||||
|
||||
Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n)
|
||||
exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten
|
||||
Ecken zur visuell verifizierten Orientierungs-Konvention in
|
||||
boardViewer.html passen (qNormalLoc dort).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
nn = float(np.linalg.norm(n))
|
||||
n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0])
|
||||
# three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren.
|
||||
r = float(n[2]) + 1.0
|
||||
if r < 1e-12:
|
||||
# antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0)
|
||||
qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0
|
||||
else:
|
||||
qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r # cross([0,0,1], n) = (-ny, nx, 0)
|
||||
ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql
|
||||
return np.array([
|
||||
[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
|
||||
[2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
|
||||
[2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
|
||||
])
|
||||
|
||||
@staticmethod
|
||||
def _marker_plane_corners(half: float) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN
|
||||
Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten
|
||||
`corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen).
|
||||
|
||||
Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell
|
||||
kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt
|
||||
sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die
|
||||
im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am
|
||||
Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm).
|
||||
|
||||
Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert,
|
||||
ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil
|
||||
Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der
|
||||
Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste
|
||||
Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz.
|
||||
|
||||
Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()`
|
||||
(outward = -cross(e01,e02)) wieder +Z liefert — identisch zur
|
||||
Beobachtungs-Konvention.
|
||||
"""
|
||||
h = float(half)
|
||||
return np.array([
|
||||
[ h, h, 0.0], # 0
|
||||
[ h, -h, 0.0], # 1
|
||||
[-h, -h, 0.0], # 2
|
||||
[-h, h, 0.0], # 3
|
||||
])
|
||||
|
||||
@classmethod
|
||||
def marker_corners_local(cls,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`.
|
||||
|
||||
Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale,
|
||||
exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n)
|
||||
plane = cls._marker_plane_corners(float(size_mm) / 2.0) # (4,3)
|
||||
return np.asarray(position, dtype=float) + (R @ plane.T).T # (4,3)
|
||||
|
||||
@classmethod
|
||||
def marker_corners_world(cls,
|
||||
transforms: Dict[str, np.ndarray],
|
||||
link_name: str,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`."""
|
||||
T = transforms.get(link_name, np.eye(4))
|
||||
local = cls.marker_corners_local(position, normal, size_mm, spin_deg)
|
||||
return np.array([transform_point(T, c) for c in local])
|
||||
|
||||
def all_markers_world(self,
|
||||
transforms: Dict[str, np.ndarray]
|
||||
) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Returns
|
||||
-------
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world}
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world, corners_world}
|
||||
|
||||
corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den
|
||||
marker_observation="corner_points"-Modus in 5_pose_estimation.py).
|
||||
"""
|
||||
default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0))
|
||||
result: Dict[int, Dict[str, Any]] = {}
|
||||
for lname, ldata in self.links.items():
|
||||
T = transforms.get(lname, np.eye(4))
|
||||
@@ -210,11 +305,15 @@ class RobotFK:
|
||||
continue
|
||||
loc = np.array(m["position"], dtype=float)
|
||||
nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
|
||||
size_mm = float(m.get("size", default_size))
|
||||
spin_deg = float(m.get("spin", 0.0))
|
||||
local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg)
|
||||
result[mid] = {
|
||||
"world_mm": transform_point(T, loc),
|
||||
"local_mm": loc,
|
||||
"link": lname,
|
||||
"normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
|
||||
"corners_world": np.array([transform_point(T, c) for c in local_corners]),
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
169
test/test_robot_fk_corners.py
Normal file
169
test/test_robot_fk_corners.py
Normal file
@@ -0,0 +1,169 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
test_robot_fk_corners.py
|
||||
========================
|
||||
Isolierter Test für RobotFK.marker_corners_world/_local (doc/
|
||||
Homing_5_Pose_MultiPoint_Weighted.md, Schritt 3) — der Baustein für den
|
||||
marker_observation="corner_points"-Modus.
|
||||
|
||||
Kein pytest nötig (das Repo testet sonst per Jest): plain asserts + main(),
|
||||
Aufruf: python test/test_robot_fk_corners.py
|
||||
|
||||
Geprüft wird die KONVENTION (Eckreihenfolge/Spin/Winding), denn ein Versatz
|
||||
dort gäbe systematischen Bias statt Verbesserung:
|
||||
|
||||
A) Selbst-Konsistenz je Marker (versch. Normalen/Spins):
|
||||
- Schwerpunkt der 4 Ecken == Marker-Center
|
||||
- alle 4 Kanten == size, planar
|
||||
- die aus den Ecken via 3b-Konvention (corner_plane_normal) zurück-
|
||||
gewonnene Normale == Marker-Normale → Orientierung + Winding stimmen
|
||||
B) Reale Daten: ROBOTER-Marker (Arm1/Ellbow/Arm2) am Seed-Pose eines echten
|
||||
Captures — die FK-Ecken müssen zu den triangulierten `corners_m` passen,
|
||||
UND die Identitäts-Paarung (Ecke i ↔ Ecke i) muss jede zyklische/
|
||||
gespiegelte Umordnung schlagen → beweist Ecke-0 + Drehrichtung gegen echte
|
||||
Triangulation. Nur Roboter-Marker, weil deren Spin kalibriert/verifiziert
|
||||
ist; die Board/Rail-Marker (Root-Link) sind spin-unkalibriert und werden im
|
||||
Fit bewusst per Center-Residuum behandelt (nicht über Ecken).
|
||||
"""
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
|
||||
HERE = os.path.dirname(os.path.abspath(__file__))
|
||||
sys.path.insert(0, os.path.join(HERE, "..", "scripts"))
|
||||
from robot_fk import RobotFK # noqa: E402
|
||||
|
||||
ROBOT_JSON = os.path.join(HERE, "..", "scripts", "robot_1781069752019.json")
|
||||
CAP_DIR = os.path.join(HERE, "homing", "20260616_133151")
|
||||
CAPTURE = os.path.join(CAP_DIR, "aruco_marker_poses.json")
|
||||
SEED = os.path.join(CAP_DIR, "state_Arm2.json")
|
||||
|
||||
|
||||
def recovered_normal(corners3d: np.ndarray) -> np.ndarray:
|
||||
"""3b_corner_marker_poses.py corner_plane_normal() — identisch nachgebaut."""
|
||||
center = corners3d.mean(axis=0)
|
||||
_, _, Vt = np.linalg.svd(corners3d - center)
|
||||
n = Vt[-1]
|
||||
cross = np.cross(corners3d[1] - corners3d[0], corners3d[2] - corners3d[0])
|
||||
if np.dot(n, cross) > 0:
|
||||
n = -n
|
||||
nn = np.linalg.norm(n)
|
||||
return n / nn if nn > 1e-12 else n
|
||||
|
||||
|
||||
def edge_lengths(c: np.ndarray):
|
||||
return [float(np.linalg.norm(c[(i + 1) % 4] - c[i])) for i in range(4)]
|
||||
|
||||
|
||||
# ── A) Selbst-Konsistenz ────────────────────────────────────────────────────
|
||||
|
||||
def test_self_consistency():
|
||||
fk = RobotFK.from_file(ROBOT_JSON)
|
||||
cases = [
|
||||
([0, 0, 1], 0.0),
|
||||
([0, 0, 1], 90.0),
|
||||
([0, 0, 1], 37.5),
|
||||
([0, -1, 0], 90.0),
|
||||
([-1, 0, 0], 0.0),
|
||||
([0, 0, -1], 12.0), # antiparalleler Sonderfall der Minimal-Rotation
|
||||
([1, 1, 0.3], 215.0), # schräge Normale + großer Spin
|
||||
]
|
||||
size = 25.0
|
||||
pos = np.array([100.0, -50.0, 30.0])
|
||||
for normal, spin in cases:
|
||||
local = RobotFK.marker_corners_local(pos, normal, size, spin)
|
||||
assert local.shape == (4, 3), f"shape {local.shape}"
|
||||
# Schwerpunkt == Center
|
||||
d_center = np.linalg.norm(local.mean(axis=0) - pos)
|
||||
assert d_center < 1e-9, f"centroid off by {d_center} (n={normal}, spin={spin})"
|
||||
# Kanten == size, alle gleich (Quadrat)
|
||||
for L in edge_lengths(local):
|
||||
assert abs(L - size) < 1e-9, f"edge {L} != {size} (n={normal}, spin={spin})"
|
||||
# planar: alle 4 in einer Ebene (Abstand zur SVD-Ebene ~0)
|
||||
rel = local - local.mean(axis=0)
|
||||
_, _, Vt = np.linalg.svd(rel)
|
||||
planar = np.max(np.abs(rel @ Vt[-1]))
|
||||
assert planar < 1e-9, f"not planar ({planar})"
|
||||
# Normale aus den Ecken (3b-Konvention) == Marker-Normale
|
||||
n_exp = np.asarray(normal, float) / np.linalg.norm(normal)
|
||||
n_rec = recovered_normal(local)
|
||||
dot = float(np.dot(n_exp, n_rec))
|
||||
assert dot > 0.99999, (
|
||||
f"recovered normal {n_rec} != {n_exp} (dot={dot:.6f}, spin={spin})")
|
||||
print(f"[PASS] A) Selbst-Konsistenz: {len(cases)} Fälle "
|
||||
"(Center, Kanten, planar, Normalen-Rückgewinnung).")
|
||||
|
||||
|
||||
# ── B) Reale Capture-Daten (Board-Marker) ───────────────────────────────────
|
||||
|
||||
def best_pairing_error(model_c: np.ndarray, obs_c: np.ndarray):
|
||||
"""
|
||||
RMS der Eck-Paarung nach Center-Abzug, für Identität und alle 8 Umordnungen
|
||||
(4 zyklische × {vorwärts, rückwärts}). Gibt (rms_identity, rms_best, name_best).
|
||||
"""
|
||||
m = model_c - model_c.mean(axis=0)
|
||||
o = obs_c - obs_c.mean(axis=0)
|
||||
best_rms, best_name, id_rms = None, None, None
|
||||
for reverse in (False, True):
|
||||
base = o[::-1] if reverse else o
|
||||
for roll in range(4):
|
||||
cand = np.roll(base, roll, axis=0)
|
||||
rms = float(np.sqrt(np.mean(np.sum((m - cand) ** 2, axis=1))))
|
||||
name = f"{'rev' if reverse else 'fwd'}+roll{roll}"
|
||||
if best_rms is None or rms < best_rms:
|
||||
best_rms, best_name = rms, name
|
||||
if not reverse and roll == 0:
|
||||
id_rms = rms
|
||||
return id_rms, best_rms, best_name
|
||||
|
||||
|
||||
def test_against_real_capture():
|
||||
if not (os.path.exists(CAPTURE) and os.path.exists(SEED)):
|
||||
print(f"[SKIP] B) Capture/Seed fehlt: {CAP_DIR}")
|
||||
return
|
||||
fk = RobotFK.from_file(ROBOT_JSON)
|
||||
seed = json.load(open(SEED, "r", encoding="utf-8")).get("accumulated_state", {})
|
||||
transforms = fk.compute(seed) # Roboter-Marker brauchen den Pose-Zustand
|
||||
model = fk.all_markers_world(transforms)
|
||||
|
||||
# Root-Link (Board) bestimmen → davon wird NICHT über Ecken validiert.
|
||||
roots = {ln for ln, ld in fk.links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in fk.links}
|
||||
|
||||
data = json.load(open(CAPTURE, "r", encoding="utf-8"))
|
||||
checked = 0
|
||||
for m in data.get("markers", []):
|
||||
mid = m["marker_id"]
|
||||
if mid not in model or not m.get("corners_m"):
|
||||
continue
|
||||
if model[mid]["link"] in roots: # Board/Rail: bewusst Center-only
|
||||
continue
|
||||
obs_c = np.array(m["corners_m"], dtype=float) * 1000.0 # m → mm
|
||||
model_c = np.asarray(model[mid]["corners_world"], float)
|
||||
# Schlecht lokalisierte/fehlassoziierte Marker (Center weit daneben am
|
||||
# Seed) überspringen — sie sagen nichts über die Eckreihenfolge aus.
|
||||
if np.linalg.norm(model[mid]["world_mm"] - obs_c.mean(axis=0)) > 50.0:
|
||||
continue
|
||||
id_rms, best_rms, best_name = best_pairing_error(model_c, obs_c)
|
||||
# 1) Form/Orientierung stimmt: Identitäts-RMS klein (Seed- + Triangulationsrauschen)
|
||||
assert id_rms < 6.0, f"Roboter-Marker {mid}: Identitäts-RMS {id_rms:.2f}mm zu groß"
|
||||
# 2) KEINE Umordnung ist besser als die Identität → Ecke-0 + Winding korrekt
|
||||
assert best_name == "fwd+roll0", (
|
||||
f"Roboter-Marker {mid}: '{best_name}' (RMS {best_rms:.2f}) schlägt Identität "
|
||||
f"(RMS {id_rms:.2f}) → Eckreihenfolge falsch")
|
||||
checked += 1
|
||||
assert checked >= 4, f"zu wenige verwertbare Roboter-Marker geprüft ({checked})"
|
||||
print(f"[PASS] B) Reale Capture-Daten: {checked} ROBOTER-Marker am Seed — "
|
||||
"FK-Ecken passen zu corners_m, Identitäts-Paarung ist optimal.")
|
||||
|
||||
|
||||
def main():
|
||||
test_self_consistency()
|
||||
test_against_real_capture()
|
||||
print("\n[OK] Alle Tests bestanden.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user