From 5285ed468b1f3fa4b8d9369c33d4c38ad5ea1198 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Wed, 10 Jun 2026 14:58:14 +0200 Subject: [PATCH] Board Kamera-Position --- public/boardViewer.html | 246 ++++++++++++++++-------------- scripts/3b_corner_marker_poses.py | 4 +- server/server.js | 28 +++- 3 files changed, 163 insertions(+), 115 deletions(-) diff --git a/public/boardViewer.html b/public/boardViewer.html index a138a40..d52071c 100644 --- a/public/boardViewer.html +++ b/public/boardViewer.html @@ -22,8 +22,6 @@ height: 100vh; overflow: hidden; } - - /* ── Topbar ── */ #topbar { display: flex; align-items: center; @@ -34,12 +32,13 @@ flex-shrink: 0; flex-wrap: wrap; } - .legend { display: flex; gap: 12px; align-items: center; } + .legend { display: flex; gap: 10px; align-items: center; } .dot { display: inline-block; width: 10px; height: 10px; border-radius: 2px; margin-right: 3px; vertical-align: middle; } - #status { color: var(--muted); flex: 1; min-width: 120px; } + .dot.circle { border-radius: 50%; } + #status { color: var(--muted); flex: 1; min-width: 100px; } #stats { color: var(--accent); } .btn { background: #1e293b; @@ -52,26 +51,18 @@ font-size: 11px; } .btn:hover { border-color: var(--accent); color: var(--accent); } - - /* ── Canvas ── */ #canvas-wrap { flex: 1; position: relative; overflow: hidden; } canvas { display: block; width: 100%; height: 100%; } - - /* ── Hint overlay ── */ #hint { - position: absolute; - bottom: 6px; right: 10px; - font-size: 9px; - color: var(--muted); - pointer-events: none; + position: absolute; bottom: 6px; right: 10px; + font-size: 9px; color: var(--muted); pointer-events: none; } - @@ -82,6 +73,7 @@
Erkannt Nicht erkannt + Gemessen (3b) Kamera
@@ -98,13 +90,12 @@ import * as THREE from 'three'; import { OrbitControls } from 'three/addons/controls/OrbitControls.js'; -const S = 1 / 1000; // mm → m +const S = 1 / 1000; // mm → m // robot (x=right, y=backward, z=up) → Three.js (x=right, y=up, z=toward viewer) -function r2v(rx, ry, rz) { return new THREE.Vector3(rx * S, rz * S, -ry * S); } +function r2v(rx, ry, rz) { return new THREE.Vector3(rx * S, rz * S, -ry * S); } function r2vArr([rx, ry, rz]) { return r2v(rx, ry, rz); } -// direction vector (no scale) -function r2dir([dx, dy, dz]) { return new THREE.Vector3(dx, dz, -dy).normalize(); } +function r2dir([dx, dy, dz]) { return new THREE.Vector3(dx, dz, -dy).normalize(); } // ── Renderer ────────────────────────────────────────────────────────────────── const canvas = document.getElementById('cv'); @@ -114,7 +105,6 @@ renderer.setPixelRatio(devicePixelRatio); const scene = new THREE.Scene(); scene.background = new THREE.Color(0x0d0f13); -// Initial camera: top-angled view, board center ≈ (0.49, -0.03, 0.015) const CAM_TARGET = new THREE.Vector3(0.49, -0.03, 0.015); const cam = new THREE.PerspectiveCamera(45, 1, 0.001, 20); cam.position.set(0.49, 1.5, 0.85); @@ -125,20 +115,19 @@ controls.target.copy(CAM_TARGET); controls.enableDamping = true; controls.dampingFactor = 0.08; -// Lighting -scene.add(new THREE.AmbientLight(0xffffff, 0.75)); +scene.add(new THREE.AmbientLight(0xffffff, 0.8)); const sun = new THREE.DirectionalLight(0xfff4e0, 0.6); sun.position.set(-0.5, 1.2, 0.5); scene.add(sun); -// World-origin axes scene.add(new THREE.AxesHelper(0.08)); // ── Groups ──────────────────────────────────────────────────────────────────── -const gPaper = new THREE.Group(); // white A0 plane -const gMarkers = new THREE.Group(); // ArUco squares -const gCameras = new THREE.Group(); // camera frustums -scene.add(gPaper, gMarkers, gCameras); +const gPaper = new THREE.Group(); // weißes A0-Papier +const gMarkers = new THREE.Group(); // Modell-Rechtecke +const gMeasured = new THREE.Group(); // gemessene Positionen (3b) +const gCameras = new THREE.Group(); // Kamera-Frusta +scene.add(gPaper, gMarkers, gMeasured, gCameras); function clearGroup(g) { while (g.children.length) { @@ -151,17 +140,7 @@ function clearGroup(g) { } // ── Geometry helpers ────────────────────────────────────────────────────────── -function makePlane(w, d, color, opacity = 1) { - const geo = new THREE.PlaneGeometry(w, d); - geo.rotateX(-Math.PI / 2); - const mat = new THREE.MeshPhongMaterial({ - color, side: THREE.DoubleSide, - transparent: opacity < 1, opacity, - }); - return new THREE.Mesh(geo, mat); -} - -function makeSquareMarker(pos, size, color) { +function makeMarkerSquare(pos, size, color) { const geo = new THREE.PlaneGeometry(size, size); geo.rotateX(-Math.PI / 2); const mat = new THREE.MeshPhongMaterial({ color, side: THREE.DoubleSide }); @@ -170,19 +149,20 @@ function makeSquareMarker(pos, size, color) { return m; } -function makeCameraFrustum(posThree, dirThree, size) { - const geo = new THREE.ConeGeometry(size * 0.55, size, 4); - geo.translate(0, -size / 2, 0); // apex → local origin - geo.rotateY(Math.PI / 4); - const mat = new THREE.MeshPhongMaterial({ - color: 0x9b7bff, transparent: true, opacity: 0.55, side: THREE.DoubleSide, - }); - const m = new THREE.Mesh(geo, mat); - m.position.copy(posThree); - const d = dirThree.clone().normalize(); - if (d.lengthSq() > 1e-9) { - m.quaternion.setFromUnitVectors(new THREE.Vector3(0, -1, 0), d); - } +function makeEdgeBorder(pos, size, color) { + const geo = new THREE.EdgesGeometry(new THREE.PlaneGeometry(size, size)); + const mat = new THREE.LineBasicMaterial({ color, transparent: true, opacity: 0.75 }); + const m = new THREE.LineSegments(geo, mat); + m.rotation.x = -Math.PI / 2; + m.position.copy(pos); + return m; +} + +function makeSphere(pos, radius, color) { + const geo = new THREE.SphereGeometry(radius, 10, 8); + const mat = new THREE.MeshPhongMaterial({ color, shininess: 80 }); + const m = new THREE.Mesh(geo, mat); + m.position.copy(pos); return m; } @@ -192,105 +172,150 @@ function makeLine(p1, p2, color, opacity = 1) { return new THREE.Line(geo, mat); } -// ── Build scene from API data ───────────────────────────────────────────────── +function makeCameraFrustum(posThree, dirThree, size) { + const geo = new THREE.ConeGeometry(size * 0.55, size, 4); + geo.translate(0, -size / 2, 0); + geo.rotateY(Math.PI / 4); + const mat = new THREE.MeshPhongMaterial({ + color: 0x9b7bff, transparent: true, opacity: 0.55, side: THREE.DoubleSide, + }); + const m = new THREE.Mesh(geo, mat); + m.position.copy(posThree); + const d = dirThree.clone().normalize(); + if (d.lengthSq() > 1e-9) m.quaternion.setFromUnitVectors(new THREE.Vector3(0, -1, 0), d); + return m; +} + +// ── Szene aus API-Daten aufbauen ────────────────────────────────────────────── function buildScene(data) { - clearGroup(gPaper); - clearGroup(gMarkers); - clearGroup(gCameras); + clearGroup(gPaper); clearGroup(gMarkers); clearGroup(gMeasured); clearGroup(gCameras); - const { robot, detections, cameraPoses } = data; + const { robot, detections, cameraPoses, measuredMarkers } = data; - // Alle erkannten Marker-IDs über alle Kameras sammeln + // Alle erkannten Marker-IDs (über alle Kameras) const detectedIds = new Set(); for (const det of (detections ?? [])) { for (const id of (det.detectedMarkerIds ?? [])) detectedIds.add(id); } const boardMarkers = robot?.links?.Board?.markers ?? []; - const markerSize = (robot?.vision_config?.MarkerSize ?? 0.025); // m + const markerSize = robot?.vision_config?.MarkerSize ?? 0.025; // m - // ── A0-Papier (weißes Rechteck unter den Markern) ── + // ── A0-Papier (weißes Rechteck) ── if (boardMarkers.length > 0) { let minRx = Infinity, maxRx = -Infinity; let minRy = Infinity, maxRy = -Infinity; let markerRz = -27.3; for (const m of boardMarkers) { const [rx, ry, rz] = m.position; - minRx = Math.min(minRx, rx); maxRx = Math.max(maxRx, rx); - minRy = Math.min(minRy, ry); maxRy = Math.max(maxRy, ry); + if (rx < minRx) minRx = rx; if (rx > maxRx) maxRx = rx; + if (ry < minRy) minRy = ry; if (ry > maxRy) maxRy = ry; markerRz = rz; } - const pad = 40; // mm Rand + const pad = 40; // mm Rand const planeW = (maxRx - minRx + 2 * pad) * S; const planeH = (maxRy - minRy + 2 * pad) * S; - const planeCx = ((minRx + maxRx) / 2) * S; - const planeCz = -((minRy + maxRy) / 2) * S; // robot y → Three.js -z - const planeCy = markerRz * S - 0.001; // leicht unter den Markern + const cx = ((minRx + maxRx) / 2) * S; + const cz = -((minRy + maxRy) / 2) * S; + const cy = markerRz * S - 0.001; - const paper = makePlane(planeW, planeH, 0xf0ebe0); // off-white - paper.position.set(planeCx, planeCy, planeCz); - gPaper.add(paper); + const geo = new THREE.PlaneGeometry(planeW, planeH); + geo.rotateX(-Math.PI / 2); + const mat = new THREE.MeshPhongMaterial({ color: 0xf0ebe0, side: THREE.DoubleSide }); + const plane = new THREE.Mesh(geo, mat); + plane.position.set(cx, cy, cz); + gPaper.add(plane); } - // ── ArUco-Marker ── - let nDetected = 0; + // ── Modell-Marker (Rechtecke) ── const visSize = markerSize * 0.9; + let nDetected = 0; for (const m of boardMarkers) { - const id = m.id; - const pos = r2vArr(m.position); - const detected = detectedIds.has(id); + const pos = r2vArr(m.position); + const detected = detectedIds.has(m.id); if (detected) nDetected++; + const color = detected ? 0x22c55e : 0xef4444; - const color = detected ? 0x22c55e : 0xef4444; - const sq = makeSquareMarker(pos, visSize, color); - sq.position.y += 0.0005; // ganz knapp über der Papier-Ebene + const sq = makeMarkerSquare(pos, visSize, color); + sq.position.y += 0.0005; gMarkers.add(sq); - // dünner Rahmen - const borderGeo = new THREE.EdgesGeometry(new THREE.PlaneGeometry(visSize, visSize)); - const borderMat = new THREE.LineBasicMaterial({ - color: detected ? 0x4ade80 : 0xfca5a5, - transparent: true, opacity: 0.8, - }); - const border = new THREE.LineSegments(borderGeo, borderMat); - border.rotation.x = -Math.PI / 2; - border.position.copy(pos); + const border = makeEdgeBorder(pos, visSize, detected ? 0x4ade80 : 0xfca5a5); border.position.y += 0.001; gMarkers.add(border); } - // ── Kamera-Frusta ── - const boardCenter = r2v(490, 0, -27.3); - for (const cp of (cameraPoses ?? [])) { - if (!cp.position_mm) continue; - const camPos = r2vArr(cp.position_mm); + // ── Gemessene Positionen von 3b (gelbe Punkte) ── + let nTriangulated = 0; + const measuredById = {}; - // Kamera-Blickrichtung: R_wc^T · [0,0,1] = dritte Zeile von R_wc - let dirWorld = [0, 0, 1]; - if (cp.rotation_matrix?.length >= 3) dirWorld = cp.rotation_matrix[2]; - const dirThree = r2dir(dirWorld); + if (measuredMarkers?.markers?.length > 0) { + // Nur A0-Marker (Board-Link) + const a0markers = measuredMarkers.markers.filter(m => + m.link === 'Board' || boardMarkers.some(bm => bm.id === m.marker_id) + ); - gCameras.add(makeCameraFrustum(camPos, dirThree, 0.07)); - gCameras.add(makeLine(camPos, boardCenter, 0x9b7bff, 0.35)); + for (const m of a0markers) { + nTriangulated++; + const mpos = r2vArr(m.position_mm); + mpos.y += 0.004; // leicht über der Papier-Ebene + measuredById[m.marker_id] = mpos; - // Kleiner Sphere an Kamera-Position - const sg = new THREE.SphereGeometry(0.012, 10, 8); - const sm = new THREE.MeshPhongMaterial({ color: 0x9b7bff }); - const sp = new THREE.Mesh(sg, sm); - sp.position.copy(camPos); - gCameras.add(sp); + // Gelber Punkt an gemessener Position + const dot = makeSphere(mpos, 0.0055, 0xfbbf24); + gMeasured.add(dot); + + // Verbindungslinie zum Modell-Mittelpunkt + const modelMarker = boardMarkers.find(bm => bm.id === m.marker_id); + if (modelMarker) { + const modelPos = r2vArr(modelMarker.position); + modelPos.y += 0.002; + gMeasured.add(makeLine(modelPos, mpos, 0x78716c, 0.5)); + } + } + + // Kamera-Frusta aus 3b (falls vorhanden, aktualisiert) + if (measuredMarkers.cameras?.length > 0) { + for (const c of measuredMarkers.cameras) { + const cpos = r2vArr(c.position_mm); + const cdir = r2dir(c.direction); + gCameras.add(makeCameraFrustum(cpos, cdir, 0.07)); + const boardCenter = r2v(490, 0, -27.3); + gCameras.add(makeLine(cpos, boardCenter, 0x9b7bff, 0.35)); + gCameras.add(makeSphere(cpos, 0.012, 0x9b7bff)); + } + } + } + + // Falls keine 3b-Daten: Kamera-Frusta aus camera_pose-Dateien + if (!measuredMarkers?.cameras?.length) { + const boardCenter = r2v(490, 0, -27.3); + for (const cp of (cameraPoses ?? [])) { + if (!cp.position_mm) continue; + const cpos = r2vArr(cp.position_mm); + let dirWorld = [0, 0, 1]; + if (cp.rotation_matrix?.length >= 3) dirWorld = cp.rotation_matrix[2]; + const cdir = r2dir(dirWorld); + gCameras.add(makeCameraFrustum(cpos, cdir, 0.07)); + gCameras.add(makeLine(cpos, boardCenter, 0x9b7bff, 0.35)); + gCameras.add(makeSphere(cpos, 0.012, 0x9b7bff)); + } } // ── Stats-Zeile ── const camInfo = (cameraPoses ?? []).map(cp => { const rms = cp.rms_px != null ? `${cp.rms_px.toFixed(1)} px` : '?'; - return `${cp.cameraId}: ${cp.usedMarkerIds?.length ?? 0} Marker, ${rms} RMS`; + return `${cp.cameraId}: ${rms}`; }).join(' │ '); + const triInfo = nTriangulated > 0 + ? ` │ 3b: ${nTriangulated} trianguliert` + : (measuredMarkers === null ? ' │ 3b: –' : ' │ 3b: <2 Kameras'); + document.getElementById('stats').textContent = - `Erkannt ${nDetected}/${boardMarkers.length}` + - (camInfo ? ` │ ${camInfo}` : ''); + `Erkannt ${nDetected}/${boardMarkers.length}${triInfo}` + + (camInfo ? ` │ RMS: ${camInfo}` : ''); } // ── Daten laden ─────────────────────────────────────────────────────────────── @@ -304,7 +329,7 @@ async function loadData() { if (!data.runDir) { statusEl.textContent = 'Kein Board-Run vorhanden.'; document.getElementById('stats').textContent = ''; - clearGroup(gPaper); clearGroup(gMarkers); clearGroup(gCameras); + clearGroup(gPaper); clearGroup(gMarkers); clearGroup(gMeasured); clearGroup(gCameras); return; } buildScene(data); @@ -316,8 +341,6 @@ async function loadData() { loadData(); document.getElementById('btnReload').addEventListener('click', loadData); - -// Reload-Trigger vom Parent-Frame (nach Board-Run) window.addEventListener('message', (e) => { if (e.data?.type === 'reload') loadData(); }); @@ -325,9 +348,8 @@ window.addEventListener('message', (e) => { // ── Resize & Render-Loop ────────────────────────────────────────────────────── function onResize() { const wrap = document.getElementById('canvas-wrap'); - const w = wrap.clientWidth, h = wrap.clientHeight; - renderer.setSize(w, h); - cam.aspect = w / h; + renderer.setSize(wrap.clientWidth, wrap.clientHeight); + cam.aspect = wrap.clientWidth / wrap.clientHeight; cam.updateProjectionMatrix(); } window.addEventListener('resize', onResize); diff --git a/scripts/3b_corner_marker_poses.py b/scripts/3b_corner_marker_poses.py index f02fd5e..213d115 100644 --- a/scripts/3b_corner_marker_poses.py +++ b/scripts/3b_corner_marker_poses.py @@ -41,11 +41,11 @@ def load_cameras(eval_dir: str) -> Dict[str, dict]: cams: Dict[str, dict] = {} for det_path in glob.glob(os.path.join(eval_dir, "*_aruco_detection.json")): base = os.path.basename(det_path) - m = re.match(r"render_([A-Za-z0-9]+)_aruco_detection\.json", base) + m = re.match(r"(.+)_aruco_detection\.json", base) if not m: continue cam_id = m.group(1) - pose_path = os.path.join(eval_dir, f"render_{cam_id}_camera_pose.json") + pose_path = os.path.join(eval_dir, f"{cam_id}_camera_pose.json") if not os.path.exists(pose_path): print(f"[WARN] no pose for camera {cam_id}, skipping") continue diff --git a/server/server.js b/server/server.js index 1f7396d..3a40535 100755 --- a/server/server.js +++ b/server/server.js @@ -411,6 +411,7 @@ const ROBOT_JSON = process.env.ROBOT_JSON || path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json'); const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py'); const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py'); +const SCRIPT_3B = path.join(__dirname, '..', 'scripts', '3b_corner_marker_poses.py'); /** * Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter. @@ -554,6 +555,24 @@ app.post('/api/board/run', async (req, res) => { send({ type: 'log', text: '' }); } + // ── Script 3b: Marker-Triangulierung (benötigt ≥2 Kamera-Posen) ── + send({ type: 'log', text: '' }); + send({ type: 'log', text: '─── 3b: Marker-Triangulierung ────────────────────────────' }); + const runFiles3b = await fsPromises.readdir(runDir); + const numPoses = runFiles3b.filter(f => f.endsWith('_camera_pose.json')).length; + if (numPoses >= 2) { + send({ type: 'log', text: `▷ 3b_corner_marker_poses (${numPoses} Kamera-Posen)` }); + const exit3b = await runScript([ + SCRIPT_3B, + '--evalDir', runDir, + '--robot', ROBOT_JSON, + ], send); + if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` }); + } else { + send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) vorhanden – Script 3b braucht ≥2 Kameras für Triangulierung, wird übersprungen.` }); + } + send({ type: 'log', text: '' }); + send({ type: 'log', text: `✅ Board-Run abgeschlossen: ${ts}` }); send({ type: 'done', exitCode: 0, runDir: ts }); if (!res.writableEnded) res.end(); @@ -633,7 +652,14 @@ app.get('/api/board/latest', async (req, res) => { } } - return res.json({ runDir: runName, robot, detections, cameraPoses }); + // aruco_marker_poses.json (Ausgabe von 3b_corner_marker_poses.py) + let measuredMarkers = null; + try { + const raw = await fsPromises.readFile(path.join(runDir, 'aruco_marker_poses.json'), 'utf8'); + measuredMarkers = JSON.parse(raw); + } catch {} + + return res.json({ runDir: runName, robot, detections, cameraPoses, measuredMarkers }); } catch (err) { return res.status(500).json({ error: String(err) }); }