152 lines
6.9 KiB
JavaScript
152 lines
6.9 KiB
JavaScript
/* fuse.js — fuseWorld: combine per-marker camera pose estimates into one world pose.
|
|
*
|
|
* Each detected marker yields a camera-in-world estimate:
|
|
* cameraWorld = anchorWorld * inverse(markerPoseInCamera)
|
|
* Estimates are fused by confidence-weighted quaternion slerp (incremental
|
|
* weighted average) and weighted position mean. Confidence = marker screen area
|
|
* (bigger/closer markers dominate). A light temporal smooth removes residual jitter.
|
|
*
|
|
* Requiring >= 2 visible markers is handled upstream by anchor placement density;
|
|
* fuseWorld itself works with 1..N.
|
|
*/
|
|
import * as THREE from 'three';
|
|
import { anchorWorldMatrix } from './pose.js';
|
|
|
|
const _inv = new THREE.Matrix4();
|
|
const _m = new THREE.Matrix4();
|
|
const _p = new THREE.Vector3();
|
|
const _q = new THREE.Quaternion();
|
|
const _s = new THREE.Vector3();
|
|
|
|
export class WorldFuser {
|
|
constructor() {
|
|
this.anchorMats = new Map(); // markerId -> Matrix4
|
|
this.smoothPos = null;
|
|
this.smoothQuat = null;
|
|
/* 1€ (OneEuro) filter — the standard cure for marker-pose jitter. It low-passes
|
|
* hard when the signal is slow (phone still => kills jitter) and eases off as
|
|
* speed rises (phone moving => stays responsive, no lag). Two knobs:
|
|
* minCutoff — lower = steadier at rest (more smoothing when still)
|
|
* beta — higher = snappier when moving (less lag during motion)
|
|
* dCutoff is the cutoff for the internal speed estimate; 1.0 is standard. */
|
|
this.oe = {
|
|
minCutoffPos: 0.6, betaPos: 0.05,
|
|
minCutoffAng: 0.7, betaAng: 0.06,
|
|
dCutoff: 1.0,
|
|
prevPos: null, dPos: new THREE.Vector3(),
|
|
prevQuat: null, dAngRate: 0,
|
|
};
|
|
this.lastFuseT = 0;
|
|
}
|
|
|
|
// low-pass alpha from a cutoff frequency (Hz) and timestep dt (s)
|
|
static alpha(cutoff, dt) {
|
|
const tau = 1 / (2 * Math.PI * cutoff);
|
|
return 1 / (1 + tau / dt);
|
|
}
|
|
|
|
setScene(scene) {
|
|
this.anchorMats.clear();
|
|
for (const a of scene.anchors || []) {
|
|
if (a.enabled === false) continue;
|
|
this.anchorMats.set(a.markerId, { mat: anchorWorldMatrix(a), sizeMM: a.sizeMM || 60 });
|
|
}
|
|
}
|
|
|
|
sizeFor(markerId) { return this.anchorMats.get(markerId)?.sizeMM || 60; }
|
|
knownIds() { return new Set(this.anchorMats.keys()); }
|
|
|
|
/** estimates: [{ markerId, position(mm), quaternion, area }] */
|
|
fuse(estimates) {
|
|
const MM2CM = 0.1; // POS-IT translation is in mm; world units are cm
|
|
const cams = [];
|
|
for (const e of estimates) {
|
|
const entry = this.anchorMats.get(e.markerId);
|
|
if (!entry) continue;
|
|
// marker pose in camera space -> matrix (translate mm->cm)
|
|
_m.compose(_p.copy(e.position).multiplyScalar(MM2CM), e.quaternion, _s.set(1, 1, 1));
|
|
_inv.copy(_m).invert(); // camera in marker space
|
|
const camWorld = new THREE.Matrix4().multiplyMatrices(entry.mat, _inv);
|
|
const pos = new THREE.Vector3();
|
|
const quat = new THREE.Quaternion();
|
|
camWorld.decompose(pos, quat, _s);
|
|
cams.push({ markerId: e.markerId, pos, quat, w: Math.max(1, e.area) });
|
|
}
|
|
if (!cams.length) return null;
|
|
|
|
/* Multi-marker fusion. Two visible markers each give an independent camera-in-world
|
|
* estimate; if they disagree (pose noise, or real-table spacing not matching the
|
|
* editor), naive area-weighting oscillates frame to frame because the areas jitter
|
|
* — this is the "steady on one marker, jittery on two" symptom. Fixes:
|
|
* 1. Deterministic order (sort by markerId) so the slerp base can't flip.
|
|
* 2. Robust weights: quantise area into coarse buckets so tiny per-frame area
|
|
* wobble doesn't shift the blend; a marker only dominates when genuinely much
|
|
* closer. Equal-ish markers then average to a STABLE midpoint, not a moving one. */
|
|
cams.sort((a, b) => a.markerId - b.markerId);
|
|
// Quantise confidence into coarse buckets from marker size (sqrt(area) ~ linear
|
|
// size). Near-equal markers land in the same bucket => equal weight => stable
|
|
// midpoint. A marker only outweighs another when it's a full bucket closer, so
|
|
// per-frame area jitter no longer shifts the blend.
|
|
for (const c of cams) c.w = Math.max(1, Math.round(Math.sqrt(c.w) / 8));
|
|
|
|
// weighted position mean + incremental weighted slerp (now order-stable)
|
|
let wSum = cams[0].w;
|
|
const pos = cams[0].pos.clone().multiplyScalar(cams[0].w);
|
|
const quat = cams[0].quat.clone();
|
|
for (let i = 1; i < cams.length; i++) {
|
|
const c = cams[i];
|
|
// hemisphere alignment before slerp (quaternion double-cover)
|
|
if (quat.dot(c.quat) < 0) c.quat.set(-c.quat.x, -c.quat.y, -c.quat.z, -c.quat.w);
|
|
const t = c.w / (wSum + c.w);
|
|
quat.slerp(c.quat, t);
|
|
pos.add(c.pos.clone().multiplyScalar(c.w));
|
|
wSum += c.w;
|
|
}
|
|
pos.multiplyScalar(1 / wSum);
|
|
|
|
// spread = how far apart the individual marker estimates are (0 for one marker).
|
|
// Large spread (>~2cm) means the markers disagree — usually real-table spacing
|
|
// not matching the editor, which no fusion can fully hide; surfaced in dev HUD.
|
|
let spread = 0;
|
|
for (let i = 0; i < cams.length; i++)
|
|
for (let j = i + 1; j < cams.length; j++)
|
|
spread = Math.max(spread, cams[i].pos.distanceTo(cams[j].pos));
|
|
|
|
// ---- 1€ filter (position + orientation) ----
|
|
const now = performance.now();
|
|
const gap = now - this.lastFuseT;
|
|
const dt = this.smoothPos ? Math.min(0.1, Math.max(0.001, gap / 1000)) : 1 / 30;
|
|
this.lastFuseT = now;
|
|
const oe = this.oe;
|
|
|
|
// reset on first frame or after a real tracking gap (>1.5s)
|
|
if (!this.smoothPos || gap > 1500) {
|
|
this.smoothPos = pos.clone(); oe.prevPos = pos.clone(); oe.dPos.set(0, 0, 0);
|
|
this.smoothQuat = quat.clone(); oe.prevQuat = quat.clone(); oe.dAngRate = 0;
|
|
return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length, spread };
|
|
}
|
|
|
|
// POSITION: derivative -> speed -> dynamic cutoff -> low-pass
|
|
const dPosRaw = pos.clone().sub(oe.prevPos).multiplyScalar(1 / dt); // cm/s
|
|
const aD = WorldFuser.alpha(oe.dCutoff, dt);
|
|
oe.dPos.lerp(dPosRaw, aD);
|
|
const speed = oe.dPos.length();
|
|
const cutoffP = oe.minCutoffPos + oe.betaPos * speed;
|
|
const aP = WorldFuser.alpha(cutoffP, dt);
|
|
this.smoothPos.lerp(pos, aP);
|
|
oe.prevPos.copy(pos);
|
|
|
|
// ORIENTATION: angular speed -> dynamic cutoff -> slerp
|
|
if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
|
|
if (oe.prevQuat.dot(quat) < 0) oe.prevQuat.set(-oe.prevQuat.x, -oe.prevQuat.y, -oe.prevQuat.z, -oe.prevQuat.w);
|
|
const angDelta = 2 * Math.acos(Math.min(1, Math.abs(oe.prevQuat.dot(quat)))) / dt; // rad/s
|
|
oe.dAngRate += aD * (angDelta - oe.dAngRate);
|
|
const cutoffA = oe.minCutoffAng + oe.betaAng * oe.dAngRate;
|
|
const aA = WorldFuser.alpha(cutoffA, dt);
|
|
this.smoothQuat.slerp(quat, aA);
|
|
oe.prevQuat.copy(quat);
|
|
|
|
return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length, spread };
|
|
}
|
|
}
|