214 lines
8.9 KiB
JavaScript
214 lines
8.9 KiB
JavaScript
/* 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;
|
|
}
|
|
}
|