AR: sub-pixel corner refinement, ROI-first detection, anchor coplanarity check

public/js/ar/subpix.js

@@ -0,0 +1,266 @@
+/* subpix.js — sub-pixel refinement of ArUco quad corners.
+ *
+ * WHY THIS EXISTS AND NOT cv.cornerSubPix: the pinned build
+ * (@techstark/opencv-js@4.12.0-release.1) does not export cornerSubPix — probed
+ * directly against the module, not assumed. `solvePnPRefineLM` IS exported, but
+ * that refines a pose from corners; it cannot improve the corners themselves.
+ *
+ * WHY IT MATTERS: js-aruco2 hands back contour corners quantised to roughly
+ * whole pixels of the detection canvas. Every downstream metric inherits that
+ * quantisation — pose jitter, reprojection error, and the gate thresholds tuned
+ * against them. At 960px long-side a 60mm crest spans ~140px, so one pixel of
+ * corner error is ~0.7% of the marker, which at typical table distance is a few
+ * millimetres of ghost placement, flickering frame to frame.
+ *
+ * METHOD — edge fitting, not corner finding. A corner operator (which is what
+ * cornerSubPix is) looks at a small window around one point and is easily pulled
+ * by the marker's own inner modules. Instead we use the whole length of each
+ * border edge:
+ *
+ *   1. walk along each edge of the quad, skipping the ends
+ *   2. at each step, scan perpendicular to the edge and find the sub-pixel peak
+ *      of the intensity gradient (parabolic fit on the three samples around the
+ *      maximum) — that is the true border crossing at that point
+ *   3. fit a line through those crossings by total least squares, with one
+ *      outlier rejection pass
+ *   4. intersect consecutive edge lines to get the refined corners
+ *
+ * Averaging ~12 independent crossings per edge beats any single-window estimate,
+ * and the corner comes from two long lines rather than from pixels near the
+ * corner itself — which is exactly where a printed marker is least reliable
+ * (ink bleed, rounding, and the contour tracer's own staircase).
+ *
+ * SAFETY: refinement is all-or-nothing per marker. If any edge is too short, too
+ * low-contrast, too few inliers, near-parallel to its neighbour, or if any
+ * corner would move further than maxShiftPx, the ORIGINAL corners are returned
+ * untouched. A bad refinement is worse than none, because the pose gates in
+ * cv-worker.js are tuned against honest corner noise.
+ *
+ * COST: samples are taken straight from the RGBA ImageData with bilinear luma,
+ * so there is no grayscale pre-pass over the frame. Per marker: 4 edges x ~12
+ * samples x ~17 taps ~= 800 taps. Five markers is ~4k taps against the 518k
+ * pixels the detector has already touched — under 1% of detection cost.
+ */
+
+const EPS = 1e-9;
+
+export const DEFAULT_OPTS = {
+  edgeInset: 0.18,       // skip this fraction at each end of an edge (corners lie)
+  samplePitchPx: 9,      // one crossing per ~9px of edge
+  minSamples: 5,
+  maxSamples: 22,
+  scanCellFrac: 0.42,    // scan +/- this fraction of ONE marker cell, so the
+                         // scan can never reach the next black/white transition
+  minScanPx: 1.5,
+  maxScanPx: 7,
+  scanStepPx: 0.5,
+  minContrast: 24,       // luma range across a scan; below this it's flat/noise
+  recentreIters: 2,      // re-centre the scan window on the estimate, then redo
+  maxOffFrac: 0.75,      // crossing further than this fraction of R => not our edge
+  sigmaMaxFrac: 0.55,    // gradient spread, as a fraction of R ...
+  sigmaMaxPx: 2.4,       // ... and an absolute ceiling, so heavy defocus still passes
+  minPoints: 4,          // inlier crossings needed to trust an edge
+  outlierPx: 1.4,
+  minEdgePx: 24,         // below this a marker is too small to refine usefully
+  maxShiftPx: 3.0,       // refinement that moves a corner further than this is
+                         // not refinement, it's a different quad
+};
+
+/* Bilinear luma sample from RGBA ImageData. NaN outside the image. */
+function lumaAt(d, w, h, x, y) {
+  if (!(x >= 0 && y >= 0 && x <= w - 1 && y <= h - 1)) return NaN;
+  const x0 = x | 0, y0 = y | 0;
+  const x1 = x0 + 1 < w ? x0 + 1 : x0;
+  const y1 = y0 + 1 < h ? y0 + 1 : y0;
+  const fx = x - x0, fy = y - y0;
+  const i00 = (y0 * w + x0) << 2, i10 = (y0 * w + x1) << 2;
+  const i01 = (y1 * w + x0) << 2, i11 = (y1 * w + x1) << 2;
+  const g00 = 0.299 * d[i00] + 0.587 * d[i00 + 1] + 0.114 * d[i00 + 2];
+  const g10 = 0.299 * d[i10] + 0.587 * d[i10 + 1] + 0.114 * d[i10 + 2];
+  const g01 = 0.299 * d[i01] + 0.587 * d[i01 + 1] + 0.114 * d[i01 + 2];
+  const g11 = 0.299 * d[i11] + 0.587 * d[i11 + 1] + 0.114 * d[i11 + 2];
+  const a = g00 + (g10 - g00) * fx;
+  const b = g01 + (g11 - g01) * fx;
+  return a + (b - a) * fy;
+}
+
+/* One pass: sample a scan line centred on (sx,sy), return the gradient
+ * centroid, its spread, the contrast and the net polarity. */
+function scanPass(d, w, h, sx, sy, nx, ny, R, step) {
+  const n = Math.floor((2 * R) / step) + 1;
+  if (n < 7) return null;
+
+  const vals = new Float64Array(n);
+  let lo = Infinity, hi = -Infinity;
+  for (let i = 0; i < n; i++) {
+    const o = -R + i * step;
+    const v = lumaAt(d, w, h, sx + nx * o, sy + ny * o);
+    if (Number.isNaN(v)) return null;            // scan left the frame
+    vals[i] = v;
+    if (v < lo) lo = v;
+    if (v > hi) hi = v;
+  }
+
+  /* CENTROID, NOT PEAK. Parabolic interpolation of the gradient peak — the
+   * obvious choice, and what cornerSubPix-style methods reduce to — is biased by
+   * the sampling phase, because the gradient of an anti-aliased step is roughly
+   * triangular and a parabola is a poor fit to a triangle. Measured against
+   * synthetic edges of known position it carried up to 0.29px of bias sharp and
+   * 0.75px defocused, which is worse than the whole-pixel corners we started
+   * with. The first moment of |gradient| has no such bias: for ANY symmetric
+   * edge profile the centroid sits exactly on the edge. Same synthetic test:
+   * 0.018px RMS, sharp or defocused. See tools/test-subpix.mjs. */
+  let sum = 0, wsum = 0, signed = 0;
+  const g = new Float64Array(n);
+  for (let i = 1; i < n - 1; i++) {
+    const dv = (vals[i + 1] - vals[i - 1]) * 0.5;
+    signed += dv;
+    g[i] = dv < 0 ? -dv : dv;
+    sum += g[i] * (-R + i * step);
+    wsum += g[i];
+  }
+  if (wsum < EPS) return null;
+  const off = sum / wsum;
+
+  // second moment: how spread out the gradient is. A single clean transition is
+  // compact; two transitions, or a textured surface, are not.
+  let m2 = 0;
+  for (let i = 1; i < n - 1; i++) { const x = -R + i * step - off; m2 += g[i] * x * x; }
+
+  return { off, sigma: Math.sqrt(m2 / wsum), contrast: hi - lo, sign: signed >= 0 ? 1 : -1 };
+}
+
+/* Locate the border crossing near a sample point, re-centring so the window
+ * ends up symmetric about the edge (which is what makes the centroid unbiased
+ * when the starting guess is off, and is worth ~3x on small markers). */
+function scanEdge(d, w, h, sx, sy, nx, ny, R, o) {
+  let acc = 0, last = null;
+  for (let k = 0; k < o.recentreIters; k++) {
+    const r = scanPass(d, w, h, sx + nx * acc, sy + ny * acc, nx, ny, R, o.scanStepPx);
+    if (!r) return null;
+    if (r.contrast < o.minContrast) return null;       // flat: nothing to find
+    if (Math.abs(r.off) > o.maxOffFrac * R) return null; // edge isn't in this window
+    acc += r.off;
+    last = r;
+  }
+  // diffuse gradient => more than one edge in the window, or surface texture
+  if (last.sigma > Math.min(o.sigmaMaxFrac * R, o.sigmaMaxPx)) return null;
+  return { off: acc, grad: last.sign };
+}
+
+/* Total least squares line fit. Returns { x, y, ux, uy } — a point and a unit
+ * direction. TLS rather than y = mx + b because a near-vertical edge is perfectly
+ * ordinary here and would blow up the slope form. */
+function fitLine(pts) {
+  const n = pts.length;
+  let mx = 0, my = 0;
+  for (let i = 0; i < n; i++) { mx += pts[i].x; my += pts[i].y; }
+  mx /= n; my /= n;
+
+  let sxx = 0, sxy = 0, syy = 0;
+  for (let i = 0; i < n; i++) {
+    const dx = pts[i].x - mx, dy = pts[i].y - my;
+    sxx += dx * dx; sxy += dx * dy; syy += dy * dy;
+  }
+  if (sxx + syy < EPS) return null;
+  const th = 0.5 * Math.atan2(2 * sxy, sxx - syy);   // major axis of the scatter
+  return { x: mx, y: my, ux: Math.cos(th), uy: Math.sin(th) };
+}
+
+function refineEdge(d, w, h, p0, p1, o) {
+  const ex = p1.x - p0.x, ey = p1.y - p0.y;
+  const len = Math.hypot(ex, ey);
+  if (len < o.minEdgePx) return null;
+
+  const ux = ex / len, uy = ey / len;
+  const nx = -uy, ny = ux;
+  const cell = len / 6;                        // 4x4 payload + 1 border cell each side
+  const R = Math.min(o.maxScanPx, Math.max(o.minScanPx, cell * o.scanCellFrac));
+  const nS = Math.max(o.minSamples, Math.min(o.maxSamples, Math.round(len / o.samplePitchPx)));
+
+  const pts = [];
+  let pos = 0;
+  for (let i = 0; i < nS; i++) {
+    const f = nS === 1 ? 0.5 : i / (nS - 1);
+    const t = o.edgeInset + (1 - 2 * o.edgeInset) * f;
+    const sx = p0.x + ex * t, sy = p0.y + ey * t;
+    const r = scanEdge(d, w, h, sx, sy, nx, ny, R, o);
+    if (!r) continue;
+    const s = r.grad;
+    if (s > 0) pos++;
+    pts.push({ x: sx + nx * r.off, y: sy + ny * r.off, s });
+  }
+  if (pts.length < o.minPoints) return null;
+
+  /* Keep only crossings with the majority gradient polarity. A scan that found a
+   * white->black step where its neighbours found black->white has locked onto a
+   * different transition (an inner module, or the far side of a thin border);
+   * including it would tilt the fitted line. */
+  const want = pos * 2 >= pts.length ? 1 : -1;
+  let kept = pts.filter(p => p.s === want);
+  if (kept.length < o.minPoints) return null;
+
+  let line = fitLine(kept);
+  if (!line) return null;
+
+  // one outlier pass against the fitted line
+  const rx = -line.uy, ry = line.ux;
+  const inl = kept.filter(p => Math.abs((p.x - line.x) * rx + (p.y - line.y) * ry) <= o.outlierPx);
+  if (inl.length >= o.minPoints && inl.length < kept.length) {
+    const l2 = fitLine(inl);
+    if (l2) { line = l2; kept = inl; }
+  }
+  line.n = kept.length;
+  return line;
+}
+
+function intersect(a, b) {
+  const cr = a.ux * b.uy - a.uy * b.ux;
+  if (Math.abs(cr) < 0.05) return null;        // within ~3 deg of parallel
+  const dx = b.x - a.x, dy = b.y - a.y;
+  const t = (dx * b.uy - dy * b.ux) / cr;
+  return { x: a.x + a.ux * t, y: a.y + a.uy * t };
+}
+
+/* Refine one quad. Returns a new 4-corner array, or null to keep the original.
+ * Corner order is preserved: corner i is the meeting of edge i-1 and edge i. */
+export function refineQuad(imageData, corners, opts = DEFAULT_OPTS) {
+  if (!corners || corners.length !== 4) return null;
+  const d = imageData.data, w = imageData.width, h = imageData.height;
+
+  const lines = new Array(4);
+  for (let i = 0; i < 4; i++) {
+    lines[i] = refineEdge(d, w, h, corners[i], corners[(i + 1) % 4], opts);
+    if (!lines[i]) return null;
+  }
+
+  const out = new Array(4);
+  let worst = 0;
+  for (let i = 0; i < 4; i++) {
+    const p = intersect(lines[(i + 3) % 4], lines[i]);
+    if (!p) return null;
+    const shift = Math.hypot(p.x - corners[i].x, p.y - corners[i].y);
+    if (shift > opts.maxShiftPx) return null;  // this is a different quad, not a better one
+    if (shift > worst) worst = shift;
+    out[i] = p;
+  }
+  out.shift = worst;
+  return out;
+}
+
+/* Refine every marker in place. Returns diagnostics for the HUD: how many
+ * markers were refined and the mean corner correction applied, which is the
+ * number that tells you whether this is doing anything at all. */
+export function refineMarkers(imageData, markers, opts = DEFAULT_OPTS) {
+  let refined = 0, shiftSum = 0;
+  for (const m of markers) {
+    const r = refineQuad(imageData, m.corners, opts);
+    if (!r) continue;
+    m.corners = [{ x: r[0].x, y: r[0].y }, { x: r[1].x, y: r[1].y },
+                 { x: r[2].x, y: r[2].y }, { x: r[3].x, y: r[3].y }];
+    m.subpix = true;
+    refined++;
+    shiftSum += r.shift;
+  }
+  return { refined, total: markers.length, meanShiftPx: refined ? shiftSum / refined : 0 };
+}