feat: homography estimate/apply/invert

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>

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);
+  });
+});

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,
+  ];
+}