update Tue 07/14/2026 11:38:16.92

2026-07-14 11:38:17 +10:00
commit b2b555ef16
38 changed files with 7348 additions and 0 deletions
@@ -0,0 +1,88 @@
+/* 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 MM = 0.001;
+    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->m)
+      _m.compose(_p.copy(e.position).multiplyScalar(MM), 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 };
+  }
+}