267 lines
12 KiB
JavaScript
267 lines
12 KiB
JavaScript
/* 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 };
|
|
}
|