diff --git a/src/geometry/homography.test.ts b/src/geometry/homography.test.ts new file mode 100644 index 0000000..5b64693 --- /dev/null +++ b/src/geometry/homography.test.ts @@ -0,0 +1,31 @@ +import { describe, it, expect } from 'vitest'; +import { estimateHomography, applyHomography, invertMat3 } from './homography'; +import type { Vec2, Mat3 } from '../types'; + +const close = (a: Vec2, b: Vec2, eps = 1e-6) => + Math.abs(a[0] - b[0]) < eps && Math.abs(a[1] - b[1]) < eps; + +describe('homography', () => { + it('recovers the identity from coincident point sets', () => { + const pts: Vec2[] = [[0, 0], [1, 0], [1, 1], [0, 1]]; + const H = estimateHomography(pts, pts); + expect(close(applyHomography(H, [0.3, 0.7]), [0.3, 0.7])).toBe(true); + }); + + it('recovers a known perspective transform', () => { + const H: Mat3 = [1.1, 0.2, 5, -0.1, 0.9, 3, 0.001, 0.002, 1]; + const src: Vec2[] = [[0, 0], [100, 0], [100, 100], [0, 100], [40, 60]]; + const dst = src.map((p) => applyHomography(H, p)); + const est = estimateHomography(src, dst); + for (const p of [[10, 20], [70, 30], [55, 90]] as Vec2[]) { + expect(close(applyHomography(est, p), applyHomography(H, p), 1e-4)).toBe(true); + } + }); + + it('invertMat3 round-trips a point', () => { + const H: Mat3 = [1.1, 0.2, 5, -0.1, 0.9, 3, 0.001, 0.002, 1]; + const inv = invertMat3(H); + const p: Vec2 = [37, 91]; + expect(close(applyHomography(inv, applyHomography(H, p)), p, 1e-6)).toBe(true); + }); +}); diff --git a/src/geometry/homography.ts b/src/geometry/homography.ts new file mode 100644 index 0000000..c058857 --- /dev/null +++ b/src/geometry/homography.ts @@ -0,0 +1,78 @@ +import type { Vec2, Mat3 } from '../types'; + +/** Apply a homography to a point (projective divide). */ +export function applyHomography(H: Mat3, p: Vec2): Vec2 { + const [a, b, c, d, e, f, g, h, i] = H; + const w = g * p[0] + h * p[1] + i; + return [(a * p[0] + b * p[1] + c) / w, (d * p[0] + e * p[1] + f) / w]; +} + +/** Solve a square linear system A x = b by Gaussian elimination with partial pivoting. */ +function solveLinear(A: number[][], b: number[]): number[] { + const n = b.length; + const M = A.map((row, i) => [...row, b[i]!]); + for (let col = 0; col < n; col++) { + let pivot = col; + for (let r = col + 1; r < n; r++) { + if (Math.abs(M[r]![col]!) > Math.abs(M[pivot]![col]!)) pivot = r; + } + if (Math.abs(M[pivot]![col]!) < 1e-12) throw new Error('Singular system in homography fit'); + [M[col], M[pivot]] = [M[pivot]!, M[col]!]; + const pivRow = M[col]!; + for (let r = 0; r < n; r++) { + if (r === col) continue; + const factor = M[r]![col]! / pivRow[col]!; + for (let k = col; k <= n; k++) M[r]![k]! -= factor * pivRow[k]!; + } + } + return M.map((row, i) => row[n]! / row[i]!); +} + +/** + * Estimate a homography mapping src → dst from ≥4 point pairs. + * Solves for 8 DoF (h33 fixed to 1) via least squares (normal equations). + */ +export function estimateHomography(src: Vec2[], dst: Vec2[]): Mat3 { + if (src.length !== dst.length || src.length < 4) { + throw new Error('Need at least 4 matching point pairs'); + } + // Build 2N×8 design matrix M and target vector t. + const rows: number[][] = []; + const t: number[] = []; + for (let i = 0; i < src.length; i++) { + const [x, y] = src[i]!; + const [u, v] = dst[i]!; + rows.push([x, y, 1, 0, 0, 0, -x * u, -y * u]); + t.push(u); + rows.push([0, 0, 0, x, y, 1, -x * v, -y * v]); + t.push(v); + } + // Normal equations: (Mᵀ M) h = Mᵀ t → 8×8 solve. + const ata: number[][] = Array.from({ length: 8 }, () => new Array(8).fill(0)); + const atb: number[] = new Array(8).fill(0); + for (let r = 0; r < rows.length; r++) { + const row = rows[r]!; + for (let i = 0; i < 8; i++) { + atb[i]! += row[i]! * t[r]!; + for (let j = 0; j < 8; j++) ata[i]![j]! += row[i]! * row[j]!; + } + } + const h = solveLinear(ata, atb); + return [h[0]!, h[1]!, h[2]!, h[3]!, h[4]!, h[5]!, h[6]!, h[7]!, 1]; +} + +/** Invert a 3×3 matrix. */ +export function invertMat3(m: Mat3): Mat3 { + const [a, b, c, d, e, f, g, h, i] = m; + const A = e * i - f * h; + const B = -(d * i - f * g); + const C = d * h - e * g; + const det = a * A + b * B + c * C; + if (Math.abs(det) < 1e-12) throw new Error('Non-invertible matrix'); + const invDet = 1 / det; + return [ + A * invDet, (c * h - b * i) * invDet, (b * f - c * e) * invDet, + B * invDet, (a * i - c * g) * invDet, (c * d - a * f) * invDet, + C * invDet, (b * g - a * h) * invDet, (a * e - b * d) * invDet, + ]; +}