From 9105dc5eac4601790cd673634873347c974e50c1 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Wed, 10 Jun 2026 17:57:05 +0200 Subject: [PATCH] board rotation --- public/boardViewer.html | 16 ++- public/calibration.js | 71 +++++++++++ public/calibration_board.html | 39 +++++++ server/editRobot.js | 214 ++++++++++++++++++++++++++++++++++ server/server.js | 72 +++++++++++- 5 files changed, 407 insertions(+), 5 deletions(-) diff --git a/public/boardViewer.html b/public/boardViewer.html index 37b5d2e..197dccd 100644 --- 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); } diff --git a/public/calibration.js b/public/calibration.js index 785cfc6..953d79d 100644 --- a/public/calibration.js +++ b/public/calibration.js @@ -481,4 +481,75 @@ function initBoard() { result.innerHTML = `❌ ${err}`; } }); + + // ── 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 = '⚠ Bitte Set-Name eingeben (z.B. "rail").'; return; + } + + result.innerHTML = 'Justierung läuft …'; + 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 = `❌ ${data.error ?? `HTTP ${r.status}`}`; return; + } + const t = data.transform; + result.innerHTML = + `✅ Set "${setToMove}": ${data.numChanged} Marker verschoben` + + ` (${data.numMatchingPts} Messpunkte)` + + `  Δx=${t.tx} mm Δy=${t.ty} mm Δz=${t.tz} mm` + + ` θ=${t.thetaDeg}°`; + loadBoardTable(); + } catch (err) { + result.innerHTML = `❌ ${err}`; + } + }); + + // ── 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 = '⚠ Bitte Marker-ID eingeben.'; return; + } + if (!set && !link) { + result.innerHTML = '⚠ Bitte mindestens Set oder Link angeben.'; return; + } + + result.innerHTML = 'Hinzufügen …'; + 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 = `❌ ${data.error ?? `HTTP ${r.status}`}`; return; + } + if (!data.changed) { + result.innerHTML = `❌ ${data.error}`; 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 = `✅ Marker ${markerId}: ${info}`; + loadBoardTable(); + } catch (err) { + result.innerHTML = `❌ ${err}`; + } + }); } diff --git a/public/calibration_board.html b/public/calibration_board.html index cdc9ceb..3256e81 100644 --- a/public/calibration_board.html +++ b/public/calibration_board.html @@ -72,6 +72,45 @@ + +
+

+ Sets justieren (zu 3b-Messung) +

+
+ Set verschieben + + + Rotation (Z-Achse) + Translation → passt Set zu 3b-Messung +
+
+ + +
+

+ Zuordnung hinzufügen +

+
+ ID + + Set + + Link + + +
+
+
diff --git a/server/editRobot.js b/server/editRobot.js index 4fe5588..723942e 100644 --- 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 ?? '' }, + }; +} diff --git a/server/server.js b/server/server.js index 6c1f070..906b668 100755 --- 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