diff --git a/public/js/ar/solve-cv.js b/public/js/ar/solve-cv.js new file mode 100644 index 0000000..7b79ee3 --- /dev/null +++ b/public/js/ar/solve-cv.js @@ -0,0 +1,251 @@ +/* solve-cv.js — v3 tracking core: joint multi-marker "board" solve via OpenCV.js. + * + * 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 call recovers the camera pose directly: + * - stability scales with marker spread instead of degrading into fusion tuning + * - the wall crest makes the point set non-coplanar => planar ambiguity gone + * - single visible marker still works (IPPE init + iterative refine w/ prior) + * + * 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) + * + * OpenCV.js (~13MB WASM) is lazy-loaded after Start; the POS-IT engine remains + * available as a runtime A/B fallback (HUD top-third tap cycles engines). + */ +import * as THREE from 'three'; +import { anchorWorldMatrix } from './pose.js'; + +// ---------------- OpenCV.js loader ---------------- +let _cv = null; +let _loading = null; + +export function isCvReady() { return !!(_cv && _cv.Mat); } +export function getCv() { return _cv; } +export function _injectCv(m) { _cv = m; } // test hook (node) + +export function loadOpenCV(url = '/vendor/opencv.js') { + if (isCvReady()) return Promise.resolve(_cv); + if (_loading) return _loading; + _loading = new Promise((resolve, reject) => { + const s = document.createElement('script'); + s.src = url; + s.async = true; + s.onerror = () => reject(new Error('opencv.js failed to load')); + s.onload = () => { + const mod = window.cv; + const finish = (m) => { _cv = m; resolve(m); }; + if (!mod) return reject(new Error('opencv.js loaded but window.cv missing')); + if (mod.Mat) return finish(mod); // already initialized + if (typeof mod.then === 'function') return mod.then(finish, reject); // promise build + mod.onRuntimeInitialized = () => finish(mod); // classic emscripten + // safety: poll in case onRuntimeInitialized was already consumed + const t0 = performance.now(); + (function poll() { + if (_cv) return; + if (mod.Mat) return finish(mod); + if (performance.now() - t0 > 30000) return reject(new Error('opencv.js init timeout')); + setTimeout(poll, 100); + })(); + }; + document.head.appendChild(s); + }); + return _loading; +} + +// ---------------- 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); + // position + 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); + // orientation + 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 ---------------- +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.prev = null; // { r:[3], t:[3], at:ms } + this.prevTTL = 1500; + this.filter = new OneEuroPose(); + this.K = null; this.dist = null; this.Kw = 0; this.Kf = 0; + this.maxReprojPx = 8; // reject frames worse than this (bad detect) + this.lastReproj = 0; + this._v = new THREE.Vector3(); + } + + setAxisMode(m) { if (CV_AXIS_MODES.includes(m)) { this.axisMode = m; this.prev = null; this.filter.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.prev = null; this.filter.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; + } + + /** markers: [{ id, corners:[{x,y}*4] }] in detection-canvas pixels. + * Returns { position(cm world), quaternion(three), markerCount, reprojPx } or null. */ + solve(markers, width, height, focalPx) { + const cv = _cv; + if (!cv || !cv.Mat) return null; + + // intrinsics (cy depends on height; rebuild K when geometry changes) + if (!this.K || this.Kw !== width || this.Kf !== focalPx || this._h !== height) { + if (this.K) { this.K.delete(); this.dist.delete(); } + this.K = cv.matFromArray(3, 3, cv.CV_64F, [focalPx, 0, width / 2, 0, focalPx, height / 2, 0, 0, 1]); + this.dist = cv.Mat.zeros(4, 1, cv.CV_64F); + this.Kw = width; this.Kf = focalPx; this._h = height; + } + + 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) return null; + const n = obj.length / 3; + + const objM = cv.matFromArray(n, 3, cv.CV_64F, obj); + const imgM = cv.matFromArray(n, 2, cv.CV_64F, img); + const rvec = new cv.Mat(3, 1, cv.CV_64F); + const tvec = new cv.Mat(3, 1, cv.CV_64F); + const R = new cv.Mat(); + const proj = new cv.Mat(); + const jac = new cv.Mat(); + let out = null; + + try { + const fresh = this.prev && (performance.now() - this.prev.at) < this.prevTTL; + if (fresh) { + // warm start: iterative LM from last frame — fast, and implicitly + // resolves single-marker planar flips by temporal continuity + rvec.data64F.set(this.prev.r); tvec.data64F.set(this.prev.t); + cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, true, cv.SOLVEPNP_ITERATIVE); + } else { + // cold start: SQPNP (any geometry) -> fall back to IPPE (planar) -> refine + let ok = false; + try { ok = cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, false, cv.SOLVEPNP_SQPNP); } catch { ok = false; } + if (!ok) { + try { ok = cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, false, cv.SOLVEPNP_IPPE); } catch { ok = false; } + } + if (!ok) return null; + cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, true, cv.SOLVEPNP_ITERATIVE); + } + + // reprojection error gate (mean px) — rejects poisoned frames before the filter + cv.projectPoints(objM, rvec, tvec, this.K, this.dist, proj, jac); + let err = 0; + for (let i = 0; i < n; i++) { + err += Math.hypot(proj.data64F[2 * i] - img[2 * i], proj.data64F[2 * i + 1] - img[2 * i + 1]); + } + err /= n; + this.lastReproj = err; + if (err > this.maxReprojPx) { this.prev = null; return null; } + + this.prev = { r: [...rvec.data64F], t: [...tvec.data64F], at: performance.now() }; + + // ---- convert to three.js world pose ---- + cv.Rodrigues(rvec, R); + const d = R.data64F; // row-major world->cvCam + const t = tvec.data64F; + // C = -R^T t + const Cx = -(d[0] * t[0] + d[3] * t[1] + d[6] * t[2]); + const Cy = -(d[1] * t[0] + d[4] * t[1] + d[7] * t[2]); + const Cz = -(d[2] * t[0] + d[5] * t[1] + d[8] * t[2]); + // world-from-threeCam = R^T * diag(1,-1,-1): columns [R^T_col0, -R^T_col1, -R^T_col2] + const m4 = new THREE.Matrix4().set( + d[0], -d[3], -d[6], 0, + d[1], -d[4], -d[7], 0, + d[2], -d[5], -d[8], 0, + 0, 0, 0, 1 + ); + const quat = new THREE.Quaternion().setFromRotationMatrix(m4); + const sm = this.filter.apply(new THREE.Vector3(Cx, Cy, Cz), quat, performance.now()); + out = { position: sm.position, quaternion: sm.quaternion, markerCount: used, reprojPx: err }; + } finally { + objM.delete(); imgM.delete(); rvec.delete(); tvec.delete(); R.delete(); proj.delete(); jac.delete(); + } + return out; + } +}