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

public/js/ar/detect.js

@@ -0,0 +1,22 @@
+/* detect.js — tuned ArUco 4x4 detection.
+ * Phantom-ID fix: reject any marker decoded with hamming distance > 0 (maxHamming: 0).
+ */
+export function createDetector() {
+  return new AR.Detector({ dictionaryName: 'ARUCO_4X4_1000', maxHammingDistance: 0 });
+}
+
+export function detectMarkers(detector, imageData, knownIds) {
+  const markers = detector.detect(imageData);
+  // keep only markers that exist in the scene, with sane geometry
+  return markers.filter(m => knownIds.has(m.id) && quadArea(m.corners) > 100);
+}
+
+export function quadArea(c) {
+  // shoelace
+  let a = 0;
+  for (let i = 0; i < 4; i++) {
+    const p = c[i], q = c[(i + 1) % 4];
+    a += p.x * q.y - q.x * p.y;
+  }
+  return Math.abs(a) / 2;
+}

public/js/ar/fuse.js

@@ -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 };
+  }
+}

public/js/ar/pose.js

@@ -0,0 +1,93 @@
+/* pose.js — marker pose estimation with the confirmed fixes:
+ *   1. POS-IT rotation used AS-IS; translation Y and Z negated (-t[1], -t[2]).
+ *   2. POS-IT planar ambiguity resolved by temporal consistency:
+ *      pick the solution (bestError vs alternativeError) closest to the previous frame.
+ *
+ * Requires vendor chain loaded in order: cv -> svd -> posit1 -> aruco -> dictionary.
+ */
+import * as THREE from 'three';
+
+const _m = new THREE.Matrix4();
+const _q = new THREE.Quaternion();
+
+export class PoseEstimator {
+  constructor(focalLength) {
+    this.focal = focalLength;
+    this.posits = new Map();   // sizeMM -> POS.Posit
+    this.prev = new Map();     // markerId -> { quat, pos, t }
+    this.prevTTL = 1500;       // ms before history is considered stale
+  }
+
+  positFor(sizeMM) {
+    if (!this.posits.has(sizeMM)) this.posits.set(sizeMM, new POS.Posit(sizeMM, this.focal));
+    return this.posits.get(sizeMM);
+  }
+
+  /** corners: aruco marker corners, image-space; cx/cy: image center.
+   *  Returns { position: THREE.Vector3 (mm, marker->camera), quaternion, error } */
+  estimate(markerId, corners, cx, cy, sizeMM) {
+    const centered = corners.map(c => ({ x: c.x - cx, y: (cy - c.y) }));
+    const pose = this.positFor(sizeMM).pose(centered);
+    if (!pose) return null;
+
+    const cand = [
+      this.candidate(pose.bestRotation, pose.bestTranslation, pose.bestError),
+      this.candidate(pose.alternativeRotation, pose.alternativeTranslation, pose.alternativeError),
+    ];
+
+    // Temporal consistency: prefer the solution nearest the previous frame's quat.
+    const prev = this.prev.get(markerId);
+    let pick;
+    if (prev && (performance.now() - prev.t) < this.prevTTL) {
+      const d0 = Math.abs(cand[0].quaternion.dot(prev.quat));
+      const d1 = Math.abs(cand[1].quaternion.dot(prev.quat));
+      // only override error-order if the alternative is clearly more consistent
+      pick = (d1 > d0 + 0.05) ? cand[1] : (d0 > d1 + 0.05 ? cand[0] : (cand[0].error <= cand[1].error ? cand[0] : cand[1]));
+    } else {
+      pick = cand[0].error <= cand[1].error ? cand[0] : cand[1];
+    }
+
+    this.prev.set(markerId, { quat: pick.quaternion.clone(), pos: pick.position.clone(), t: performance.now() });
+    return pick;
+  }
+
+  candidate(rot, t, error) {
+    // Rotation as-is (row-major 3x3 -> Matrix4)
+    _m.set(
+      rot[0][0], rot[0][1], rot[0][2], 0,
+      rot[1][0], rot[1][1], rot[1][2], 0,
+      rot[2][0], rot[2][1], rot[2][2], 0,
+      0, 0, 0, 1
+    );
+    const quaternion = new THREE.Quaternion().setFromRotationMatrix(_m);
+    // Translation: negate Y and Z only (confirmed fix)
+    const position = new THREE.Vector3(t[0], -t[1], -t[2]);
+    return { position, quaternion, error };
+  }
+}
+
+/** Build the marker->world transform for an anchor.
+ *  mount 'flat': marker printed face-up on a horizontal surface.
+ *  mount 'wall': marker on a vertical surface; yawDeg = facing direction.
+ *  mount 'custom': explicit yaw/pitch/roll (deg) applied in YXZ order.
+ *  All mounts additionally honour yaw/pitch/roll offsets for fine trim.
+ */
+export function anchorWorldMatrix(anchor) {
+  const pos = new THREE.Vector3(...anchor.position);
+  const yaw = THREE.MathUtils.degToRad(anchor.yawDeg || 0);
+  const pitch = THREE.MathUtils.degToRad(anchor.pitchDeg || 0);
+  const roll = THREE.MathUtils.degToRad(anchor.rollDeg || 0);
+
+  // Base orientation by mount:
+  //  flat: marker face-up — marker +Z (out of print face) -> world +Y
+  //  wall: marker vertical — marker +Z faces world +Z when yawDeg = 0
+  const base = new THREE.Quaternion();
+  if (anchor.mount !== 'wall' && anchor.mount !== 'custom') {
+    base.setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2);
+  }
+
+  // Trim (fully editable): yaw about world Y, then pitch/roll fine adjustment
+  const trim = new THREE.Quaternion().setFromEuler(new THREE.Euler(pitch, yaw, roll, 'YXZ'));
+  const q = trim.multiply(base);
+  return new THREE.Matrix4().compose(pos, q, new THREE.Vector3(1, 1, 1));
+}