gps-plus-slam-app-framework 1.3.0 → 1.7.0

package/dist/qr-size-measurer-DLb33geH.js

@@ -0,0 +1,596 @@
+import { vec3 } from "gl-matrix";
+//#region ../src/ar/qr-size-from-depth.ts
+/**
+* QR size from depth — Note 4 of the QR-tracking follow-up plan
+* (docs `2026-06-15-followup-qr-tracking-generalization-overlay-and-north.md`).
+*
+* MEASURE a QR's printed physical size directly from the depth map, so the
+* QR content + printed size are irrelevant and `qr.physicalSizeM` need not be
+* hand-authored. The 4 corner pixels (and a few interior samples to catch edge
+* depth-bleed) are unprojected to 3D via the existing `createDepthUnprojector`
+* (depth + projection + camera pose → raw-WebXR points); the pairwise side
+* lengths of the unprojected square give a per-observation size estimate WITH
+* metric scale from depth — no `solvePnP` scale assumption needed.
+*
+* Two pieces:
+* - {@link estimateQrSizeFromDepth} — one observation → `{ sizeM, quality }`.
+*   `quality ∈ [0,1]` falls as the 4 sides disagree, the diagonals deviate from
+*   `√2·side`, or the corners/interior points are non-planar (depth noise / not
+*   a planar square facing us). A caller rejects low-quality reads.
+* - {@link createQrSizeAccumulator} — a robust running MEDIAN over accepted
+*   observations, reporting the Note 3 size lifecycle (`unknown → measuring →
+*   estimated`). The median is robust to depth noise; the lifecycle gate
+*   (min sample count + low spread) is what later promotes a measured size to
+*   drive size-dependent features.
+*
+* The size-estimate VALUE types ({@link QrSizeStatus}, {@link QrSizeEstimate})
+* live here (not in the `qrDetected` state slice) so the slice can import them
+* WITHOUT the `ar` layer ever importing `state` — that would close a cycle.
+*
+* @see depth-unprojection.ts — `createDepthUnprojector` (the unprojection it composes).
+* @see ../state/qr-detected-slice.ts — consumes `QrSizeEstimate` (the size lifecycle).
+*/
+const EPS$1 = 1e-9;
+/**
+* Absolute floor (meters) for the robust plane-fit inlier band, used when the
+* MAD-derived σ is ~0 (a near-perfect plane). Comfortably above float round-
+* trip noise yet far below a real depth outlier at QR scale. See WS-A.
+*/
+const PLANE_INLIER_FLOOR_M = .005;
+function dist(a, b) {
+	return Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
+}
+/** Median of a numeric list (mean of the middle two for even n). */
+function median(values) {
+	const sorted = [...values].sort((a, b) => a - b);
+	const n = sorted.length;
+	const mid = n >> 1;
+	return n % 2 === 1 ? sorted[mid] : (sorted[mid - 1] + sorted[mid]) / 2;
+}
+/**
+* Estimate a QR's physical side length from one detection's depth samples.
+*
+* @param corners - the 4 corner depth samples, ordered TL, TR, BR, BL (matching
+*   `buildObjectPoints` / the detector corner-order normalization).
+* @param interiorSamples - a few interior depth samples (may be empty); used
+*   only to strengthen the planarity check against corner edge depth-bleed.
+* @param unprojector - built once per depth sample via `createDepthUnprojector`.
+* @returns `{ sizeM, quality }`, or `null` when a corner cannot be unprojected
+*   or the quad is degenerate (collinear / zero-area).
+*/
+function estimateQrSizeFromDepth(corners, interiorSamples, unprojector) {
+	const world = [];
+	for (const c of corners) {
+		const p = unprojector.unproject(c);
+		if (!p) return null;
+		world.push(p);
+	}
+	const [c0, c1, c2, c3] = world;
+	const edges = [
+		dist(c0, c1),
+		dist(c1, c2),
+		dist(c2, c3),
+		dist(c3, c0)
+	];
+	const meanEdge = (edges[0] + edges[1] + edges[2] + edges[3]) / 4;
+	if (!(meanEdge > EPS$1)) return null;
+	const u = vec3.subtract(vec3.create(), [
+		c1[0],
+		c1[1],
+		c1[2]
+	], [
+		c0[0],
+		c0[1],
+		c0[2]
+	]);
+	const v = vec3.subtract(vec3.create(), [
+		c3[0],
+		c3[1],
+		c3[2]
+	], [
+		c0[0],
+		c0[1],
+		c0[2]
+	]);
+	const normal = vec3.cross(vec3.create(), u, v);
+	const normalLen = vec3.length(normal);
+	if (!(normalLen > EPS$1)) return null;
+	vec3.scale(normal, normal, 1 / normalLen);
+	const planeOffset = (p) => Math.abs(normal[0] * (p[0] - c0[0]) + normal[1] * (p[1] - c0[1]) + normal[2] * (p[2] - c0[2]));
+	let maxPlaneOffset = planeOffset(c2);
+	for (const s of interiorSamples) {
+		const p = unprojector.unproject(s);
+		if (!p) continue;
+		maxPlaneOffset = Math.max(maxPlaneOffset, planeOffset(p));
+	}
+	const edgeErr = Math.max(...edges.map((e) => Math.abs(e - meanEdge))) / meanEdge;
+	const expectedDiag = meanEdge * Math.SQRT2;
+	const diagErr = Math.max(Math.abs(dist(c0, c2) - expectedDiag), Math.abs(dist(c1, c3) - expectedDiag)) / expectedDiag;
+	const planeErr = maxPlaneOffset / meanEdge;
+	const quality = Math.max(0, Math.min(1, 1 - Math.max(edgeErr, diagErr, planeErr)));
+	return {
+		sizeM: median(edges),
+		quality
+	};
+}
+/**
+* Least-squares plane through a point set, refined from a seed normal. Avoids a
+* full eigensolver: the seed (from LMS) picks which axis is most aligned with
+* the normal, that axis becomes the dependent variable of a 2-parameter linear
+* fit through the centroid (well-conditioned precisely because the plane is not
+* near-vertical in that axis), and the 2×2 normal equations are solved in closed
+* form. Returns `null` for fewer than 3 points or a singular (collinear) fit.
+*/
+function fitPlaneLeastSquares(points, seedNormal) {
+	const n = points.length;
+	if (n < 3) return null;
+	let cx = 0;
+	let cy = 0;
+	let cz = 0;
+	for (const p of points) {
+		cx += p[0];
+		cy += p[1];
+		cz += p[2];
+	}
+	cx /= n;
+	cy /= n;
+	cz /= n;
+	const ax = Math.abs(seedNormal[0]);
+	const ay = Math.abs(seedNormal[1]);
+	const az = Math.abs(seedNormal[2]);
+	const dep = ax >= ay && ax >= az ? 0 : ay >= az ? 1 : 2;
+	const split = (p) => dep === 0 ? [
+		p[1] - cy,
+		p[2] - cz,
+		p[0] - cx
+	] : dep === 1 ? [
+		p[0] - cx,
+		p[2] - cz,
+		p[1] - cy
+	] : [
+		p[0] - cx,
+		p[1] - cy,
+		p[2] - cz
+	];
+	let Suu = 0;
+	let Suw = 0;
+	let Sww = 0;
+	let Sud = 0;
+	let Swd = 0;
+	for (const p of points) {
+		const [u, w, d] = split(p);
+		Suu += u * u;
+		Suw += u * w;
+		Sww += w * w;
+		Sud += u * d;
+		Swd += w * d;
+	}
+	const det = Suu * Sww - Suw * Suw;
+	if (Math.abs(det) < EPS$1) return null;
+	const a = (Sud * Sww - Swd * Suw) / det;
+	const b = (Swd * Suu - Sud * Suw) / det;
+	const nUWD = [
+		-a,
+		-b,
+		1
+	];
+	const nxyz = dep === 0 ? [
+		nUWD[2],
+		nUWD[0],
+		nUWD[1]
+	] : dep === 1 ? [
+		nUWD[0],
+		nUWD[2],
+		nUWD[1]
+	] : [
+		nUWD[0],
+		nUWD[1],
+		nUWD[2]
+	];
+	const len = Math.hypot(nxyz[0], nxyz[1], nxyz[2]);
+	if (!(len > EPS$1)) return null;
+	const sign = nxyz[0] * seedNormal[0] + nxyz[1] * seedNormal[1] + nxyz[2] * seedNormal[2] < 0 ? -1 : 1;
+	const normal = [
+		sign * nxyz[0] / len,
+		sign * nxyz[1] / len,
+		sign * nxyz[2] / len
+	];
+	return {
+		centroid: [
+			cx,
+			cy,
+			cz
+		],
+		normal
+	};
+}
+/**
+* Robustly fit a plane to 3D points. A plain least-squares (PCA) plane is
+* fragile here: one gross depth outlier (edge depth-bleed, a stray background
+* read) can dominate the off-plane variance and flip which axis PCA calls the
+* normal, so MAD rejection then runs against a wrong normal. Instead we use
+* Least-Median-of-Squares — pick the candidate plane (from a point triple) that
+* minimizes the MEDIAN squared orthogonal residual (≈50% breakdown, no
+* threshold) — derive a robust scale from that median, select inliers, then
+* least-squares refit on the inliers for an accurate normal/centroid.
+*
+* Deterministic (fixed-seed triple sampling) so property tests are
+* reproducible. Returns `null` for fewer than 3 points or a degenerate
+* (collinear / coincident) set where no non-degenerate triple exists.
+*/
+function fitPlaneRobust(points) {
+	const n = points.length;
+	if (n < 3) return null;
+	let best = null;
+	let rng = 625341585;
+	const nextIdx = () => {
+		rng = rng * 1103515245 + 12345 & 2147483647;
+		return rng % n;
+	};
+	const TRIALS = 200;
+	for (let t = 0; t < TRIALS; t++) {
+		const i = nextIdx();
+		const j = nextIdx();
+		const k = nextIdx();
+		if (i === j || j === k || i === k) continue;
+		const a = points[i];
+		const b = points[j];
+		const c = points[k];
+		const ux = b[0] - a[0];
+		const uy = b[1] - a[1];
+		const uz = b[2] - a[2];
+		const vx = c[0] - a[0];
+		const vy = c[1] - a[1];
+		const vz = c[2] - a[2];
+		let nx = uy * vz - uz * vy;
+		let ny = uz * vx - ux * vz;
+		let nz = ux * vy - uy * vx;
+		const len = Math.hypot(nx, ny, nz);
+		if (!(len > EPS$1)) continue;
+		nx /= len;
+		ny /= len;
+		nz /= len;
+		const medSq = median(points.map((p) => {
+			const d = nx * (p[0] - a[0]) + ny * (p[1] - a[1]) + nz * (p[2] - a[2]);
+			return d * d;
+		}));
+		if (!best || medSq < best.medSq) best = {
+			normal: [
+				nx,
+				ny,
+				nz
+			],
+			p0: a,
+			medSq
+		};
+	}
+	if (!best) return null;
+	const sigma = 1.4826 * (1 + 5 / Math.max(1, n - 3)) * Math.sqrt(best.medSq);
+	const band = Math.max(2.5 * sigma, PLANE_INLIER_FLOOR_M);
+	const { normal: bn, p0 } = best;
+	let inliers = points.filter((p) => Math.abs(bn[0] * (p[0] - p0[0]) + bn[1] * (p[1] - p0[1]) + bn[2] * (p[2] - p0[2])) <= band);
+	if (inliers.length < 3) inliers = [...points];
+	const refit = fitPlaneLeastSquares(inliers, bn);
+	if (!refit) return null;
+	const refitDist = (p) => refit.normal[0] * (p[0] - refit.centroid[0]) + refit.normal[1] * (p[1] - refit.centroid[1]) + refit.normal[2] * (p[2] - refit.centroid[2]);
+	const rms = Math.sqrt(inliers.reduce((acc, p) => acc + refitDist(p) ** 2, 0) / inliers.length);
+	return {
+		point: refit.centroid,
+		normal: refit.normal,
+		inlierCount: inliers.length,
+		rms
+	};
+}
+/**
+* Intersect the camera ray through a normalized screen point with a plane.
+* The ray is recovered by unprojecting the screen point at two distinct depths
+* (both lie on the corner's pixel ray); no explicit camera center is needed.
+* Returns the metric 3D intersection, or `null` for a parallel/degenerate ray.
+*/
+function intersectScreenRayWithPlane(screen, plane, unprojector) {
+	const a = unprojector.unproject({
+		...screen,
+		depthM: 1
+	});
+	const b = unprojector.unproject({
+		...screen,
+		depthM: 2
+	});
+	if (!a || !b) return null;
+	const dir = [
+		b[0] - a[0],
+		b[1] - a[1],
+		b[2] - a[2]
+	];
+	const { normal: n, point: pp } = plane;
+	const denom = n[0] * dir[0] + n[1] * dir[1] + n[2] * dir[2];
+	if (Math.abs(denom) < EPS$1) return null;
+	const t = (n[0] * (pp[0] - a[0]) + n[1] * (pp[1] - a[1]) + n[2] * (pp[2] - a[2])) / denom;
+	const hit = [
+		a[0] + t * dir[0],
+		a[1] + t * dir[1],
+		a[2] + t * dir[2]
+	];
+	return hit.every((v) => Number.isFinite(v)) ? hit : null;
+}
+/**
+* Estimate a QR's physical side length by fitting the QR plane to many interior
+* depth reads and intersecting the 4 corner pixel rays with that plane.
+*
+* @param cornerScreens - the 4 corner screen positions, ordered TL, TR, BR, BL.
+* @param samples - interior (and optionally corner) depth reads across the QR
+*   face used for the robust plane fit. Need ≥3 non-collinear usable reads.
+* @param unprojector - built once per depth sample via `createDepthUnprojector`.
+* @returns `{ sizeM, quality }`, or `null` when the plane is under-determined
+*   (too few/collinear reads) or a corner ray cannot meet the plane.
+*/
+function estimateQrSizeFromDepthDense(cornerScreens, samples, unprojector) {
+	const points = [];
+	for (const s of samples) {
+		const p = unprojector.unproject(s);
+		if (p) points.push(p);
+	}
+	const plane = fitPlaneRobust(points);
+	if (!plane) return null;
+	const corners = [];
+	for (const screen of cornerScreens) {
+		const hit = intersectScreenRayWithPlane(screen, plane, unprojector);
+		if (!hit) return null;
+		corners.push(hit);
+	}
+	const [c0, c1, c2, c3] = corners;
+	const edges = [
+		dist(c0, c1),
+		dist(c1, c2),
+		dist(c2, c3),
+		dist(c3, c0)
+	];
+	const meanEdge = (edges[0] + edges[1] + edges[2] + edges[3]) / 4;
+	if (!(meanEdge > EPS$1)) return null;
+	const edgeErr = Math.max(...edges.map((e) => Math.abs(e - meanEdge))) / meanEdge;
+	const expectedDiag = meanEdge * Math.SQRT2;
+	const diagErr = Math.max(Math.abs(dist(c0, c2) - expectedDiag), Math.abs(dist(c1, c3) - expectedDiag)) / expectedDiag;
+	const planeErr = plane.rms / meanEdge;
+	const quality = Math.max(0, Math.min(1, 1 - Math.max(edgeErr, diagErr, planeErr)));
+	return {
+		sizeM: median(edges),
+		quality
+	};
+}
+const UNKNOWN = {
+	status: "unknown",
+	estimateM: null,
+	sampleCount: 0,
+	spreadM: 0
+};
+function createQrSizeAccumulator(options = {}) {
+	const { qualityThreshold = .8, minSamples = 8, maxSpreadM = .01, maxSamples = Infinity } = options;
+	let sizes = [];
+	let everConverged = false;
+	function estimate() {
+		if (sizes.length === 0) return { ...UNKNOWN };
+		const med = median(sizes);
+		const spreadM = 1.4826 * median(sizes.map((s) => Math.abs(s - med))) / Math.sqrt(sizes.length);
+		if (sizes.length >= minSamples && spreadM <= maxSpreadM) everConverged = true;
+		return {
+			status: everConverged ? "estimated" : "measuring",
+			estimateM: med,
+			sampleCount: sizes.length,
+			spreadM
+		};
+	}
+	return {
+		add(observation) {
+			if (observation && Number.isFinite(observation.sizeM) && observation.sizeM > 0 && observation.quality >= qualityThreshold) {
+				sizes.push(observation.sizeM);
+				if (sizes.length > maxSamples) sizes = sizes.slice(-maxSamples);
+			}
+			return estimate();
+		},
+		current: estimate,
+		reset() {
+			sizes = [];
+			everConverged = false;
+		}
+	};
+}
+//#endregion
+//#region ../src/ar/qr-size-measurer.ts
+const EPS = 1e-9;
+/** Pixel corner → normalized screen point for the given frame. */
+function toScreen(corner, image) {
+	return {
+		x: corner.x / image.width,
+		y: corner.y / image.height
+	};
+}
+/**
+* Depth at a corner with inset fallback: try the corner pixel first, then points
+* progressively inset toward the centroid. The returned {@link DepthPoint} always
+* carries the TRUE corner's screen position — only the depth VALUE may be borrowed
+* from an inset sample (valid because the marker face is locally planar over the
+* few-percent inset) — so the unprojected square keeps its real size. `null` when
+* no inset offers a depth read either.
+*/
+function sampleCornerDepth(corner, centroid, image, depthAt, insetFractions) {
+	const s = toScreen(corner, image);
+	const exact = depthAt(s.x, s.y);
+	if (exact !== null) return {
+		screenX: s.x,
+		screenY: s.y,
+		depthM: exact
+	};
+	for (const f of insetFractions) {
+		const si = toScreen({
+			x: corner.x + f * (centroid.x - corner.x),
+			y: corner.y + f * (centroid.y - corner.y)
+		}, image);
+		const depthM = depthAt(si.x, si.y);
+		if (depthM !== null) return {
+			screenX: s.x,
+			screenY: s.y,
+			depthM
+		};
+	}
+	return null;
+}
+/**
+* Estimate a missing corner's depth by fitting a plane `depth = f(screenX,
+* screenY)` through three known corner samples and evaluating it at the missing
+* corner's screen position. `null` when the three are collinear in screen space
+* (degenerate plane).
+*/
+function reconstructDepth(known, at) {
+	if (known.length < 3) return null;
+	const [a, b, c] = known;
+	const abx = b.screenX - a.screenX;
+	const aby = b.screenY - a.screenY;
+	const abz = b.depthM - a.depthM;
+	const acx = c.screenX - a.screenX;
+	const acy = c.screenY - a.screenY;
+	const acz = c.depthM - a.depthM;
+	const nx = aby * acz - abz * acy;
+	const ny = abz * acx - abx * acz;
+	const nz = abx * acy - aby * acx;
+	if (Math.abs(nz) < EPS) return null;
+	const depthM = a.depthM - (nx * (at.x - a.screenX) + ny * (at.y - a.screenY)) / nz;
+	return Number.isFinite(depthM) ? depthM : null;
+}
+/**
+* Build the 4 corner depth samples with inset fallback and at-most-`maxRecon`
+* planar reconstruction. `null` when more corners than allowed lack a depth read.
+*/
+function cornerDepthPoints(corners, image, depthAt, insetFractions, maxRecon) {
+	if (corners.length !== 4) return null;
+	const centroid = {
+		x: corners.reduce((s, c) => s + c.x, 0) / corners.length,
+		y: corners.reduce((s, c) => s + c.y, 0) / corners.length
+	};
+	const sampled = corners.map((corner) => sampleCornerDepth(corner, centroid, image, depthAt, insetFractions));
+	const missing = sampled.filter((p) => p === null).length;
+	if (missing === 0) return sampled;
+	if (missing > maxRecon) return null;
+	const known = sampled.filter((p) => p !== null);
+	const out = [];
+	for (let i = 0; i < 4; i++) {
+		const s = sampled[i];
+		if (s) {
+			out.push(s);
+			continue;
+		}
+		const at = toScreen(corners[i], image);
+		const depthM = reconstructDepth(known, at);
+		if (depthM === null) return null;
+		out.push({
+			screenX: at.x,
+			screenY: at.y,
+			depthM
+		});
+	}
+	return out;
+}
+/**
+* Sample an `n×n` interior lattice across the detected quad (bilinear in pixel
+* space, strictly inside via `(i+0.5)/n` fractions so the high-contrast print
+* boundary is avoided) and return the reads that have depth. These feed the
+* dense plane fit; reads with no depth are simply skipped.
+*/
+function latticeDepthPoints(corners, image, depthAt, n) {
+	if (corners.length !== 4 || n < 1) return [];
+	const [tl, tr, br, bl] = corners;
+	const out = [];
+	for (let i = 0; i < n; i++) {
+		const u = (i + .5) / n;
+		const topX = tl.x + (tr.x - tl.x) * u;
+		const topY = tl.y + (tr.y - tl.y) * u;
+		const botX = bl.x + (br.x - bl.x) * u;
+		const botY = bl.y + (br.y - bl.y) * u;
+		for (let j = 0; j < n; j++) {
+			const v = (j + .5) / n;
+			const s = toScreen({
+				x: topX + (botX - topX) * v,
+				y: topY + (botY - topY) * v
+			}, image);
+			const depthM = depthAt(s.x, s.y);
+			if (depthM !== null) out.push({
+				screenX: s.x,
+				screenY: s.y,
+				depthM
+			});
+		}
+	}
+	return out;
+}
+/** The 4 corner screen positions (detector order) for the dense ray-plane fit. */
+function cornerScreens(corners, image) {
+	if (corners.length !== 4) return null;
+	return corners.map((c) => {
+		const s = toScreen(c, image);
+		return {
+			screenX: s.x,
+			screenY: s.y
+		};
+	});
+}
+/** The QR centroid as a single interior depth sample (may be empty). */
+function interiorDepthPoints(corners, image, depthAt) {
+	const s = toScreen({
+		x: corners.reduce((s, c) => s + c.x, 0) / corners.length,
+		y: corners.reduce((s, c) => s + c.y, 0) / corners.length
+	}, image);
+	const depthM = depthAt(s.x, s.y);
+	return depthM === null ? [] : [{
+		screenX: s.x,
+		screenY: s.y,
+		depthM
+	}];
+}
+/**
+* Create a size measurer. Accumulator `options` (quality threshold, min samples,
+* spread, cap) are forwarded to every per-marker {@link createQrSizeAccumulator};
+* the depth-at-corners knobs ({@link QrSizeMeasurerOptions.cornerInsetFractions},
+* {@link QrSizeMeasurerOptions.maxReconstructedCorners}) tune corner sampling.
+*/
+function createQrSizeMeasurer(options = {}) {
+	const insetFractions = options.cornerInsetFractions ?? [.12, .25];
+	const maxRecon = options.maxReconstructedCorners ?? 1;
+	const latticeSize = options.latticeSize ?? 7;
+	const accumulators = /* @__PURE__ */ new Map();
+	function accumulatorFor(text) {
+		let acc = accumulators.get(text);
+		if (!acc) {
+			acc = createQrSizeAccumulator(options);
+			accumulators.set(text, acc);
+		}
+		return acc;
+	}
+	return {
+		measure(text, corners, image, ctx) {
+			const screens = cornerScreens(corners, image);
+			if (!screens) return null;
+			const cornerSamples = cornerDepthPoints(corners, image, ctx.depthAt, insetFractions, maxRecon);
+			const lattice = latticeDepthPoints(corners, image, ctx.depthAt, latticeSize);
+			let observation = estimateQrSizeFromDepthDense(screens, lattice, ctx.unprojector);
+			let interiorSamples = lattice;
+			if (observation === null) {
+				if (!cornerSamples) return null;
+				interiorSamples = interiorDepthPoints(corners, image, ctx.depthAt);
+				observation = estimateQrSizeFromDepth(cornerSamples, interiorSamples, ctx.unprojector);
+			}
+			return {
+				estimate: accumulatorFor(text).add(observation),
+				cornerSamples,
+				interiorSamples
+			};
+		},
+		current(text) {
+			return accumulatorFor(text).current();
+		},
+		reset(text) {
+			if (text === void 0) accumulators.clear();
+			else accumulators.delete(text);
+		}
+	};
+}
+//#endregion
+export { createQrSizeAccumulator as n, estimateQrSizeFromDepth as r, createQrSizeMeasurer as t };