perf: solve-cv becomes a worker client — non-blocking solves, Mat reuse, filter stays on main thread
This commit is contained in:
+93
-169
@@ -1,94 +1,27 @@
|
||||
/* solve-cv.js — v3 tracking core: joint multi-marker "board" solve via OpenCV.js.
|
||||
/* 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 call recovers the camera pose directly:
|
||||
* convention as v2), and ONE solvePnP 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)
|
||||
*
|
||||
* 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)
|
||||
*
|
||||
* 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).
|
||||
* 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';
|
||||
|
||||
// ---------------- 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', onProgress = null) {
|
||||
if (isCvReady()) return Promise.resolve(_cv);
|
||||
if (_loading) return _loading;
|
||||
_loading = (async () => {
|
||||
// 1) download with progress (13MB — visitors on exhibit wifi deserve a %)
|
||||
const resp = await fetch(url);
|
||||
if (!resp.ok) throw new Error(`opencv.js HTTP ${resp.status}`);
|
||||
const total = +resp.headers.get('content-length') || 0;
|
||||
let blob;
|
||||
if (resp.body && resp.body.getReader) {
|
||||
const reader = resp.body.getReader();
|
||||
const chunks = [];
|
||||
let got = 0;
|
||||
for (;;) {
|
||||
const { done, value } = await reader.read();
|
||||
if (done) break;
|
||||
chunks.push(value); got += value.length;
|
||||
if (onProgress) onProgress(total ? Math.round(got / total * 100) : Math.round(got / 1e6) + 'MB');
|
||||
}
|
||||
blob = new Blob(chunks, { type: 'text/javascript' });
|
||||
} else {
|
||||
blob = await resp.blob();
|
||||
}
|
||||
// 2) evaluate
|
||||
if (onProgress) onProgress('init');
|
||||
const src = URL.createObjectURL(blob);
|
||||
try {
|
||||
await new Promise((res, rej) => {
|
||||
const s = document.createElement('script');
|
||||
s.src = src;
|
||||
s.onload = res;
|
||||
s.onerror = () => rej(new Error('opencv.js eval failed'));
|
||||
document.head.appendChild(s);
|
||||
});
|
||||
} finally {
|
||||
setTimeout(() => URL.revokeObjectURL(src), 5000);
|
||||
}
|
||||
// 3) resolve the runtime — window.cv may be the module, a thenable that
|
||||
// resolves to it, or get swapped mid-init depending on the emscripten
|
||||
// build. Poll fresh every tick; hard 45s ceiling so a hang becomes a
|
||||
// visible failover instead of an eternal "loading".
|
||||
const t0 = performance.now();
|
||||
let thenAttached = false;
|
||||
for (;;) {
|
||||
const mod = window.cv;
|
||||
if (mod && mod.Mat) { _cv = mod; return mod; }
|
||||
if (mod && typeof mod.then === 'function' && !thenAttached) {
|
||||
thenAttached = true;
|
||||
mod.then((m) => { if (m && m.Mat) { window.cv = m; } }, () => {});
|
||||
}
|
||||
if (mod && !mod.Mat && typeof mod.then !== 'function' && !mod.__nbxHook) {
|
||||
mod.__nbxHook = true;
|
||||
const prev = mod.onRuntimeInitialized;
|
||||
mod.onRuntimeInitialized = () => { if (typeof prev === 'function') prev(); };
|
||||
}
|
||||
if (performance.now() - t0 > 45000) throw new Error('opencv.js init timeout');
|
||||
await new Promise(r => setTimeout(r, 100));
|
||||
}
|
||||
})();
|
||||
_loading.catch(() => { _loading = null; }); // allow retry after failure
|
||||
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.
|
||||
@@ -132,13 +65,11 @@ class OneEuroPose {
|
||||
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;
|
||||
@@ -150,22 +81,76 @@ class OneEuroPose {
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------- the solver ----------------
|
||||
// ---------------- 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.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.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._v = new THREE.Vector3();
|
||||
this._m4 = new THREE.Matrix4();
|
||||
this._q = new THREE.Quaternion();
|
||||
}
|
||||
|
||||
setAxisMode(m) { if (CV_AXIS_MODES.includes(m)) { this.axisMode = m; this.prev = null; this.filter.reset(); } }
|
||||
/** 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;
|
||||
if (!m.ok) return;
|
||||
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) {
|
||||
@@ -174,7 +159,9 @@ export class CvBoardSolver {
|
||||
if (a.enabled === false) continue;
|
||||
this.anchors.set(a.markerId, { mat: anchorWorldMatrix(a), sizeMM: a.sizeMM || 60 });
|
||||
}
|
||||
this.prev = null; this.filter.reset();
|
||||
this.filter.reset();
|
||||
this.lastPose = null;
|
||||
if (this.worker) this.worker.postMessage({ type: 'reset' });
|
||||
}
|
||||
knownIds() { return new Set(this.anchors.keys()); }
|
||||
|
||||
@@ -194,92 +181,29 @@ export class CvBoardSolver {
|
||||
return flat;
|
||||
}
|
||||
|
||||
/** markers: [{ id, corners:[{x,y}*4] }] in detection-canvas pixels.
|
||||
* Returns { position(cm world), quaternion(three), markerCount, reprojPx } or null. */
|
||||
/** 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) {
|
||||
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);
|
||||
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++;
|
||||
}
|
||||
|
||||
// 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]);
|
||||
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
|
||||
}
|
||||
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;
|
||||
return this.lastPose;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user