108 lines
4.4 KiB
JavaScript
108 lines
4.4 KiB
JavaScript
/* geom.js — is this layout's crest geometry actually solvable without ambiguity?
|
|
*
|
|
* A coplanar set of markers admits two camera poses that reproject almost
|
|
* identically: the true one and a mirrored twin. cv-worker.js suppresses the
|
|
* twin with physics gates (camera above the table, no teleporting), but those
|
|
* are a safety net over a geometry problem. The cure is a crest that does not
|
|
* share the plane of the others — the A3 test rig does this with a 160mm tower
|
|
* carrying crests at 10cm and 16cm.
|
|
*
|
|
* The thing is, "the anchors are coplanar" is invisible. A layout saved from the
|
|
* editor, or a tower knocked over and re-entered as flat, looks completely
|
|
* normal in the JSON and only shows up later as ghosts intermittently flipping
|
|
* to the wrong side of the table — the most expensive bug in this project to
|
|
* date. So: measure it once when the scene loads, and put the number on screen.
|
|
*
|
|
* The measure is the RMS distance of every anchor CORNER from the best-fit plane
|
|
* through all of them. Corners rather than anchor centres, because a crest
|
|
* mounted vertically among flat ones contributes out-of-plane spread through its
|
|
* own extent even if its centre happens to sit near the common plane.
|
|
*/
|
|
|
|
/* Eigen-decomposition of a symmetric 3x3 by cyclic Jacobi. Small, exact enough,
|
|
* and no dependency — three.js has no symmetric eigensolver. */
|
|
function jacobiEigen(m) {
|
|
const a = [[m[0], m[1], m[2]], [m[1], m[3], m[4]], [m[2], m[4], m[5]]];
|
|
const v = [[1, 0, 0], [0, 1, 0], [0, 0, 1]];
|
|
for (let sweep = 0; sweep < 24; sweep++) {
|
|
let off = 0;
|
|
for (let p = 0; p < 2; p++) for (let q = p + 1; q < 3; q++) off += a[p][q] * a[p][q];
|
|
if (off < 1e-20) break;
|
|
for (let p = 0; p < 2; p++) {
|
|
for (let q = p + 1; q < 3; q++) {
|
|
if (Math.abs(a[p][q]) < 1e-18) continue;
|
|
const theta = (a[q][q] - a[p][p]) / (2 * a[p][q]);
|
|
const t = Math.sign(theta || 1) / (Math.abs(theta) + Math.sqrt(theta * theta + 1));
|
|
const c = 1 / Math.sqrt(t * t + 1), s = t * c;
|
|
for (let k = 0; k < 3; k++) {
|
|
const akp = a[k][p], akq = a[k][q];
|
|
a[k][p] = c * akp - s * akq;
|
|
a[k][q] = s * akp + c * akq;
|
|
}
|
|
for (let k = 0; k < 3; k++) {
|
|
const apk = a[p][k], aqk = a[q][k];
|
|
a[p][k] = c * apk - s * aqk;
|
|
a[q][k] = s * apk + c * aqk;
|
|
}
|
|
for (let k = 0; k < 3; k++) {
|
|
const vkp = v[k][p], vkq = v[k][q];
|
|
v[k][p] = c * vkp - s * vkq;
|
|
v[k][q] = s * vkp + c * vkq;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return { values: [a[0][0], a[1][1], a[2][2]], vectors: v };
|
|
}
|
|
|
|
/* points: flat [x,y,z, x,y,z, ...] in world cm.
|
|
* Returns null if there is nothing meaningful to measure. */
|
|
export function planarity(points) {
|
|
const n = points.length / 3;
|
|
if (n < 3) return null;
|
|
|
|
let cx = 0, cy = 0, cz = 0;
|
|
for (let i = 0; i < n; i++) { cx += points[3 * i]; cy += points[3 * i + 1]; cz += points[3 * i + 2]; }
|
|
cx /= n; cy /= n; cz /= n;
|
|
|
|
let xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
|
|
for (let i = 0; i < n; i++) {
|
|
const dx = points[3 * i] - cx, dy = points[3 * i + 1] - cy, dz = points[3 * i + 2] - cz;
|
|
xx += dx * dx; xy += dx * dy; xz += dx * dz;
|
|
yy += dy * dy; yz += dy * dz; zz += dz * dz;
|
|
}
|
|
const { values, vectors } = jacobiEigen([xx, xy, xz, yy, yz, zz]);
|
|
|
|
// smallest eigenvalue -> normal of the best-fit plane
|
|
let k = 0;
|
|
if (values[1] < values[k]) k = 1;
|
|
if (values[2] < values[k]) k = 2;
|
|
const nx = vectors[0][k], ny = vectors[1][k], nz = vectors[2][k];
|
|
|
|
let s2 = 0, mx = 0;
|
|
for (let i = 0; i < n; i++) {
|
|
const d = (points[3 * i] - cx) * nx + (points[3 * i + 1] - cy) * ny + (points[3 * i + 2] - cz) * nz;
|
|
s2 += d * d;
|
|
if (Math.abs(d) > mx) mx = Math.abs(d);
|
|
}
|
|
return {
|
|
points: n,
|
|
rmsCm: Math.sqrt(s2 / n),
|
|
maxCm: mx,
|
|
normal: [nx, ny, nz],
|
|
};
|
|
}
|
|
|
|
/* COPLANAR_CM: below this the layout is effectively flat and the mirrored pose
|
|
* is live. The A3 rig's tower gives ~5cm RMS; print tolerance and a crest taped
|
|
* slightly proud of the table give well under 0.5cm, so 1.5cm separates a real
|
|
* height difference from noise without being so strict that a modest raised
|
|
* crest fails to register. */
|
|
export const COPLANAR_CM = 1.5;
|
|
|
|
export function describePlanarity(p) {
|
|
if (!p) return 'geom —';
|
|
if (p.rmsCm < COPLANAR_CM) return `geom COPLANAR ${p.rmsCm.toFixed(1)}cm — raise a crest`;
|
|
return `geom spread ${p.rmsCm.toFixed(1)}cm (max ${p.maxCm.toFixed(1)})`;
|
|
}
|