Import v2 base (unchanged files)
This commit is contained in:
load-v3
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,151 @@
|
||||
/* 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 };
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
/* 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();
|
||||
|
||||
/* Rotation frame correction. The translation is converted from POS-IT's frame to
|
||||
* the Three.js frame by F = diag(1,-1,-1) (the -t[1]/-t[2] negation). The rotation
|
||||
* needs a matching left-multiply by F: R' = F R. Confirmed on-device — orientation
|
||||
* then survives phone rotation and viewing angle. (Leaving rotation as-is ('asis')
|
||||
* puts position and orientation in different frames so pitch leaks into yaw/roll;
|
||||
* the full conjugation F R F^T ('frf') over-corrects for this pipeline.) Modes stay
|
||||
* runtime-switchable for future re-tuning; the debug overlay cycles ROT_MODES. */
|
||||
const F = new THREE.Matrix4().set(1,0,0,0, 0,-1,0,0, 0,0,-1,0, 0,0,0,1);
|
||||
const Ft = F.clone().transpose();
|
||||
export const ROT_MODES = ['fr', 'frf', 'asis', 'rf'];
|
||||
let rotMode = 'fr'; // F R — confirmed correct on-device (orientation survives rotation/angle)
|
||||
export function setRotMode(m) { if (ROT_MODES.includes(m)) rotMode = m; }
|
||||
export function getRotMode() { return rotMode; }
|
||||
|
||||
function correctRotation(R) {
|
||||
// R is a Matrix4 holding the raw POS-IT rotation.
|
||||
switch (rotMode) {
|
||||
case 'asis': return R;
|
||||
case 'fr': return _m2.multiplyMatrices(F, R);
|
||||
case 'rf': return _m2.multiplyMatrices(R, F);
|
||||
case 'frf':
|
||||
default: return _m2.multiplyMatrices(F, R).multiply(Ft);
|
||||
}
|
||||
}
|
||||
const _m2 = new THREE.Matrix4();
|
||||
|
||||
/* Input convention (matches the v1-validated fix): raw centered image coords are
|
||||
* passed to POS-IT — NO Y pre-flip. The -t[1]/-t[2] negation below is what converts
|
||||
* to the Three.js camera frame. Pre-flipping Y here double-flips the vertical axis
|
||||
* and inverts pitch response (ghosts move opposite when tilting the phone).
|
||||
* If tilt ever reads inverted on a device, toggle this for a quick A/B test. */
|
||||
const FLIP_INPUT_Y = false;
|
||||
|
||||
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: FLIP_INPUT_Y ? (cy - c.y) : (c.y - cy) }));
|
||||
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) {
|
||||
// Raw POS-IT rotation (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
|
||||
);
|
||||
// Conjugate into the Three.js frame so rotation matches the flipped translation
|
||||
const quaternion = new THREE.Quaternion().setFromRotationMatrix(correctRotation(_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));
|
||||
}
|
||||
Reference in New Issue
Block a user