106 lines
4.4 KiB
JavaScript
106 lines
4.4 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;
|
|
// adaptive smoothing bounds: minAlpha when nearly still (heavy smoothing,
|
|
// kills jitter), maxAlpha when moving fast (light smoothing, stays responsive)
|
|
this.minAlpha = 0.08;
|
|
this.maxAlpha = 0.6;
|
|
this.lastFuseT = 0;
|
|
}
|
|
|
|
// Map a motion magnitude between [lo, hi] thresholds to an alpha in
|
|
// [minAlpha, maxAlpha]. Below lo => minAlpha (still); above hi => maxAlpha (moving).
|
|
adaptAlpha(delta, lo, hi) {
|
|
const tt = Math.min(1, Math.max(0, (delta - lo) / (hi - lo)));
|
|
return this.minAlpha + (this.maxAlpha - this.minAlpha) * tt;
|
|
}
|
|
|
|
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({ pos, quat, w: Math.max(1, e.area) });
|
|
}
|
|
if (!cams.length) return null;
|
|
|
|
// weighted position mean + incremental weighted slerp for orientation
|
|
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);
|
|
|
|
// temporal smoothing — adaptive: smooth hard when nearly still (kills jitter),
|
|
// loosen when genuinely moving (stays responsive). Reset only after a real
|
|
// tracking gap so brief single-frame dropouts don't cause a visible snap.
|
|
const now = performance.now();
|
|
if (this.smoothPos && now - this.lastFuseT < 1500) {
|
|
// positional delta in cm; rotational delta in radians
|
|
const dPos = this.smoothPos.distanceTo(pos);
|
|
if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
|
|
const dAng = 2 * Math.acos(Math.min(1, Math.abs(this.smoothQuat.dot(quat))));
|
|
// map motion -> alpha in [minAlpha, maxAlpha]. Small motion => small alpha.
|
|
const pAlpha = this.adaptAlpha(dPos, 0.5, 6); // still<0.5cm .. moving>6cm
|
|
const qAlpha = this.adaptAlpha(dAng, 0.01, 0.15); // still<0.6° .. moving>8.6°
|
|
this.smoothPos.lerp(pos, pAlpha);
|
|
this.smoothQuat.slerp(quat, qAlpha);
|
|
} else {
|
|
this.smoothPos = pos.clone();
|
|
this.smoothQuat = quat.clone();
|
|
}
|
|
this.lastFuseT = now;
|
|
return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
|
|
}
|
|
}
|