89 lines
3.4 KiB
JavaScript
89 lines
3.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;
|
|
this.posAlpha = 0.35; // smoothing factors (higher = snappier)
|
|
this.quatAlpha = 0.35;
|
|
this.lastFuseT = 0;
|
|
}
|
|
|
|
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
|
|
const now = performance.now();
|
|
if (this.smoothPos && now - this.lastFuseT < 500) {
|
|
this.smoothPos.lerp(pos, this.posAlpha);
|
|
if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
|
|
this.smoothQuat.slerp(quat, this.quatAlpha);
|
|
} else {
|
|
this.smoothPos = pos.clone();
|
|
this.smoothQuat = quat.clone();
|
|
}
|
|
this.lastFuseT = now;
|
|
return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
|
|
}
|
|
}
|