ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Board Kamera-Position

Browse Source
This commit is contained in:
chk
2026-06-10 14:58:14 +02:00
parent 1032c53630
commit 5285ed468b
3 changed files with 163 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

226
public/boardViewer.html
View File

@@ -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,21 +51,13 @@
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;
}
</style>
<script type="importmap">
{
"imports": {
@@ -82,6 +73,7 @@
<div class="legend">
<span><span class="dot" style="background:#22c55e"></span>Erkannt</span>
<span><span class="dot" style="background:#ef4444"></span>Nicht erkannt</span>
<span><span class="dot circle" style="background:#fbbf24"></span>Gemessen (3b)</span>
<span><span class="dot" style="background:#9b7bff"></span>Kamera</span>
</div>
<span id="stats"></span>
@@ -103,7 +95,6 @@ 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 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(); }
// ── Renderer ──────────────────────────────────────────────────────────────────
@@ -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 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 detected = detectedIds.has(m.id);
if (detected) nDetected++;
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 ──
// ── Gemessene Positionen von 3b (gelbe Punkte) ──
let nTriangulated = 0;
const measuredById = {};
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)
);
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;
// 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 camPos = r2vArr(cp.position_mm);
// Kamera-Blickrichtung: R_wc^T · [0,0,1] = dritte Zeile von R_wc
const cpos = r2vArr(cp.position_mm);
let dirWorld = [0, 0, 1];
if (cp.rotation_matrix?.length >= 3) dirWorld = cp.rotation_matrix[2];
const dirThree = r2dir(dirWorld);
gCameras.add(makeCameraFrustum(camPos, dirThree, 0.07));
gCameras.add(makeLine(camPos, boardCenter, 0x9b7bff, 0.35));
// 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);
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);

4
scripts/3b_corner_marker_poses.py
View File

@@ -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

28
server/server.js
View File

@@ -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) });
}