board rotation

2026-06-10 17:57:05 +02:00
parent 5d13b7685c
commit 9105dc5eac
5 changed files with 407 additions and 5 deletions
--- a/public/boardViewer.html
+++ b/public/boardViewer.html
@@ -260,13 +260,14 @@ function buildScene(data) {
  if (boardMarkers.length > 0) {
    let minRx = Infinity, maxRx = -Infinity;
    let minRy = Infinity, maxRy = -Infinity;
-    let markerRz = -27.3;
+    let markerRz = Infinity;  // Minimum aller z – tiefster Marker (z=up → kleinstes = am tiefsten)
    for (const m of boardMarkers) {
      const [rx, ry, rz] = m.position;
      if (rx < minRx) minRx = rx; if (rx > maxRx) maxRx = rx;
      if (ry < minRy) minRy = ry; if (ry > maxRy) maxRy = ry;
-      markerRz = rz;
+      if (rz < markerRz) markerRz = rz;
    }
+    if (!isFinite(markerRz)) markerRz = -27.3;
    const pad    = 40;  // mm Rand
    const planeW = (maxRx - minRx + 2 * pad) * S;
    const planeH = (maxRy - minRy + 2 * pad) * S;
@@ -290,13 +291,20 @@ function buildScene(data) {
    const pos     = r2vArr(m.position);
    const detected = detectedIds.has(m.id);
    if (detected) nDetected++;
-    const color   = detected ? 0x22c55e : 0xef4444;
+    const isA0 = m.set === 'A0';
+    // A0: grün (erkannt) / rot (nicht erkannt)   andere Sets: blau / dunkelblau
+    const color = isA0
+      ? (detected ? 0x22c55e : 0xef4444)
+      : (detected ? 0x3b82f6 : 0x1e40af);

    const sq = makeMarkerSquare(pos, visSize, color);
    sq.position.y += 0.0005;
    gMarkers.add(sq);

-    const border = makeEdgeBorder(pos, visSize, detected ? 0x4ade80 : 0xfca5a5);
+    const borderCol = isA0
+      ? (detected ? 0x4ade80 : 0xfca5a5)
+      : (detected ? 0x60a5fa : 0x3b82f6);
+    const border = makeEdgeBorder(pos, visSize, borderCol);
    border.position.y += 0.001;
    gMarkers.add(border);
  }
--- a/public/calibration.js
+++ b/public/calibration.js
@@ -481,4 +481,75 @@ function initBoard() {
      result.innerHTML = `<span style="color:#f87171">❌ ${err}</span>`;
    }
  });
+
+  // ── Aktion 3: Sets justieren (Kabsch 2D+Z) ─────────────────────────────────
+  document.getElementById('btn-act-align').addEventListener('click', async () => {
+    const setToMove = document.getElementById('act-align-set').value.trim();
+    const result    = document.getElementById('act-result');
+
+    if (!setToMove) {
+      result.innerHTML = '<span style="color:#f87171">⚠ Bitte Set-Name eingeben (z.B. "rail").</span>'; return;
+    }
+
+    result.innerHTML = '<span style="color:#555b6e">Justierung läuft …</span>';
+    try {
+      const r = await fetch('/api/robot/align-sets', {
+        method: 'POST',
+        headers: { 'Content-Type': 'application/json' },
+        body: JSON.stringify({ setToMove }),
+      });
+      const data = await r.json();
+      if (!r.ok || data.error) {
+        result.innerHTML = `<span style="color:#f87171">❌ ${data.error ?? `HTTP ${r.status}`}</span>`; return;
+      }
+      const t = data.transform;
+      result.innerHTML =
+        `<span style="color:#22c55e">✅ Set "${setToMove}": ${data.numChanged} Marker verschoben` +
+        ` (${data.numMatchingPts} Messpunkte)</span>` +
+        ` &ensp;Δx=${t.tx}&thinsp;mm&ensp;Δy=${t.ty}&thinsp;mm&ensp;Δz=${t.tz}&thinsp;mm` +
+        `&ensp;θ=${t.thetaDeg}°`;
+      loadBoardTable();
+    } catch (err) {
+      result.innerHTML = `<span style="color:#f87171">❌ ${err}</span>`;
+    }
+  });
+
+  // ── Aktion 4: Zuordnung hinzufügen ─────────────────────────────────────────
+  document.getElementById('btn-act-add').addEventListener('click', async () => {
+    const markerId = document.getElementById('act-add-id').value.trim();
+    const set      = document.getElementById('act-add-set').value.trim();
+    const link     = document.getElementById('act-add-link').value.trim();
+    const result   = document.getElementById('act-result');
+
+    if (!markerId) {
+      result.innerHTML = '<span style="color:#f87171">⚠ Bitte Marker-ID eingeben.</span>'; return;
+    }
+    if (!set && !link) {
+      result.innerHTML = '<span style="color:#f87171">⚠ Bitte mindestens Set oder Link angeben.</span>'; return;
+    }
+
+    result.innerHTML = '<span style="color:#555b6e">Hinzufügen …</span>';
+    try {
+      const r = await fetch('/api/robot/assign-id', {
+        method: 'POST',
+        headers: { 'Content-Type': 'application/json' },
+        body: JSON.stringify({ markerId: parseInt(markerId, 10), set, link }),
+      });
+      const data = await r.json();
+      if (!r.ok) {
+        result.innerHTML = `<span style="color:#f87171">❌ ${data.error ?? `HTTP ${r.status}`}</span>`; return;
+      }
+      if (!data.changed) {
+        result.innerHTML = `<span style="color:#f87171">❌ ${data.error}</span>`; return;
+      }
+      const c = data.change;
+      const info = c.action === 'added'
+        ? `neu in Link "${c.newLink}"${c.newSet ? ` Set "${c.newSet}"` : ''}`
+        : `aktualisiert → Link "${c.newLink}"${c.newSet ? ` Set "${c.newSet}"` : ''}`;
+      result.innerHTML = `<span style="color:#22c55e">✅ Marker ${markerId}: ${info}</span>`;
+      loadBoardTable();
+    } catch (err) {
+      result.innerHTML = `<span style="color:#f87171">❌ ${err}</span>`;
+    }
+  });
 }
--- a/public/calibration_board.html
+++ b/public/calibration_board.html
@@ -72,6 +72,45 @@
      </div>
    </div>

+    <!-- Aktion 3: Sets justieren (Kabsch 2D+Z) -->
+    <div style="margin-top:14px">
+      <p style="font-size:10px;color:var(--muted);text-transform:uppercase;letter-spacing:.06em;margin-bottom:8px">
+        Sets justieren (zu 3b-Messung)
+      </p>
+      <div style="display:flex;flex-wrap:wrap;align-items:center;gap:8px;font-size:12px;color:var(--text)">
+        Set verschieben
+        <input id="act-align-set" type="text" placeholder="rail"
+               style="width:90px;background:#1e293b;border:1px solid #334155;color:#c8cdd8;border-radius:3px;padding:3px 7px;font:inherit;font-size:12px">
+        <button id="btn-act-align"
+                style="background:#1e293b;color:#c8cdd8;border:1px solid #4a9eff;border-radius:3px;padding:4px 14px;cursor:pointer;font:inherit;font-size:12px">
+          Justieren
+        </button>
+        <span style="color:var(--muted);font-size:10px">Rotation (Z-Achse) + Translation → passt Set zu 3b-Messung</span>
+      </div>
+    </div>
+
+    <!-- Aktion 4: Zuordnung hinzufügen -->
+    <div style="margin-top:14px">
+      <p style="font-size:10px;color:var(--muted);text-transform:uppercase;letter-spacing:.06em;margin-bottom:8px">
+        Zuordnung hinzufügen
+      </p>
+      <div style="display:flex;flex-wrap:wrap;align-items:center;gap:8px;font-size:12px;color:var(--text)">
+        ID
+        <input id="act-add-id" type="number" placeholder="z.B. 210"
+               style="width:90px;background:#1e293b;border:1px solid #334155;color:#c8cdd8;border-radius:3px;padding:3px 7px;font:inherit;font-size:12px">
+        Set
+        <input id="act-add-set" type="text" placeholder="A0"
+               style="width:72px;background:#1e293b;border:1px solid #334155;color:#c8cdd8;border-radius:3px;padding:3px 7px;font:inherit;font-size:12px">
+        Link
+        <input id="act-add-link" type="text" placeholder="Board"
+               style="width:90px;background:#1e293b;border:1px solid #334155;color:#c8cdd8;border-radius:3px;padding:3px 7px;font:inherit;font-size:12px">
+        <button id="btn-act-add"
+                style="background:#1e293b;color:#c8cdd8;border:1px solid #22c55e;border-radius:3px;padding:4px 14px;cursor:pointer;font:inherit;font-size:12px">
+          Hinzufügen
+        </button>
+      </div>
+    </div>
+
    <!-- Ergebnis-Zeile -->
    <div id="act-result" style="margin-top:10px;font-size:11px;min-height:18px;color:var(--muted)"></div>
  </div>
--- a/server/editRobot.js
+++ b/server/editRobot.js
@@ -175,3 +175,217 @@ export async function removeMarkerAssignment(robotPath, { markerId, removeFrom }

  return { changed: false, error: `Marker-ID ${id} nicht gefunden.` };
 }
+
+// ── Aktion 3: Sets untereinander justieren (2D+Z Kabsch) ─────────────────────
+
+/**
+ * Optimale 2D-Rotation (um Z-Achse) + 3D-Translation, die model→measured minimiert.
+ * Gibt { tx, ty, tz, theta, thetaDeg } zurück.
+ * modelPts / measuredPts: Arrays von [x, y, z] in mm.
+ */
+function computeRigid2DZ(modelPts, measuredPts) {
+  const n = modelPts.length;
+  if (n === 0) return { tx: 0, ty: 0, tz: 0, theta: 0, thetaDeg: 0 };
+
+  // Schwerpunkte
+  let mCx = 0, mCy = 0, mCz = 0, pCx = 0, pCy = 0, pCz = 0;
+  for (let i = 0; i < n; i++) {
+    mCx += modelPts[i][0];   mCy += modelPts[i][1];   mCz += modelPts[i][2];
+    pCx += measuredPts[i][0]; pCy += measuredPts[i][1]; pCz += measuredPts[i][2];
+  }
+  mCx /= n; mCy /= n; mCz /= n;
+  pCx /= n; pCy /= n; pCz /= n;
+
+  // 2D-Kreuzkovarianz für Kabsch-Rotation in XY-Ebene
+  let H00 = 0, H01 = 0, H10 = 0, H11 = 0;
+  for (let i = 0; i < n; i++) {
+    const ax = modelPts[i][0] - mCx,   ay = modelPts[i][1] - mCy;
+    const bx = measuredPts[i][0] - pCx, by = measuredPts[i][1] - pCy;
+    H00 += ax * bx; H01 += ax * by;
+    H10 += ay * bx; H11 += ay * by;
+  }
+
+  // Optimaler Drehwinkel (2D-Sonderfall von Kabsch / SVD → atan2)
+  const theta = Math.atan2(H10 - H01, H00 + H11);
+  const cos   = Math.cos(theta);
+  const sin   = Math.sin(theta);
+
+  // Translation: gemessener Schwerpunkt − R × Modell-Schwerpunkt
+  const tx = pCx - (cos * mCx - sin * mCy);
+  const ty = pCy - (sin * mCx + cos * mCy);
+  const tz = pCz - mCz;
+
+  return { tx, ty, tz, theta, thetaDeg: theta * 180 / Math.PI };
+}
+
+function applyRigid2DZ([x, y, z], { tx, ty, tz, theta }) {
+  const cos = Math.cos(theta);
+  const sin = Math.sin(theta);
+  return [
+    Math.round((cos * x - sin * y + tx) * 100) / 100,
+    Math.round((sin * x + cos * y + ty) * 100) / 100,
+    Math.round((z + tz) * 100) / 100,
+  ];
+}
+
+/**
+ * Richtet alle Marker des Sets `setToMove` rigid an ihren triangulieren 3b-Positionen aus.
+ * Das andere Set bleibt unberührt – die Transformation wird aus den Paaren
+ * (Modellposition, Messposition) der matching Marker berechnet.
+ *
+ * setToMove:    Name des zu verschiebenden Sets (z.B. "rail")
+ * extraMarkers: Marker aus aruco_marker_poses.json (mit marker_id, position_mm)
+ *
+ * Gibt { numChanged, numMatchingPts, transform: {tx, ty, tz, thetaDeg} } zurück,
+ * oder { error } bei Fehler.
+ */
+export async function alignSetToMeasured(robotPath, { setToMove, extraMarkers = [] }) {
+  const robot = await readRobot(robotPath);
+  const links = robot.links ?? {};
+
+  // Alle Marker des zu verschiebenden Sets sammeln
+  const setMarkers = [];
+  for (const [linkName, linkData] of Object.entries(links)) {
+    for (const marker of (linkData.markers ?? [])) {
+      if (marker.set === setToMove) {
+        setMarkers.push({ marker, linkName });
+      }
+    }
+  }
+
+  if (setMarkers.length === 0) {
+    return { error: `Set "${setToMove}" nicht gefunden oder leer.` };
+  }
+
+  // Gemessene Positionen aus 3b indizieren
+  const measuredMap = new Map();
+  for (const em of extraMarkers) {
+    if (Array.isArray(em.position_mm) && em.position_mm.length >= 3) {
+      measuredMap.set(Number(em.marker_id), em.position_mm.map(Number));
+    }
+  }
+
+  // Matching-Paare für Kabsch (nur Marker, die sowohl Modellposition als auch Messung haben)
+  const modelPts = [], measuredPts = [];
+  for (const { marker } of setMarkers) {
+    if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
+    const mpos = measuredMap.get(Number(marker.id));
+    if (!mpos) continue;
+    modelPts.push(marker.position.map(Number));
+    measuredPts.push(mpos);
+  }
+
+  if (modelPts.length < 2) {
+    return {
+      error: `Zu wenig Messpunkte für Set "${setToMove}" (${modelPts.length} von ${setMarkers.length} Markern gemessen). ` +
+             `Bitte Board-Run mit ≥2 Kameras durchführen.`,
+    };
+  }
+
+  const transform = computeRigid2DZ(modelPts, measuredPts);
+
+  // Transformation auf ALLE Marker des Sets anwenden (auch nicht gemessene)
+  let numChanged = 0;
+  for (const { marker } of setMarkers) {
+    if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
+    marker.position = applyRigid2DZ(marker.position.map(Number), transform);
+    numChanged++;
+  }
+
+  robot.links = links;
+  await writeRobot(robotPath, robot);
+
+  return {
+    numChanged,
+    numMatchingPts: modelPts.length,
+    transform: {
+      tx:       Math.round(transform.tx       * 100) / 100,
+      ty:       Math.round(transform.ty       * 100) / 100,
+      tz:       Math.round(transform.tz       * 100) / 100,
+      thetaDeg: Math.round(transform.thetaDeg * 100) / 100,
+    },
+  };
+}
+
+// ── Aktion 4: Einzelnen Marker per ID hinzufügen / aktualisieren ──────────────
+
+/**
+ * Fügt einen Marker per ID zu Set und Link hinzu oder aktualisiert einen bestehenden.
+ *
+ * - Existiert der Marker bereits in robot.json: set und/oder link werden aktualisiert.
+ * - Existiert er noch nicht: Position wird aus extraMarkers (3b-Output) geholt und
+ *   der Marker wird dem angegebenen Link hinzugefügt.
+ *
+ * Gibt { changed, change } oder { changed: false, error } zurück.
+ */
+export async function assignMarkerId(robotPath, { markerId, set, link, extraMarkers = [] }) {
+  const id = Number(markerId);
+  if (!Number.isFinite(id) || id < 0) return { changed: false, error: 'Ungültige Marker-ID.' };
+
+  const robot = await readRobot(robotPath);
+  const links = robot.links ?? {};
+
+  // Marker in robot.json suchen
+  let foundMarker = null, foundLink = null;
+  outer: for (const [linkName, linkData] of Object.entries(links)) {
+    for (const m of (linkData.markers ?? [])) {
+      if (Number(m.id) === id) { foundMarker = m; foundLink = linkName; break outer; }
+    }
+  }
+
+  if (foundMarker) {
+    // Bestehenden Marker: Set und ggf. Link aktualisieren
+    const change = {
+      action:  'updated',
+      markerId: id,
+      oldLink:  foundLink,
+      oldSet:   foundMarker.set ?? '',
+      newLink:  foundLink,
+      newSet:   foundMarker.set ?? '',
+    };
+
+    if (set !== undefined && set !== '') { foundMarker.set = set; change.newSet = set; }
+
+    if (link && link !== foundLink) {
+      // In neuen Link verschieben
+      links[foundLink].markers = links[foundLink].markers.filter(m => Number(m.id) !== id);
+      if (!links[link])         links[link]         = { markers: [] };
+      if (!links[link].markers) links[link].markers = [];
+      links[link].markers.push(foundMarker);
+      change.newLink = link;
+    }
+
+    robot.links = links;
+    await writeRobot(robotPath, robot);
+    return { changed: true, change };
+  }
+
+  // Neuer Marker: Position aus 3b-Output holen
+  const em = extraMarkers.find(m => Number(m.marker_id) === id);
+  if (!em) {
+    return {
+      changed: false,
+      error: `Marker ${id} ist nicht in robot.json und nicht im letzten 3b-Run vorhanden.`,
+    };
+  }
+  if (!link) {
+    return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
+  }
+
+  const newMarker = {
+    id,
+    position: em.position_mm.map(v => Math.round(Number(v) * 100) / 100),
+  };
+  if (set) newMarker.set = set;
+
+  if (!links[link])         links[link]         = { markers: [] };
+  if (!links[link].markers) links[link].markers = [];
+  links[link].markers.push(newMarker);
+
+  robot.links = links;
+  await writeRobot(robotPath, robot);
+  return {
+    changed: true,
+    change: { action: 'added', markerId: id, oldLink: null, oldSet: '', newLink: link, newSet: set ?? '' },
+  };
+}
--- a/server/server.js
+++ b/server/server.js
@@ -8,7 +8,7 @@ import { fileURLToPath } from 'url';
 import process from 'process';
 import { spawn } from 'child_process';
 import { WebcamClient } from './webcamClient.js';
-import { assignByZRange, removeMarkerAssignment } from './editRobot.js';
+import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId } from './editRobot.js';

 const __filename = fileURLToPath(import.meta.url);
 const __dirname = path.dirname(__filename);
@@ -753,6 +753,76 @@ app.post('/api/robot/remove-marker', async (req, res) => {
  }
 });

+/**
+ * POST /api/robot/align-sets
+ * Richtet alle Marker des angegebenen Sets rigid (2D-Rotation um Z + 3D-Translation)
+ * an den aktuellen 3b-Messpositionen aus.
+ * Body: { setToMove }
+ */
+app.post('/api/robot/align-sets', async (req, res) => {
+  try {
+    const { setToMove } = req.body ?? {};
+    if (!setToMove) return res.status(400).json({ error: '"setToMove" ist erforderlich.' });
+
+    let extraMarkers = [];
+    try {
+      const latestRun = await findLatestBoardRun();
+      if (latestRun) {
+        const posesPath = path.join(boardDataDir, latestRun, 'aruco_marker_poses.json');
+        const poses = JSON.parse(await fsPromises.readFile(posesPath, 'utf8'));
+        extraMarkers = poses.markers ?? [];
+      }
+    } catch { /* kein 3b-Output vorhanden */ }
+
+    const result = await alignSetToMeasured(ROBOT_JSON, { setToMove, extraMarkers });
+    if (result.error) return res.status(400).json(result);
+
+    console.log(
+      `robot/align-sets set="${setToMove}" → ${result.numChanged} Marker verschoben` +
+      ` (${result.numMatchingPts} Messpunkte) Δx=${result.transform.tx} Δy=${result.transform.ty}` +
+      ` Δz=${result.transform.tz} mm θ=${result.transform.thetaDeg}°`,
+    );
+    return res.json(result);
+  } catch (err) {
+    console.error('robot/align-sets error:', err);
+    return res.status(500).json({ error: String(err) });
+  }
+});
+
+/**
+ * POST /api/robot/assign-id
+ * Fügt einen einzelnen Marker per ID zu Set und Link hinzu oder aktualisiert ihn.
+ * Body: { markerId, set?, link? }
+ */
+app.post('/api/robot/assign-id', async (req, res) => {
+  try {
+    const { markerId, set, link } = req.body ?? {};
+    if (markerId == null) return res.status(400).json({ error: '"markerId" ist erforderlich.' });
+
+    let extraMarkers = [];
+    try {
+      const latestRun = await findLatestBoardRun();
+      if (latestRun) {
+        const posesPath = path.join(boardDataDir, latestRun, 'aruco_marker_poses.json');
+        const poses = JSON.parse(await fsPromises.readFile(posesPath, 'utf8'));
+        extraMarkers = poses.markers ?? [];
+      }
+    } catch { /* kein 3b-Output vorhanden */ }
+
+    const result = await assignMarkerId(ROBOT_JSON, { markerId, set, link, extraMarkers });
+    if (!result.changed && result.error) return res.status(400).json(result);
+
+    console.log(
+      `robot/assign-id id=${markerId} set="${set ?? ''}" link="${link ?? ''}"` +
+      ` → ${result.change?.action ?? 'unverändert'}`,
+    );
+    return res.json(result);
+  } catch (err) {
+    console.error('robot/assign-id error:', err);
+    return res.status(500).json({ error: String(err) });
+  }
+});
+
 /**
 * POST /api/calibration/upload-npz
 * Liest {camera}_calibration.npz aus der aktuellen Session und