feat: bivariate polynomial warp model (degree-3 default for barrel)

src/geometry/polywarp.test.ts

@@ -0,0 +1,64 @@
+import { describe, it, expect } from 'vitest';
+import { estimatePolyWarp, applyPolyWarp, applyPolyWarpInverse } from './polywarp';
+import { estimateHomography, applyHomography } from './homography';
+import type { Vec2 } from '../types';
+
+const dist = (a: Vec2, b: Vec2) => Math.hypot(a[0] - b[0], a[1] - b[1]);
+
+// 16-point "box + #" calibration layout on a 2440x1220 bed (thirds in each axis).
+const XS = [0, 813.3, 1626.7, 2440];
+const YS = [0, 406.7, 813.3, 1220];
+const machineGrid: Vec2[] = XS.flatMap((x) => YS.map((y): Vec2 => [x, y]));
+
+// "True" camera map: affine into ~[0.1,0.9] then barrel distortion about the image centre.
+function trueMap(p: Vec2): Vec2 {
+  const u0 = 0.1 + 0.8 * (p[0] / 2440);
+  const v0 = 0.1 + 0.8 * (p[1] / 1220);
+  const cx = 0.5, cy = 0.5, k = 0.25;
+  const dx = u0 - cx, dy = v0 - cy, r2 = dx * dx + dy * dy;
+  const f = 1 + k * r2;
+  return [cx + dx * f, cy + dy * f];
+}
+
+describe('polywarp', () => {
+  it('recovers an exact quadratic map at unseen points', () => {
+    // dst is an exact degree-2 polynomial of src → fit must reproduce it.
+    const quad = (p: Vec2): Vec2 => {
+      const x = p[0] / 2440, y = p[1] / 1220;
+      return [0.2 + 0.5 * x + 0.1 * y + 0.3 * x * x, 0.1 + 0.6 * y + 0.2 * x * y];
+    };
+    const image = machineGrid.map(quad);
+    const w = estimatePolyWarp(machineGrid, image, 2);
+    for (const m of [[400, 200], [1200, 900], [2000, 300]] as Vec2[]) {
+      expect(dist(applyPolyWarp(w, m), quad(m))).toBeLessThan(1e-6);
+    }
+  });
+
+  it('beats a homography on barrel-distorted data', () => {
+    // Barrel distortion multiplies the coordinate by (1 + k·r²), so as a function of
+    // machine coords it is degree 3 (dx·r² → dx³ + dx·dy²). A homography (and a degree-2
+    // polynomial) cannot represent that; degree 3 fits it essentially exactly.
+    const image = machineGrid.map(trueMap);
+    const w = estimatePolyWarp(machineGrid, image, 3);
+    const H = estimateHomography(machineGrid, image);
+    // Evaluate on off-grid interior points (fifths), where barrel error is real.
+    const test: Vec2[] = [[488, 244], [1464, 732], [1952, 488], [976, 976]];
+    const polyMax = Math.max(...test.map((m) => dist(applyPolyWarp(w, m), trueMap(m))));
+    const homoMax = Math.max(...test.map((m) => dist(applyHomography(H, m), trueMap(m))));
+    expect(polyMax).toBeLessThan(homoMax * 0.05);
+  });
+
+  it('inverse round-trips machine points within a small tolerance', () => {
+    const image = machineGrid.map(trueMap);
+    const w = estimatePolyWarp(machineGrid, image, 3);
+    for (const m of [[400, 200], [1200, 900], [2000, 300]] as Vec2[]) {
+      expect(dist(applyPolyWarpInverse(w, applyPolyWarp(w, m)), m)).toBeLessThan(2); // mm
+    }
+  });
+
+  it('throws when given fewer points than the degree needs', () => {
+    const five = machineGrid.slice(0, 5);
+    const img = five.map(trueMap);
+    expect(() => estimatePolyWarp(five, img, 2)).toThrow(); // degree 2 needs 6
+  });
+});

src/geometry/polywarp.ts

@@ -0,0 +1,82 @@
+import type { Vec2, PolyMap, PolyWarp } from '../types';
+import { leastSquares } from './linalg';
+
+/** Number of monomial terms of total degree ≤ d in two variables. */
+function termCount(degree: number): number {
+  return ((degree + 1) * (degree + 2)) / 2;
+}
+
+/** Monomials x^i·y^j with i+j ≤ degree, in a fixed order. */
+function monomials(degree: number, x: number, y: number): number[] {
+  const terms: number[] = [];
+  for (let i = 0; i <= degree; i++) {
+    for (let j = 0; j <= degree - i; j++) terms.push(x ** i * y ** j);
+  }
+  return terms;
+}
+
+/** Center+scale so normalized inputs sit roughly in [-1,1] (conditions the fit). */
+function computeNorm(pts: Vec2[]): { off: Vec2; scl: Vec2 } {
+  const xs = pts.map((p) => p[0]);
+  const ys = pts.map((p) => p[1]);
+  const off: Vec2 = [xs.reduce((a, b) => a + b, 0) / xs.length, ys.reduce((a, b) => a + b, 0) / ys.length];
+  const sclx = Math.max(...xs.map((x) => Math.abs(x - off[0]))) || 1;
+  const scly = Math.max(...ys.map((y) => Math.abs(y - off[1]))) || 1;
+  return { off, scl: [sclx, scly] };
+}
+
+function applyNorm(norm: { off: Vec2; scl: Vec2 }, p: Vec2): Vec2 {
+  return [(p[0] - norm.off[0]) / norm.scl[0], (p[1] - norm.off[1]) / norm.scl[1]];
+}
+
+/** Fit one polynomial map src → dst of the given degree. */
+function fitPolyMap(src: Vec2[], dst: Vec2[], degree: number): PolyMap {
+  if (src.length !== dst.length) throw new Error('fitPolyMap: src/dst length mismatch');
+  const need = termCount(degree);
+  if (src.length < need) throw new Error(`Need at least ${need} points for degree ${degree}`);
+  const norm = computeNorm(src);
+  const rows = src.map((p) => {
+    const [x, y] = applyNorm(norm, p);
+    return monomials(degree, x, y);
+  });
+  const cu = leastSquares(rows, dst.map((d) => d[0]));
+  const cv = leastSquares(rows, dst.map((d) => d[1]));
+  return { degree, norm, cu, cv };
+}
+
+/** Evaluate a fitted polynomial map at a point. */
+export function applyPolyMap(m: PolyMap, p: Vec2): Vec2 {
+  const [x, y] = applyNorm(m.norm, p);
+  const mon = monomials(m.degree, x, y);
+  let u = 0;
+  let v = 0;
+  for (let i = 0; i < mon.length; i++) {
+    u += m.cu[i]! * mon[i]!;
+    v += m.cv[i]! * mon[i]!;
+  }
+  return [u, v];
+}
+
+/**
+ * Fit a bidirectional warp from matching machine↔image point pairs.
+ * Defaults to degree 3: barrel distortion is a cubic effect on position
+ * (the radial term r² multiplies the coordinate), so degree 3 is what actually
+ * compensates a wide-angle lens. Degree 2 is a fallback for sparse calibration.
+ */
+export function estimatePolyWarp(machine: Vec2[], image: Vec2[], degree = 3): PolyWarp {
+  return {
+    degree,
+    fwd: fitPolyMap(machine, image, degree),
+    inv: fitPolyMap(image, machine, degree),
+  };
+}
+
+/** machine-mm → normalized image. */
+export function applyPolyWarp(w: PolyWarp, machine: Vec2): Vec2 {
+  return applyPolyMap(w.fwd, machine);
+}
+
+/** normalized image → machine-mm. */
+export function applyPolyWarpInverse(w: PolyWarp, image: Vec2): Vec2 {
+  return applyPolyMap(w.inv, image);
+}

src/types.ts

@@ -3,6 +3,24 @@ export type Vec2 = [number, number];
 /** Row-major 3×3 matrix. */
 export type Mat3 = [number, number, number, number, number, number, number, number, number];
 
+/** One fitted polynomial map: dst = Σ cᵢ · monomialᵢ(normalized src). */
+export interface PolyMap {
+  degree: number;
+  /** Input normalization applied before evaluating monomials. */
+  norm: { off: Vec2; scl: Vec2 };
+  /** Coefficients for the u/x output channel. */
+  cu: number[];
+  /** Coefficients for the v/y output channel. */
+  cv: number[];
+}
+
+/** A bidirectional polynomial warp: forward machine→image, inverse image→machine. */
+export interface PolyWarp {
+  degree: number;
+  fwd: PolyMap;
+  inv: PolyMap;
+}
+
 export type MoveKind = 'cut' | 'rapid';
 
 /** A polyline in millimetres (machine coordinates before per-job alignment). */