252 lines
11 KiB
JavaScript
252 lines
11 KiB
JavaScript
/* 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;
|
|
}
|
|
}
|