diff --git a/public/js/ar/pose.js b/public/js/ar/pose.js index 12a2fff..b986dd4 100644 --- a/public/js/ar/pose.js +++ b/public/js/ar/pose.js @@ -10,6 +10,32 @@ 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). A rotation + * must be conjugated by the SAME F to stay in one consistent frame: R' = F R F^T. + * Applying F to translation but leaving rotation as-is (mode 'asis') puts position + * and orientation in different frames — position tracks but orientation tumbles + * (phone pitch leaks into world yaw/roll). Modes are runtime-switchable for on-device + * A/B: the debug overlay cycles ROT_MODES and calls setRotMode(). */ +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 = ['frf', 'asis', 'fr', 'rf']; +let rotMode = 'frf'; // F R F^T — the frame-consistent default +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 @@ -59,14 +85,15 @@ export class PoseEstimator { } candidate(rot, t, error) { - // Rotation as-is (row-major 3x3 -> Matrix4) + // 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 ); - const quaternion = new THREE.Quaternion().setFromRotationMatrix(_m); + // 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 };