From c7b48105a6c14ff0306dbff0b56828f435b97dc2 Mon Sep 17 00:00:00 2001 From: sjat Date: Thu, 11 Jun 2026 09:16:02 +0200 Subject: [PATCH] feat: bivariate polynomial warp model (degree-3 default for barrel) --- src/geometry/polywarp.test.ts | 64 +++++++++++++++++++++++++++ src/geometry/polywarp.ts | 82 +++++++++++++++++++++++++++++++++++ src/types.ts | 18 ++++++++ 3 files changed, 164 insertions(+) create mode 100644 src/geometry/polywarp.test.ts create mode 100644 src/geometry/polywarp.ts diff --git a/src/geometry/polywarp.test.ts b/src/geometry/polywarp.test.ts new file mode 100644 index 0000000..7f88b22 --- /dev/null +++ b/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 + }); +}); diff --git a/src/geometry/polywarp.ts b/src/geometry/polywarp.ts new file mode 100644 index 0000000..305a702 --- /dev/null +++ b/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); +} diff --git a/src/types.ts b/src/types.ts index 666b06a..52abd87 100644 --- a/src/types.ts +++ b/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). */