/* solve-cv.js — v3 tracking core: joint multi-marker "board" solve. * * Replaces the per-marker POS-IT -> fuse pipeline. Every detected marker corner * becomes a 3D world point (from scene.json anchors, same anchorWorldMatrix * convention as v2), and ONE solvePnP recovers the camera pose directly: * - stability scales with marker spread instead of degrading into fusion tuning * - a non-coplanar crest (one standing vertical) removes planar ambiguity * - single visible marker still works (IPPE init + iterative refine w/ prior) * * The solve itself runs in cv-worker.js. OpenCV.js embeds ~8MB of WASM as a * base64 data URI in a 10.8MB script; decoding and compiling that on the main * thread froze the page for seconds ("page unresponsive"). This module is the * client: it keeps the anchor geometry and the 1€ filter here (both cheap) and * ships only corner arrays to the worker. Results arrive a frame or so later, * which the filter absorbs. * * Frame conversion (validated against synthetic projections, error ~0): * solvePnP gives world->cvCamera (R, t). cv cam: +X right, +Y down, +Z fwd. * camera position in world: C = -R^T t * three.js world-from-camera rotation: R^T * diag(1,-1,-1) [done in worker] */ import * as THREE from 'three'; import { anchorWorldMatrix } from './pose.js'; // ---------------- marker corner templates ---------------- /* js-aruco2 corner order (canonical, matches POS-IT buildModel): TL, TR, BR, BL. * Marker-local frame: +X right, +Y up (toward printed top edge), +Z out of face. * 'ymirror' axis mode flips local Y — on-device insurance in case the effective * marker frame v2 calibrated against differs by a Y mirror (cycle via HUD). */ export const CV_AXIS_MODES = ['std', 'ymirror']; function markerLocalCorners(sizeMM, mode) { const h = sizeMM / 20; // mm -> cm, half size const y = mode === 'ymirror' ? -1 : 1; return [ new THREE.Vector3(-h, h * y, 0), new THREE.Vector3( h, h * y, 0), new THREE.Vector3( h, -h * y, 0), new THREE.Vector3(-h, -h * y, 0), ]; } // ---------------- 1€ filter (same maths as v2 fuse.js, standalone) ---------------- class OneEuroPose { /* Raw board-solve noise is far below POS-IT's, so cutoffs sit higher (less * smoothing => less lag) than v2's fuse.js tuning. Retune on-device if needed. */ constructor({ minCutoffPos = 1.2, betaPos = 0.10, minCutoffAng = 1.4, betaAng = 0.12, dCutoff = 1.0 } = {}) { Object.assign(this, { minCutoffPos, betaPos, minCutoffAng, betaAng, dCutoff }); this.reset(); } reset() { this.pos = null; this.quat = null; this.prevPos = null; this.dPos = new THREE.Vector3(); this.prevQuat = null; this.dAngRate = 0; this.lastT = 0; } static alpha(cutoff, dt) { const tau = 1 / (2 * Math.PI * cutoff); return 1 / (1 + tau / dt); } apply(pos, quat, now) { const gap = now - this.lastT; const dt = this.pos ? Math.min(0.1, Math.max(0.001, gap / 1000)) : 1 / 30; this.lastT = now; if (!this.pos || gap > 1500) { this.pos = pos.clone(); this.prevPos = pos.clone(); this.dPos.set(0, 0, 0); this.quat = quat.clone(); this.prevQuat = quat.clone(); this.dAngRate = 0; return { position: this.pos.clone(), quaternion: this.quat.clone() }; } const aD = OneEuroPose.alpha(this.dCutoff, dt); const dRaw = pos.clone().sub(this.prevPos).multiplyScalar(1 / dt); this.dPos.lerp(dRaw, aD); const aP = OneEuroPose.alpha(this.minCutoffPos + this.betaPos * this.dPos.length(), dt); this.pos.lerp(pos, aP); this.prevPos.copy(pos); if (this.quat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w); if (this.prevQuat.dot(quat) < 0) this.prevQuat.set(-this.prevQuat.x, -this.prevQuat.y, -this.prevQuat.z, -this.prevQuat.w); const ang = 2 * Math.acos(Math.min(1, Math.abs(this.prevQuat.dot(quat)))) / dt; this.dAngRate += aD * (ang - this.dAngRate); const aA = OneEuroPose.alpha(this.minCutoffAng + this.betaAng * this.dAngRate, dt); this.quat.slerp(quat, aA); this.prevQuat.copy(quat); return { position: this.pos.clone(), quaternion: this.quat.clone() }; } } // ---------------- the solver (worker client) ---------------- export class CvBoardSolver { constructor() { this.anchors = new Map(); // markerId -> { mat: Matrix4, sizeMM } this.worldCorners = new Map(); // `${axisMode}:${markerId}` -> [x,y,z]*4 flat this.axisMode = 'std'; this.filter = new OneEuroPose(); this.worker = null; this.state = 'idle'; // idle | loading | ready | failed this.inFlight = false; // one solve at a time; drop frames instead of queueing this.reqId = 0; this.lastPose = null; // newest smoothed pose from the worker this.lastReproj = 0; this.lastCount = 0; this.lastReject = ''; // why the worker last refused a pose (HUD) this._v = new THREE.Vector3(); this._m4 = new THREE.Matrix4(); this._q = new THREE.Quaternion(); } /** Boot the worker. Resolves when OpenCV is initialised inside it. */ load() { if (this.state === 'ready') return Promise.resolve(); if (this._loading) return this._loading; this.state = 'loading'; this._loading = new Promise((resolve, reject) => { try { this.worker = new Worker('/js/ar/cv-worker.js'); } catch (e) { this.state = 'failed'; return reject(e); } this.worker.onerror = (e) => { this.state = 'failed'; reject(new Error(e.message || 'worker error')); }; this.worker.onmessage = (ev) => { const m = ev.data; if (m.type === 'ready') { this.state = 'ready'; resolve(); return; } if (m.type === 'failed') { this.state = 'failed'; reject(new Error(m.msg)); return; } if (m.type === 'pose') this._onPose(m); }; this.worker.postMessage({ type: 'init' }); }); this._loading.catch(() => { this._loading = null; }); // allow retry return this._loading; } isReady() { return this.state === 'ready'; } getState() { return this.state; } _onPose(m) { this.inFlight = false; // a rejected pose leaves the previous one in place: better a slightly stale // camera than a mirrored one that throws every ghost across the table if (!m.ok) { this.lastReject = m.why || 'fail'; return; } this.lastReject = ''; this.lastReproj = m.reproj || 0; this.lastCount = m.n / 4; this._m4.set( m.m3[0], m.m3[1], m.m3[2], 0, m.m3[3], m.m3[4], m.m3[5], 0, m.m3[6], m.m3[7], m.m3[8], 0, 0, 0, 0, 1); this._q.setFromRotationMatrix(this._m4); const sm = this.filter.apply(this._v.set(m.C[0], m.C[1], m.C[2]).clone(), this._q.clone(), performance.now()); this.lastPose = { position: sm.position, quaternion: sm.quaternion, markerCount: this.lastCount, reprojPx: this.lastReproj, }; } setAxisMode(mode) { if (!CV_AXIS_MODES.includes(mode)) return; this.axisMode = mode; this.filter.reset(); this.lastPose = null; if (this.worker) this.worker.postMessage({ type: 'reset' }); } getAxisMode() { return this.axisMode; } setScene(scene) { this.anchors.clear(); this.worldCorners.clear(); for (const a of scene.anchors || []) { if (a.enabled === false) continue; this.anchors.set(a.markerId, { mat: anchorWorldMatrix(a), sizeMM: a.sizeMM || 60 }); } this.filter.reset(); this.lastPose = null; if (this.worker) this.worker.postMessage({ type: 'reset' }); } knownIds() { return new Set(this.anchors.keys()); } cornersFor(markerId) { const key = `${this.axisMode}:${markerId}`; let flat = this.worldCorners.get(key); if (!flat) { const a = this.anchors.get(markerId); if (!a) return null; flat = []; for (const c of markerLocalCorners(a.sizeMM, this.axisMode)) { this._v.copy(c).applyMatrix4(a.mat); flat.push(this._v.x, this._v.y, this._v.z); } this.worldCorners.set(key, flat); } return flat; } /** Dispatch a solve for the current frame and return the newest pose available. * Non-blocking: if a solve is still running the frame is skipped, so the main * thread never waits on OpenCV. Returns the last smoothed pose, or null. */ solve(markers, width, height, focalPx) { if (this.state !== 'ready') return null; if (!this.inFlight && markers.length) { const obj = [], img = []; let used = 0; for (const m of markers) { const flat = this.cornersFor(m.id); if (!flat) continue; obj.push(...flat); for (const c of m.corners) img.push(c.x, c.y); used++; } if (used) { const objA = new Float64Array(obj), imgA = new Float64Array(img); this.inFlight = true; this.worker.postMessage( { type: 'solve', id: ++this.reqId, obj: objA, img: imgA, n: used * 4, W: width, H: height, f: focalPx }, [objA.buffer, imgA.buffer]); // transfer, no copy } } return this.lastPose; } }