101 lines
4.4 KiB
JavaScript
101 lines
4.4 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();
|
|
|
|
/* 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) {
|
|
// 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));
|
|
}
|