128 lines
5.8 KiB
JavaScript
128 lines
5.8 KiB
JavaScript
/* 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));
|
|
}
|