From 65f03366f1b275879ffb9103213ca6e0df977850 Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Fri, 2 Oct 2026 11:10:49 -0400 Subject: [PATCH] M6.6: mask tracking + mask interpolation core (KLT, RANSAC fits, stabilization math) Library groundwork for mask tracking (MSK-3) and the Warp Stabilizer (TRK-2), clean-room from published methods: - track::klt: Shi-Tomasi corners, pyramidal Lucas-Kanade with forward-backward check - track::fit: RANSAC + least-squares translation/scale/similarity/affine/homography - track::mask: MaskTracker following the pixels inside a mask outline per frame - track::stabilize: incremental clip analysis (serde WarpAnalysis), Gaussian path smoothing (Matsushita 2006), No Motion, preserve scale, crop / auto-scale with max-scale relaxation - path::interp: Mask Interpolation correspondence (arc-length vertex insertion, shape-context matching) and rigid/linear in-betweens Engine commands, UI and the effect itself are not wired yet (stubs unchanged). Co-Authored-By: Claude Opus 5.5 --- Cargo.lock | 1 + crates/path/src/interp.rs | 586 ++++++++++++++++++++++++++++++++++ crates/path/src/lib.rs | 1 + crates/track/Cargo.toml | 1 + crates/track/src/fit.rs | 308 ++++++++++++++++++ crates/track/src/klt.rs | 428 +++++++++++++++++++++++++ crates/track/src/lib.rs | 4 + crates/track/src/mask.rs | 222 +++++++++++++ crates/track/src/solve.rs | 85 ++++- crates/track/src/stabilize.rs | 576 +++++++++++++++++++++++++++++++++ 10 files changed, 2211 insertions(+), 1 deletion(-) create mode 100644 crates/path/src/interp.rs create mode 100644 crates/track/src/fit.rs create mode 100644 crates/track/src/klt.rs create mode 100644 crates/track/src/mask.rs create mode 100644 crates/track/src/stabilize.rs diff --git a/Cargo.lock b/Cargo.lock index 6ce4ac9f..bf29ec8a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1398,6 +1398,7 @@ dependencies = [ "effectcraft-raster", "rayon", "serde", + "serde_json", ] [[package]] diff --git a/crates/path/src/interp.rs b/crates/path/src/interp.rs new file mode 100644 index 00000000..a3fe075a --- /dev/null +++ b/crates/path/src/interp.rs @@ -0,0 +1,586 @@ +//! Smart mask interpolation (the Mask Interpolation panel): give two mask shapes the same vertex +//! count with a sensible vertex correspondence, and compute in-between shapes. +//! +//! - **Vertex insertion** splits Bezier segments with de Casteljau subdivision, so both shapes +//! keep their exact outline while gaining vertices; vertices are distributed by **arc length** +//! so the i-th vertex of each shape sits at a comparable fraction of the outline. +//! - **Correspondence** picks the cyclic offset (and, for closed shapes, the winding) of the +//! second shape that minimises a matching cost: normalised position differences plus, when +//! Quality is above 0, the χ² distance of log-polar **shape context** histograms (Belongie, +//! Malik & Puzicha, "Shape Matching and Object Recognition Using Shape Contexts", PAMI 2002). +//! *First Vertices Match* pins the offset to 0; *Use 1:1 Vertex Matches* keeps the original +//! vertices when the counts agree. +//! - **In-betweens** move vertices along straight lines (*Use Linear Vertex Paths*) or, by +//! default, with the shapes' best-fit rotation and scale (Procrustes) interpolated so turning +//! shapes turn instead of collapsing; *Bending Resistance* weights that rigid component +//! against the straight-line one. + +use effectcraft_keyframe::ShapePath; + +/// Matching Method. +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] +pub enum Matching { + #[default] + Auto, + Curve, + Polyline, +} + +/// Add Mask Shape Vertices. +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum AddVertices { + /// Pixels between vertices. + Pixels(f64), + /// Total vertices. + Total(usize), + /// One vertex every this percentage of the outline. + Percent(f64), +} + +/// Mask Interpolation options. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct InterpOpts { + pub linear: bool, + /// Bending Resistance, 0–100. + pub bending_resistance: f64, + /// Quality, 0–100. + pub quality: f64, + pub add_vertices: Option, + pub matching: Matching, + pub one_to_one: bool, + pub first_vertices_match: bool, +} + +impl Default for InterpOpts { + fn default() -> Self { + InterpOpts { + linear: false, + bending_resistance: 50.0, + quality: 50.0, + add_vertices: Some(AddVertices::Pixels(15.0)), + matching: Matching::Auto, + one_to_one: false, + first_vertices_match: true, + } + } +} + +type P = [f64; 2]; + +fn add(a: P, b: P) -> P { + [a[0] + b[0], a[1] + b[1]] +} +fn sub(a: P, b: P) -> P { + [a[0] - b[0], a[1] - b[1]] +} +fn mul(a: P, k: f64) -> P { + [a[0] * k, a[1] * k] +} +fn lerp(a: P, b: P, t: f64) -> P { + [a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t] +} + +/// Segment `i` (vertex i → i+1, wrapping for closed paths) as cubic control points. +fn segment(p: &ShapePath, i: usize) -> [P; 4] { + let n = p.len(); + let j = (i + 1) % n; + let (a, b) = (p.vertices[i], p.vertices[j]); + let o = p.out_tangents.get(i).copied().unwrap_or([0.0; 2]); + let it = p.in_tangents.get(j).copied().unwrap_or([0.0; 2]); + [a, add(a, o), add(b, it), b] +} + +fn seg_count(p: &ShapePath) -> usize { + let n = p.len(); + if n < 2 { + 0 + } else if p.closed { + n + } else { + n - 1 + } +} + +fn cubic(c: &[P; 4], t: f64) -> P { + let u = 1.0 - t; + let (a, b, cc, d) = (u * u * u, 3.0 * u * u * t, 3.0 * u * t * t, t * t * t); + [a * c[0][0] + b * c[1][0] + cc * c[2][0] + d * c[3][0], a * c[0][1] + b * c[1][1] + cc * c[2][1] + d * c[3][1]] +} + +fn seg_len(c: &[P; 4]) -> f64 { + let mut l = 0.0; + let mut prev = c[0]; + for k in 1..=24 { + let q = cubic(c, k as f64 / 24.0); + l += (q[0] - prev[0]).hypot(q[1] - prev[1]); + prev = q; + } + l +} + +/// Outline length. +pub fn length(p: &ShapePath) -> f64 { + (0..seg_count(p)).map(|i| seg_len(&segment(p, i))).sum() +} + +/// Split cubic `c` at parameter values `ts` (increasing, in (0, 1)) into `ts.len() + 1` cubics. +fn split_many(c: &[P; 4], ts: &[f64]) -> Vec<[P; 4]> { + let mut out = vec![]; + let mut rest = *c; + let mut t0 = 0.0; + for &t in ts { + let u = ((t - t0) / (1.0 - t0)).clamp(0.0, 1.0); + let (l, r) = split(&rest, u); + out.push(l); + rest = r; + t0 = t; + } + out.push(rest); + out +} + +fn split(c: &[P; 4], t: f64) -> ([P; 4], [P; 4]) { + let p01 = lerp(c[0], c[1], t); + let p12 = lerp(c[1], c[2], t); + let p23 = lerp(c[2], c[3], t); + let a = lerp(p01, p12, t); + let b = lerp(p12, p23, t); + let m = lerp(a, b, t); + ([c[0], p01, a, m], [m, b, p23, c[3]]) +} + +/// Parameter values splitting `c` into `k` pieces of (approximately) equal arc length. +fn arc_params(c: &[P; 4], k: usize) -> Vec { + if k <= 1 { + return vec![]; + } + const S: usize = 64; + let mut cum = vec![0.0; S + 1]; + let mut prev = c[0]; + for i in 1..=S { + let q = cubic(c, i as f64 / S as f64); + cum[i] = cum[i - 1] + (q[0] - prev[0]).hypot(q[1] - prev[1]); + prev = q; + } + let total = cum[S]; + if total < 1e-12 { + return (1..k).map(|j| j as f64 / k as f64).collect(); + } + (1..k) + .map(|j| { + let target = total * j as f64 / k as f64; + let i = cum.partition_point(|v| *v < target).clamp(1, S); + let f = (target - cum[i - 1]) / (cum[i] - cum[i - 1]).max(1e-12); + ((i - 1) as f64 + f) / S as f64 + }) + .collect() +} + +/// Rebuild a ShapePath from consecutive cubics (closed: the last ends at the first's start). +fn from_cubics(cs: &[[P; 4]], closed: bool) -> ShapePath { + let n = if closed { cs.len() } else { cs.len() + 1 }; + let mut v = vec![[0.0; 2]; n]; + let mut ins = vec![[0.0; 2]; n]; + let mut outs = vec![[0.0; 2]; n]; + for (i, c) in cs.iter().enumerate() { + let j = if closed { (i + 1) % n } else { i + 1 }; + v[i] = c[0]; + outs[i] = sub(c[1], c[0]); + ins[j] = sub(c[2], c[3]); + if !closed && j == n - 1 { + v[j] = c[3]; + } + } + ShapePath { vertices: v, in_tangents: ins, out_tangents: outs, closed } +} + +/// Exactly the same outline with `total` vertices (≥ the current count), the extra vertices +/// distributed over segments in proportion to their length. +pub fn with_vertex_count(p: &ShapePath, total: usize) -> ShapePath { + let m = seg_count(p); + let base = if p.closed { m } else { m + 1 }; + if m == 0 || total <= base { + return p.clone(); + } + let segs: Vec<[P; 4]> = (0..m).map(|i| segment(p, i)).collect(); + let lens: Vec = segs.iter().map(seg_len).collect(); + let sum: f64 = lens.iter().sum::().max(1e-12); + let pieces = total - base + m; // total segments wanted + // Largest remainder allocation, at least one piece per segment. + let extra = pieces - m; + let want: Vec = lens.iter().map(|l| l / sum * extra as f64).collect(); + let mut k: Vec = want.iter().map(|w| 1 + w.floor() as usize).collect(); + let mut left = pieces - k.iter().sum::(); + let mut order: Vec = (0..m).collect(); + order.sort_by(|a, b| (want[*b] - want[*b].floor()).total_cmp(&(want[*a] - want[*a].floor())).then(a.cmp(b))); + for i in order.iter().cycle() { + if left == 0 { + break; + } + k[*i] += 1; + left -= 1; + } + let mut cubics = vec![]; + for (c, ki) in segs.iter().zip(&k) { + cubics.extend(split_many(c, &arc_params(c, *ki))); + } + from_cubics(&cubics, p.closed) +} + +/// The same outline as a polyline with `total` vertices at equal arc-length spacing, starting at +/// vertex 0. +pub fn polyline(p: &ShapePath, total: usize) -> ShapePath { + let m = seg_count(p); + if m == 0 || total < 2 { + return ShapePath::polygon(&p.vertices, p.closed); + } + // Dense samples with cumulative length. + let mut pts = vec![p.vertices[0]]; + for i in 0..m { + let c = segment(p, i); + for k in 1..=32 { + pts.push(cubic(&c, k as f64 / 32.0)); + } + } + let mut cum = vec![0.0]; + for w in pts.windows(2) { + let l = cum.last().copied().unwrap_or(0.0) + (w[1][0] - w[0][0]).hypot(w[1][1] - w[0][1]); + cum.push(l); + } + let total_len = *cum.last().unwrap_or(&0.0); + let n = total; + let div = if p.closed { n as f64 } else { (n - 1) as f64 }; + let out: Vec

= (0..n) + .map(|j| { + let target = total_len * j as f64 / div; + let i = cum.partition_point(|v| *v < target).clamp(1, cum.len() - 1); + let f = (target - cum[i - 1]) / (cum[i] - cum[i - 1]).max(1e-12); + lerp(pts[i - 1], pts[i], f) + }) + .collect(); + ShapePath::polygon(&out, p.closed) +} + +fn has_curves(p: &ShapePath) -> bool { + p.in_tangents.iter().chain(&p.out_tangents).any(|t| t[0].abs() > 1e-9 || t[1].abs() > 1e-9) +} + +/// Rotate a closed path's vertex order to start at `k`. +fn rotate(p: &ShapePath, k: usize) -> ShapePath { + let n = p.len(); + if n == 0 || k.is_multiple_of(n) { + return p.clone(); + } + let r = |v: &Vec

| (0..n).map(|i| v[(i + k) % n]).collect(); + ShapePath { vertices: r(&p.vertices), in_tangents: r(&p.in_tangents), out_tangents: r(&p.out_tangents), closed: p.closed } +} + +/// The same path traversed the other way (closed paths keep vertex 0 first). +pub fn reversed(p: &ShapePath) -> ShapePath { + let n = p.len(); + if n == 0 { + return p.clone(); + } + let idx: Vec = if p.closed { (0..n).map(|i| (n - i) % n).collect() } else { (0..n).rev().collect() }; + ShapePath { + vertices: idx.iter().map(|i| p.vertices[*i]).collect(), + in_tangents: idx.iter().map(|i| p.out_tangents[*i]).collect(), + out_tangents: idx.iter().map(|i| p.in_tangents[*i]).collect(), + closed: p.closed, + } +} + +fn signed_area(v: &[P]) -> f64 { + let n = v.len(); + (0..n).map(|i| v[i][0] * v[(i + 1) % n][1] - v[(i + 1) % n][0] * v[i][1]).sum::() * 0.5 +} + +/// Centroid and RMS radius of points. +fn frame(v: &[P]) -> (P, f64) { + let n = v.len().max(1) as f64; + let c = [v.iter().map(|p| p[0]).sum::() / n, v.iter().map(|p| p[1]).sum::() / n]; + let r = (v.iter().map(|p| (p[0] - c[0]).powi(2) + (p[1] - c[1]).powi(2)).sum::() / n).sqrt().max(1e-9); + (c, r) +} + +const RBINS: usize = 5; +const ABINS: usize = 12; + +/// Log-polar shape context histograms of every point (normalised by the RMS radius). +fn shape_contexts(v: &[P]) -> Vec<[f64; RBINS * ABINS]> { + let (_, r) = frame(v); + let n = v.len(); + (0..n) + .map(|i| { + let mut h = [0.0; RBINS * ABINS]; + for j in 0..n { + if i == j { + continue; + } + let d = sub(v[j], v[i]); + let dist = d[0].hypot(d[1]) / r; + // log radius in [1/8, 2] → 5 bins. + let lr = (dist.max(1e-6).log2() + 3.0) / 4.0 * RBINS as f64; + if !(0.0..RBINS as f64).contains(&lr) { + continue; + } + let a = (d[1].atan2(d[0]) + std::f64::consts::PI) / std::f64::consts::TAU * ABINS as f64; + let ai = (a as usize).min(ABINS - 1); + h[lr as usize * ABINS + ai] += 1.0; + } + let s: f64 = h.iter().sum::().max(1.0); + h.iter_mut().for_each(|x| *x /= s); + h + }) + .collect() +} + +fn chi2(a: &[f64], b: &[f64]) -> f64 { + a.iter().zip(b).map(|(x, y)| if x + y > 1e-12 { (x - y) * (x - y) / (x + y) } else { 0.0 }).sum::() * 0.5 +} + +/// Matching cost of `a[i] ↔ b[(i + k) % n]`. +fn cost(a: &[P], b: &[P], sa: Option<&[[f64; RBINS * ABINS]]>, sb: Option<&[[f64; RBINS * ABINS]]>, k: usize) -> f64 { + let n = a.len(); + let (ca, ra) = frame(a); + let (cb, rb) = frame(b); + let mut c = 0.0; + for i in 0..n { + let j = (i + k) % n; + let pa = mul(sub(a[i], ca), 1.0 / ra); + let pb = mul(sub(b[j], cb), 1.0 / rb); + c += (pa[0] - pb[0]).powi(2) + (pa[1] - pb[1]).powi(2); + if let (Some(sa), Some(sb)) = (sa, sb) { + c += chi2(&sa[i], &sb[j]); + } + } + c / n as f64 +} + +/// Give `a` and `b` the same vertex count with matching vertices (the shapes that the first +/// and last keys get, and the input of [`interpolate`]). +pub fn correspond(a: &ShapePath, b: &ShapePath, o: &InterpOpts) -> (ShapePath, ShapePath) { + if a.is_empty() || b.is_empty() { + return (a.clone(), b.clone()); + } + let closed = a.closed && b.closed; + let curves = match o.matching { + Matching::Curve => true, + Matching::Polyline => false, + Matching::Auto => has_curves(a) || has_curves(b), + }; + let (mut ra, mut rb); + if o.one_to_one && a.len() == b.len() { + (ra, rb) = (a.clone(), b.clone()); + } else { + let (la, lb) = (length(a), length(b)); + let mut n = a.len().max(b.len()); + if let Some(av) = o.add_vertices { + let want = match av { + AddVertices::Pixels(px) => (la.max(lb) / px.max(1.0)).ceil() as usize, + AddVertices::Total(t) => t, + AddVertices::Percent(pc) => (100.0 / pc.clamp(0.1, 100.0)).ceil() as usize, + }; + n = n.max(want.min(2000)); + } + if curves { + ra = with_vertex_count(a, n); + rb = with_vertex_count(b, n); + } else { + ra = polyline(a, n); + rb = polyline(b, n); + } + // Proportional splitting can leave counts unequal when one shape already had more + // vertices than its share: fall back to arc-length polylines then. + if ra.len() != rb.len() { + ra = polyline(a, n); + rb = polyline(b, n); + } + } + if closed && ra.len() == rb.len() && ra.len() >= 3 { + // Same winding. + if signed_area(&ra.vertices).signum() != signed_area(&rb.vertices).signum() { + rb = reversed(&rb); + } + if !o.first_vertices_match { + let n = ra.len(); + let use_sc = o.quality > 0.0; + let (sa, sb) = if use_sc { (Some(shape_contexts(&ra.vertices)), Some(shape_contexts(&rb.vertices))) } else { (None, None) }; + // Quality limits the offsets examined exhaustively (coarse step), then refines. + let step = if o.quality >= 50.0 { 1 } else { ((n as f64 / 24.0).ceil() as usize).max(1) }; + let mut best = (f64::MAX, 0usize); + let mut k = 0; + while k < n { + let c = cost(&ra.vertices, &rb.vertices, sa.as_deref(), sb.as_deref(), k); + if c < best.0 { + best = (c, k); + } + k += step; + } + if step > 1 { + let k0 = best.1; + for d in 1..step { + for k in [(k0 + d) % n, (k0 + n - d) % n] { + let c = cost(&ra.vertices, &rb.vertices, sa.as_deref(), sb.as_deref(), k); + if c < best.0 { + best = (c, k); + } + } + } + } + rb = rotate(&rb, best.1); + } + } + (ra, rb) +} + +/// The in-between shape at fraction `u` of two corresponded shapes (equal vertex counts). +pub fn interpolate(a: &ShapePath, b: &ShapePath, u: f64, o: &InterpOpts) -> ShapePath { + if a.len() != b.len() || a.is_empty() { + return if u < 1.0 { a.clone() } else { b.clone() }; + } + let n = a.len(); + let lin = |pa: &[P], pb: &[P]| -> Vec

{ pa.iter().zip(pb).map(|(x, y)| lerp(*x, *y, u)).collect() }; + if o.linear { + return ShapePath { + vertices: lin(&a.vertices, &b.vertices), + in_tangents: lin(&a.in_tangents, &b.in_tangents), + out_tangents: lin(&a.out_tangents, &b.out_tangents), + closed: a.closed, + }; + } + // Procrustes: b ≈ s R (a − ca) + cb. + let (ca, _) = frame(&a.vertices); + let (cb, _) = frame(&b.vertices); + let (mut ss, mut dot, mut cross) = (0.0, 0.0, 0.0); + for i in 0..n { + let (p, q) = (sub(a.vertices[i], ca), sub(b.vertices[i], cb)); + ss += p[0] * p[0] + p[1] * p[1]; + dot += p[0] * q[0] + p[1] * q[1]; + cross += p[0] * q[1] - p[1] * q[0]; + } + let theta = cross.atan2(dot); + let s = if ss > 1e-12 { dot.hypot(cross) / ss } else { 1.0 }; + let rot = |v: P, ang: f64, k: f64| { + let (sn, cs) = ang.sin_cos(); + [k * (cs * v[0] - sn * v[1]), k * (sn * v[0] + cs * v[1])] + }; + let (th_u, s_u) = (theta * u, s.max(1e-9).powf(u)); + let c_u = lerp(ca, cb, u); + let w = (o.bending_resistance / 100.0).clamp(0.0, 1.0); + let mut out = ShapePath { vertices: vec![], in_tangents: vec![], out_tangents: vec![], closed: a.closed }; + for i in 0..n { + // Rigid path: a's vertex carried by the interpolated similarity, plus the residual + // (b − fully transformed a) blended in linearly. + let pa = sub(a.vertices[i], ca); + let full = add(rot(pa, theta, s), cb); + let resid = sub(b.vertices[i], full); + let rigid = add(add(rot(pa, th_u, s_u), c_u), mul(resid, u)); + let linear = lerp(a.vertices[i], b.vertices[i], u); + out.vertices.push(lerp(linear, rigid, w)); + for (src_a, src_b, dst) in [(&a.in_tangents, &b.in_tangents, &mut out.in_tangents), (&a.out_tangents, &b.out_tangents, &mut out.out_tangents)] { + let ta = src_a[i]; + let tr = sub(src_b[i], rot(ta, theta, s)); + let rigid_t = add(rot(ta, th_u, s_u), mul(tr, u)); + dst.push(lerp(lerp(ta, src_b[i], u), rigid_t, w)); + } + } + out +} + +/// Do any two vertex paths (straight segments from `a[i]` to `b[i]`) cross? (A sanity measure of +/// the correspondence: good matches don't make vertices pass through each other.) +pub fn paths_cross(a: &ShapePath, b: &ShapePath) -> usize { + let n = a.len().min(b.len()); + let mut c = 0; + let inter = |p1: P, p2: P, p3: P, p4: P| { + let d = |a: P, b: P, c: P| (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]); + let (d1, d2, d3, d4) = (d(p3, p4, p1), d(p3, p4, p2), d(p1, p2, p3), d(p1, p2, p4)); + d1 * d2 < 0.0 && d3 * d4 < 0.0 + }; + for i in 0..n { + for j in i + 1..n { + if inter(a.vertices[i], b.vertices[i], a.vertices[j], b.vertices[j]) { + c += 1; + } + } + } + c +} + +#[cfg(test)] +mod tests { + use super::*; + + fn star(c: P, r: f64, k: usize, rot: f64) -> ShapePath { + let pts: Vec

= (0..2 * k) + .map(|i| { + let a = rot + i as f64 * std::f64::consts::PI / k as f64; + let rr = if i % 2 == 0 { r } else { r * 0.5 }; + [c[0] + rr * a.cos(), c[1] + rr * a.sin()] + }) + .collect(); + ShapePath::polygon(&pts, true) + } + + #[test] + fn splitting_keeps_the_outline() { + let e = ShapePath::ellipse([50.0, 50.0], 80.0, 40.0); + let m = with_vertex_count(&e, 23); + assert_eq!(m.len(), 23); + assert!((length(&m) - length(&e)).abs() < 0.05, "{} vs {}", length(&m), length(&e)); + // Points on the split path lie on the ellipse. + for i in 0..m.len() { + let v = m.vertices[i]; + let q = ((v[0] - 50.0) / 40.0).powi(2) + ((v[1] - 50.0) / 20.0).powi(2); + assert!((q - 1.0).abs() < 0.01, "{v:?}"); + } + let r = reversed(&ShapePath::rect([0.0, 0.0], 10.0, 10.0)); + assert_eq!(r.vertices[0], [-5.0, -5.0]); + assert_eq!(r.vertices[1], [-5.0, 5.0]); + } + + #[test] + fn correspondence_finds_the_rotation_and_paths_dont_cross() { + // A star rotated by 2 points' worth and with its first vertex elsewhere. + let a = star([100.0, 100.0], 60.0, 5, 0.0); + let b0 = star([140.0, 110.0], 50.0, 5, 0.3); + let b = rotate(&b0, 3); + let o = InterpOpts { first_vertices_match: false, add_vertices: None, ..Default::default() }; + let (ra, rb) = correspond(&a, &b, &o); + assert_eq!(ra.len(), rb.len()); + // The true correspondence is recovered (b0's vertex 0 matches a's vertex 0). + assert!((rb.vertices[0][0] - b0.vertices[0][0]).abs() < 1e-9 && (rb.vertices[0][1] - b0.vertices[0][1]).abs() < 1e-9); + assert!(paths_cross(&ra, &rb) <= 1); + // With First Vertices Match the given (bad) start is kept: paths cross. + let o2 = InterpOpts { first_vertices_match: true, add_vertices: None, ..Default::default() }; + let (xa, xb) = correspond(&a, &b, &o2); + assert!(paths_cross(&xa, &xb) > 5); + // In-betweens keep the vertex count and stay star-like (area between the ends). + let mid = interpolate(&ra, &rb, 0.5, &o); + assert_eq!(mid.len(), ra.len()); + let (aa, ab, am) = (signed_area(&ra.vertices).abs(), signed_area(&rb.vertices).abs(), signed_area(&mid.vertices).abs()); + assert!(am > ab.min(aa) * 0.8 && am < aa.max(ab) * 1.2, "{aa} {ab} {am}"); + // Different vertex counts with added vertices. + let c = ShapePath::ellipse([100.0, 100.0], 120.0, 120.0); + let o3 = InterpOpts { first_vertices_match: false, ..Default::default() }; + let (pa, pb) = correspond(&a, &c, &o3); + assert_eq!(pa.len(), pb.len()); + assert!(pa.len() >= 25); + assert!(paths_cross(&pa, &pb) <= pa.len() / 10, "{}", paths_cross(&pa, &pb)); + } + + #[test] + fn rigid_interpolation_turns_instead_of_collapsing() { + let a = ShapePath::polygon(&[[-50.0, -10.0], [50.0, -10.0], [50.0, 10.0], [-50.0, 10.0]], true); + // The same bar turned 90°. + let b = ShapePath::polygon(&[[10.0, -50.0], [10.0, 50.0], [-10.0, 50.0], [-10.0, -50.0]], true); + let o = InterpOpts { bending_resistance: 100.0, ..Default::default() }; + let m = interpolate(&a, &b, 0.5, &o); + let len = (m.vertices[1][0] - m.vertices[0][0]).hypot(m.vertices[1][1] - m.vertices[0][1]); + assert!((len - 100.0).abs() < 1e-6, "{len}"); + let lin = interpolate(&a, &b, 0.5, &InterpOpts { linear: true, ..Default::default() }); + let l2 = (lin.vertices[1][0] - lin.vertices[0][0]).hypot(lin.vertices[1][1] - lin.vertices[0][1]); + assert!(l2 < 80.0); + } +} diff --git a/crates/path/src/lib.rs b/crates/path/src/lib.rs index 1a1ae2ad..d57f857e 100644 --- a/crates/path/src/lib.rs +++ b/crates/path/src/lib.rs @@ -7,6 +7,7 @@ pub mod boolean; mod fit; +pub mod interp; pub mod offset; pub mod ops; diff --git a/crates/track/Cargo.toml b/crates/track/Cargo.toml index f8cd71d1..ba449c92 100644 --- a/crates/track/Cargo.toml +++ b/crates/track/Cargo.toml @@ -13,3 +13,4 @@ workspace = true effectcraft-raster = { workspace = true } rayon = { workspace = true } serde = { workspace = true } +serde_json = { workspace = true } diff --git a/crates/track/src/fit.rs b/crates/track/src/fit.rs new file mode 100644 index 00000000..75a0d9d1 --- /dev/null +++ b/crates/track/src/fit.rs @@ -0,0 +1,308 @@ +//! Robust motion-model fits between point correspondences: **RANSAC** (Fischler & Bolles, +//! "Random Sample Consensus", 1981) with minimal samples, followed by least-squares refits on the +//! consensus set (Hartley & Zisserman, *Multiple View Geometry*, §4.7–4.8). +//! +//! Models, in increasing freedom: translation, translation + uniform scale, similarity (rotation, +//! uniform scale, translation), affine (adds skew / non-uniform scale) and homography +//! (perspective). Every fit is returned as a [`Homography`] matrix mapping `src` to `dst`. + +use serde::{Deserialize, Serialize}; + +use crate::solve::{Homography, solve_linear}; + +/// A 2D motion model. +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(rename_all = "camelCase")] +pub enum Model { + Translation, + TranslationScale, + #[default] + Similarity, + Affine, + Homography, +} + +impl Model { + /// Points in a minimal sample. + pub fn min_points(self) -> usize { + match self { + Model::Translation => 1, + Model::TranslationScale | Model::Similarity => 2, + Model::Affine => 3, + Model::Homography => 4, + } + } +} + +fn centroid(p: &[[f64; 2]]) -> [f64; 2] { + let n = p.len().max(1) as f64; + [p.iter().map(|q| q[0]).sum::() / n, p.iter().map(|q| q[1]).sum::() / n] +} + +/// Least-squares fit of `model` taking `src` to `dst` (`None` when degenerate). +pub fn least_squares(model: Model, src: &[[f64; 2]], dst: &[[f64; 2]]) -> Option { + let n = src.len().min(dst.len()); + if n < model.min_points() { + return None; + } + let (src, dst) = (&src[..n], &dst[..n]); + let (cs, cd) = (centroid(src), centroid(dst)); + let h = match model { + Model::Translation => Homography::translation([cd[0] - cs[0], cd[1] - cs[1]]), + Model::TranslationScale | Model::Similarity => { + let (mut ss, mut dot, mut cross) = (0.0, 0.0, 0.0); + for (p, q) in src.iter().zip(dst) { + let (a, b) = ([p[0] - cs[0], p[1] - cs[1]], [q[0] - cd[0], q[1] - cd[1]]); + ss += a[0] * a[0] + a[1] * a[1]; + dot += a[0] * b[0] + a[1] * b[1]; + cross += a[0] * b[1] - a[1] * b[0]; + } + if ss < 1e-12 { + return None; + } + let (a, b) = if model == Model::Similarity { (dot / ss, cross / ss) } else { (dot / ss, 0.0) }; + if a.hypot(b) < 1e-9 { + return None; + } + // q = R p + t with R = [[a, -b], [b, a]]. + let t = [cd[0] - (a * cs[0] - b * cs[1]), cd[1] - (b * cs[0] + a * cs[1])]; + Homography([[a, -b, t[0]], [b, a, t[1]], [0.0, 0.0, 1.0]]) + } + Model::Affine => { + let mut m = [[0.0; 3]; 3]; + let (mut rx, mut ry) = ([0.0; 3], [0.0; 3]); + for (p, q) in src.iter().zip(dst) { + let v = [p[0] - cs[0], p[1] - cs[1], 1.0]; + for i in 0..3 { + for j in 0..3 { + m[i][j] += v[i] * v[j]; + } + rx[i] += v[i] * (q[0] - cd[0]); + ry[i] += v[i] * (q[1] - cd[1]); + } + } + let x = solve_linear(m, rx)?; + let y = solve_linear(m, ry)?; + // q - cd = A (p - cs) + t' + let t = [cd[0] + x[2] - x[0] * cs[0] - x[1] * cs[1], cd[1] + y[2] - y[0] * cs[0] - y[1] * cs[1]]; + Homography([[x[0], x[1], t[0]], [y[0], y[1], t[1]], [0.0, 0.0, 1.0]]) + } + Model::Homography => { + // Normalised DLT with h33 = 1 (normal equations). + let norm = |p: &[[f64; 2]], c: [f64; 2]| { + let d = p.iter().map(|q| (q[0] - c[0]).hypot(q[1] - c[1])).sum::() / p.len() as f64; + if d > 1e-12 { std::f64::consts::SQRT_2 / d } else { 1.0 } + }; + let (ks, kd) = (norm(src, cs), norm(dst, cd)); + let mut a = [[0.0; 8]; 8]; + let mut b = [0.0; 8]; + for (p, q) in src.iter().zip(dst) { + let (x, y) = ((p[0] - cs[0]) * ks, (p[1] - cs[1]) * ks); + let (u, v) = ((q[0] - cd[0]) * kd, (q[1] - cd[1]) * kd); + let rows = [([x, y, 1.0, 0.0, 0.0, 0.0, -u * x, -u * y], u), ([0.0, 0.0, 0.0, x, y, 1.0, -v * x, -v * y], v)]; + for (r, t) in rows { + for i in 0..8 { + b[i] += r[i] * t; + for j in 0..8 { + a[i][j] += r[i] * r[j]; + } + } + } + } + let h = solve_linear(a, b)?; + let hn = Homography([[h[0], h[1], h[2]], [h[3], h[4], h[5]], [h[6], h[7], 1.0]]); + let ts = Homography([[ks, 0.0, -ks * cs[0]], [0.0, ks, -ks * cs[1]], [0.0, 0.0, 1.0]]); + let td_inv = Homography([[1.0 / kd, 0.0, cd[0]], [0.0, 1.0 / kd, cd[1]], [0.0, 0.0, 1.0]]); + let m = td_inv.then_after(&hn.then_after(&ts)); + if m.0[2][2].abs() < 1e-12 { + return None; + } + m.normalized() + } + }; + h.0.iter().flatten().all(|v| v.is_finite()).then_some(h) +} + +fn residual2(h: &Homography, p: [f64; 2], q: [f64; 2]) -> f64 { + let m = h.apply(p); + (m[0] - q[0]).powi(2) + (m[1] - q[1]).powi(2) +} + +/// Deterministic xorshift generator (reproducible fits). +struct Rng(u64); +impl Rng { + fn next(&mut self) -> u64 { + let mut x = self.0; + x ^= x << 13; + x ^= x >> 7; + x ^= x << 17; + self.0 = x; + x + } + fn below(&mut self, n: usize) -> usize { + (self.next() % n.max(1) as u64) as usize + } +} + +/// A robust fit: the model, which correspondences are inliers, and the RMS inlier error. +#[derive(Clone, Debug)] +pub struct Fit { + pub h: Homography, + pub inliers: Vec, + pub rms: f64, +} + +impl Fit { + pub fn inlier_count(&self) -> usize { + self.inliers.iter().filter(|b| **b).count() + } +} + +/// RANSAC fit of `model` with inlier `threshold` (pixels) and at most `iterations` samples. +pub fn ransac(model: Model, src: &[[f64; 2]], dst: &[[f64; 2]], threshold: f64, iterations: usize, seed: u64) -> Option { + let n = src.len().min(dst.len()); + let k = model.min_points(); + if n < k { + return None; + } + let t2 = threshold * threshold; + let count = |h: &Homography| { + let mut c = 0; + let mut e = 0.0; + for i in 0..n { + let r = residual2(h, src[i], dst[i]); + if r < t2 { + c += 1; + e += r; + } + } + (c, e) + }; + let mut best: Option<(usize, f64, Homography)> = None; + if n == k { + let h = least_squares(model, src, dst)?; + let (c, e) = count(&h); + best = Some((c, e, h)); + } else { + let mut rng = Rng(seed.wrapping_mul(0x9E37_79B9_7F4A_7C15) | 1); + let mut max_iter = iterations.max(1); + let mut it = 0; + let mut idx = vec![0usize; k]; + let (mut ss, mut sd) = (vec![[0.0; 2]; k], vec![[0.0; 2]; k]); + while it < max_iter { + it += 1; + // Distinct random sample. + let mut j = 0; + let mut guard = 0; + while j < k && guard < 100 { + guard += 1; + let c = rng.below(n); + if idx[..j].contains(&c) { + continue; + } + idx[j] = c; + j += 1; + } + if j < k { + continue; + } + for (m, &i) in idx.iter().enumerate() { + ss[m] = src[i]; + sd[m] = dst[i]; + } + let Some(h) = least_squares(model, &ss, &sd) else { continue }; + let (c, e) = count(&h); + let better = match &best { + None => true, + Some((bc, be, _)) => c > *bc || (c == *bc && e < *be), + }; + if better { + best = Some((c, e, h)); + // Adaptive stopping (99 % confidence of an outlier-free sample). + let w = c as f64 / n as f64; + let p_good = w.powi(k as i32); + if p_good > 1e-9 && p_good < 1.0 { + let need = ((1.0 - 0.99f64).ln() / (1.0 - p_good).ln()).ceil(); + if need.is_finite() { + max_iter = max_iter.min((need as usize).max(it + 1)); + } + } else if p_good >= 1.0 { + break; + } + } + } + } + let (_, _, mut h) = best?; + // Refine on the consensus set (twice: the set can grow after the first refit). + let mut inl: Vec = (0..n).map(|i| residual2(&h, src[i], dst[i]) < t2).collect(); + for _ in 0..2 { + let (s2, d2): (Vec<[f64; 2]>, Vec<[f64; 2]>) = (0..n).filter(|i| inl[*i]).map(|i| (src[i], dst[i])).unzip(); + if s2.len() < k { + break; + } + if let Some(r) = least_squares(model, &s2, &d2) { + h = r; + } + inl = (0..n).map(|i| residual2(&h, src[i], dst[i]) < t2).collect(); + } + let c = inl.iter().filter(|b| **b).count(); + let e: f64 = (0..n).filter(|i| inl[*i]).map(|i| residual2(&h, src[i], dst[i])).sum(); + Some(Fit { h, inliers: inl, rms: if c > 0 { (e / c as f64).sqrt() } else { f64::INFINITY } }) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn pts() -> Vec<[f64; 2]> { + let mut v = vec![]; + let mut s = 12345u64; + for _ in 0..80 { + s = s.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407); + let x = (s >> 33) as f64 / (1u64 << 31) as f64 * 300.0; + s = s.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407); + let y = (s >> 33) as f64 / (1u64 << 31) as f64 * 200.0; + v.push([x, y]); + } + v + } + + #[test] + fn ransac_rejects_outliers_for_every_model() { + let truth = [ + (Model::Translation, Homography::translation([5.0, -3.0])), + (Model::TranslationScale, Homography([[1.1, 0.0, -4.0], [0.0, 1.1, 2.0], [0.0, 0.0, 1.0]])), + (Model::Similarity, Homography([[0.98, -0.17, 12.0], [0.17, 0.98, -6.0], [0.0, 0.0, 1.0]])), + (Model::Affine, Homography([[1.05, 0.12, 3.0], [-0.04, 0.93, 1.0], [0.0, 0.0, 1.0]])), + (Model::Homography, Homography([[1.02, 0.05, 4.0], [-0.03, 0.97, 2.0], [0.0004, -0.0003, 1.0]])), + ]; + for (m, h) in truth { + let src = pts(); + let mut dst: Vec<[f64; 2]> = src.iter().map(|p| h.apply(*p)).collect(); + // 30 % gross outliers. + for (i, d) in dst.iter_mut().enumerate() { + if i % 10 < 3 { + d[0] += 40.0 + i as f64; + d[1] -= 25.0; + } + } + let f = ransac(m, &src, &dst, 1.0, 500, 7).unwrap(); + assert_eq!(f.inlier_count(), 56, "{m:?}"); + for p in [[0.0, 0.0], [150.0, 100.0], [300.0, 200.0]] { + let (a, b) = (f.h.apply(p), h.apply(p)); + assert!((a[0] - b[0]).abs() < 1e-6 && (a[1] - b[1]).abs() < 1e-6, "{m:?} {a:?} {b:?}"); + } + } + } + + #[test] + fn inverse_and_compose() { + let h = Homography([[1.02, 0.05, 4.0], [-0.03, 0.97, 2.0], [0.0004, -0.0003, 1.0]]); + let i = h.inverse().unwrap(); + let p = [123.0, 45.0]; + let q = i.apply(h.apply(p)); + assert!((q[0] - p[0]).abs() < 1e-9 && (q[1] - p[1]).abs() < 1e-9); + let c = Homography::compose(&h, &i); + assert!(c.max_diff(&Homography::IDENTITY) < 1e-9); + } +} diff --git a/crates/track/src/klt.rs b/crates/track/src/klt.rs new file mode 100644 index 00000000..6c569c31 --- /dev/null +++ b/crates/track/src/klt.rs @@ -0,0 +1,428 @@ +//! Sparse feature tracking (KLT), clean-room from the published methods: +//! +//! - **corners**: the Shi–Tomasi criterion (Shi & Tomasi, "Good Features to Track", CVPR 1994): +//! the smaller eigenvalue of the gradient structure tensor summed over a small window, with +//! grid-based non-maximum suppression and a minimum distance between features; +//! - **tracking**: iterative Lucas–Kanade translation estimation (Lucas & Kanade 1981) run coarse +//! to fine over an image pyramid (the pyramidal scheme described in Bouguet's 2000 technical +//! note "Pyramidal Implementation of the Lucas Kanade Feature Tracker"), with a +//! **forward–backward** consistency check (Kalal et al., "Forward-Backward Error", ICPR 2010) to +//! drop features that were lost or occluded. +//! +//! Frames are analysed in luma at a reduced resolution ([`GrayPyramid::from_image`] takes an +//! integer box-downsampling factor). Coordinates follow [`Plane`]: pixel `i` covers `[i, i + 1)`. + +use effectcraft_raster::Image; +use rayon::prelude::*; + +use crate::plane::Plane; + +/// A luma pyramid with gradients per level. +#[derive(Clone, Debug, Default)] +pub struct GrayPyramid { + pub levels: Vec, + pub grads: Vec<(Plane, Plane)>, + /// Image pixels per level-0 pixel. + pub factor: f64, + /// Image pixel offset of the plane origin (layer pixel `p` is image pixel `p + offset`). + pub offset: [f64; 2], +} + +/// Luma of `img` averaged over `factor × factor` blocks (straight colour over premultiplied +/// alpha is ignored: tracking looks at the visible pixels). +pub fn luma_plane(img: &Image, factor: usize) -> Plane { + let f = factor.max(1); + let w = (img.width as usize).div_ceil(f).max(1); + let h = (img.height as usize).div_ceil(f).max(1); + let mut p = Plane::new(w, h, 1); + if img.is_empty() { + return p; + } + p.data.par_chunks_mut(w).enumerate().for_each(|(y, row)| { + for (x, o) in row.iter_mut().enumerate() { + let mut s = 0.0f32; + let mut n = 0.0f32; + for yy in y * f..((y + 1) * f).min(img.height as usize) { + for xx in x * f..((x + 1) * f).min(img.width as usize) { + let px = img.data[yy * img.width as usize + xx]; + s += 0.2126 * px[0] + 0.7152 * px[1] + 0.0722 * px[2]; + n += 1.0; + } + } + *o = if n > 0.0 { s / n } else { 0.0 }; + } + }); + p +} + +impl GrayPyramid { + /// A pyramid of `img` downsampled by `factor`, with `levels` extra half-resolution levels. + pub fn from_image(img: &Image, offset: [f64; 2], factor: usize, levels: usize) -> GrayPyramid { + let base = luma_plane(img, factor); + Self::from_plane(base, offset, factor as f64, levels) + } + + pub fn from_plane(base: Plane, offset: [f64; 2], factor: f64, levels: usize) -> GrayPyramid { + let mut lv = vec![base]; + for _ in 0..levels { + let last = lv.last().expect("level"); + if last.w < 16 || last.h < 16 { + break; + } + let d = last.downsample(); + lv.push(d); + } + let grads = lv.par_iter().map(Plane::gradients).collect(); + GrayPyramid { levels: lv, grads, factor, offset } + } + + /// Layer pixels → level-0 plane coordinates. + pub fn to_plane(&self, p: [f64; 2]) -> [f64; 2] { + [(p[0] + self.offset[0]) / self.factor, (p[1] + self.offset[1]) / self.factor] + } + /// Level-0 plane coordinates → layer pixels. + pub fn to_layer(&self, q: [f64; 2]) -> [f64; 2] { + [q[0] * self.factor - self.offset[0], q[1] * self.factor - self.offset[1]] + } + pub fn width(&self) -> usize { + self.levels.first().map(|p| p.w).unwrap_or(0) + } + pub fn height(&self) -> usize { + self.levels.first().map(|p| p.h).unwrap_or(0) + } +} + +/// Feature detection options. +#[derive(Clone, Copy, Debug)] +pub struct CornerOpts { + pub max_features: usize, + /// Minimum distance between features (level-0 plane pixels). + pub min_distance: f64, + /// Keep corners whose score is at least this fraction of the strongest. + pub quality: f64, + /// Structure-tensor window radius. + pub window: usize, + /// Ignore this many pixels at the border. + pub border: usize, +} + +impl Default for CornerOpts { + fn default() -> Self { + CornerOpts { max_features: 300, min_distance: 8.0, quality: 0.01, window: 2, border: 6 } + } +} + +/// Shi–Tomasi corner score (smaller eigenvalue of the structure tensor) per pixel. +pub fn corner_scores(p: &Plane, gx: &Plane, gy: &Plane, r: usize) -> Vec { + let (w, h) = (p.w, p.h); + // Products, then box sums via summed-area tables. + let n = w * h; + let mut sxx = vec![0.0f64; (w + 1) * (h + 1)]; + let mut sxy = vec![0.0f64; (w + 1) * (h + 1)]; + let mut syy = vec![0.0f64; (w + 1) * (h + 1)]; + for y in 0..h { + let (mut axx, mut axy, mut ayy) = (0.0, 0.0, 0.0); + for x in 0..w { + let i = y * w + x; + let (dx, dy) = (gx.data[i] as f64, gy.data[i] as f64); + axx += dx * dx; + axy += dx * dy; + ayy += dy * dy; + let o = (y + 1) * (w + 1) + x + 1; + let up = y * (w + 1) + x + 1; + sxx[o] = sxx[up] + axx; + sxy[o] = sxy[up] + axy; + syy[o] = syy[up] + ayy; + } + } + let mut out = vec![0.0f32; n]; + out.par_chunks_mut(w).enumerate().for_each(|(y, row)| { + let y0 = y.saturating_sub(r); + let y1 = (y + r + 1).min(h); + for (x, o) in row.iter_mut().enumerate() { + let x0 = x.saturating_sub(r); + let x1 = (x + r + 1).min(w); + let bs = |s: &[f64]| s[y1 * (w + 1) + x1] - s[y0 * (w + 1) + x1] - s[y1 * (w + 1) + x0] + s[y0 * (w + 1) + x0]; + let (a, b, c) = (bs(&sxx), bs(&sxy), bs(&syy)); + let tr = 0.5 * (a + c); + let d = (0.25 * (a - c) * (a - c) + b * b).sqrt(); + *o = (tr - d).max(0.0) as f32; + } + }); + out +} + +/// Good features to track in `pyr` level 0, optionally restricted by `inside(plane point)`. +/// Returns level-0 plane coordinates (pixel centres), strongest first. +pub fn good_features(pyr: &GrayPyramid, opts: &CornerOpts, inside: Option<&(dyn Fn([f64; 2]) -> bool + Sync)>) -> Vec<[f64; 2]> { + let Some(p) = pyr.levels.first() else { return vec![] }; + let (gx, gy) = &pyr.grads[0]; + let score = corner_scores(p, gx, gy, opts.window.max(1)); + let (w, h) = (p.w, p.h); + let b = opts.border.min(w / 4).min(h / 4); + // Local maxima (3 × 3) above the quality threshold. + let mut cands: Vec<(f32, usize, usize)> = vec![]; + let mut best = 0.0f32; + for y in b..h.saturating_sub(b) { + for x in b..w.saturating_sub(b) { + let v = score[y * w + x]; + if v <= 1e-9 { + continue; + } + let mut is_max = true; + 'n: for dy in -1i64..=1 { + for dx in -1i64..=1 { + if dx == 0 && dy == 0 { + continue; + } + let (xx, yy) = (x as i64 + dx, y as i64 + dy); + if xx < 0 || yy < 0 || xx >= w as i64 || yy >= h as i64 { + continue; + } + let o = score[yy as usize * w + xx as usize]; + if o > v || (o == v && (dy < 0 || (dy == 0 && dx < 0))) { + is_max = false; + break 'n; + } + } + } + if !is_max { + continue; + } + let c = [x as f64 + 0.5, y as f64 + 0.5]; + if let Some(f) = inside + && !f(c) + { + continue; + } + best = best.max(v); + cands.push((v, x, y)); + } + } + let thr = best * opts.quality as f32; + cands.retain(|c| c.0 >= thr); + cands.sort_by(|a, b| b.0.total_cmp(&a.0).then(a.2.cmp(&b.2)).then(a.1.cmp(&b.1))); + // Minimum distance via a coarse occupancy grid. + let md = opts.min_distance.max(1.0); + let cell = md; + let gw = (w as f64 / cell).ceil() as usize + 1; + let gh = (h as f64 / cell).ceil() as usize + 1; + let mut grid: Vec> = vec![vec![]; gw * gh]; + let mut out = vec![]; + for (_, x, y) in cands { + let c = [x as f64 + 0.5, y as f64 + 0.5]; + let (cx, cy) = ((c[0] / cell) as usize, (c[1] / cell) as usize); + let mut ok = true; + 'g: for yy in cy.saturating_sub(1)..=(cy + 1).min(gh - 1) { + for xx in cx.saturating_sub(1)..=(cx + 1).min(gw - 1) { + for q in &grid[yy * gw + xx] { + if (q[0] - c[0]).powi(2) + (q[1] - c[1]).powi(2) < md * md { + ok = false; + break 'g; + } + } + } + } + if ok { + grid[cy * gw + cx].push(c); + out.push(c); + if out.len() >= opts.max_features { + break; + } + } + } + out +} + +/// Lucas–Kanade options. +#[derive(Clone, Copy, Debug)] +pub struct LkOpts { + /// Window radius (the window is `2r + 1` square). + pub radius: i32, + pub iterations: usize, + /// Stop when the update is below this (pixels). + pub epsilon: f64, + /// Reject windows whose structure tensor's smaller eigenvalue (per pixel) is below this. + pub min_eigen: f64, + /// Forward–backward error limit (level-0 pixels); 0 = no check. + pub fb_max: f64, +} + +impl Default for LkOpts { + fn default() -> Self { + LkOpts { radius: 5, iterations: 20, epsilon: 0.01, min_eigen: 1e-5, fb_max: 1.0 } + } +} + +fn lk_point(a: &GrayPyramid, b: &GrayPyramid, p: [f64; 2], guess: [f64; 2], o: &LkOpts) -> Option<[f64; 2]> { + let top = a.levels.len().min(b.levels.len()).checked_sub(1)?; + let r = o.radius; + let n = ((2 * r + 1) * (2 * r + 1)) as f64; + let s0 = (1u32 << top) as f64; + let mut g = [guess[0] / s0, guess[1] / s0]; + let mut win = vec![0.0f32; ((2 * r + 1) * (2 * r + 1)) as usize]; + let mut wgx = win.clone(); + let mut wgy = win.clone(); + let mut v1 = [0.0f32]; + for l in (0..=top).rev() { + let s = (1u32 << l) as f64; + let (ia, ib) = (&a.levels[l], &b.levels[l]); + let (gx, gy) = &a.grads[l]; + let c = [p[0] / s, p[1] / s]; + if c[0] < -1.0 || c[1] < -1.0 || c[0] > ia.w as f64 + 1.0 || c[1] > ia.h as f64 + 1.0 { + return None; + } + let (mut gxx, mut gxy, mut gyy) = (0.0f64, 0.0f64, 0.0f64); + let mut k = 0; + for dy in -r..=r { + for dx in -r..=r { + let (x, y) = (c[0] + dx as f64, c[1] + dy as f64); + ia.sample(x, y, &mut v1); + win[k] = v1[0]; + gx.sample(x, y, &mut v1); + wgx[k] = v1[0]; + gy.sample(x, y, &mut v1); + wgy[k] = v1[0]; + let (ix, iy) = (wgx[k] as f64, wgy[k] as f64); + gxx += ix * ix; + gxy += ix * iy; + gyy += iy * iy; + k += 1; + } + } + let det = gxx * gyy - gxy * gxy; + let tr = 0.5 * (gxx + gyy); + let mineig = (tr - (0.25 * (gxx - gyy).powi(2) + gxy * gxy).sqrt()) / n; + if mineig < o.min_eigen || det.abs() < 1e-18 { + return None; + } + let mut v = [0.0f64; 2]; + for _ in 0..o.iterations { + let (mut bx, mut by) = (0.0f64, 0.0f64); + let mut k = 0; + for dy in -r..=r { + for dx in -r..=r { + ib.sample(c[0] + g[0] + v[0] + dx as f64, c[1] + g[1] + v[1] + dy as f64, &mut v1); + let diff = (win[k] - v1[0]) as f64; + bx += diff * wgx[k] as f64; + by += diff * wgy[k] as f64; + k += 1; + } + } + let nx = (gyy * bx - gxy * by) / det; + let ny = (gxx * by - gxy * bx) / det; + v[0] += nx; + v[1] += ny; + if !(v[0].is_finite() && v[1].is_finite()) { + return None; + } + if nx * nx + ny * ny < o.epsilon * o.epsilon { + break; + } + } + if l > 0 { + g = [2.0 * (g[0] + v[0]), 2.0 * (g[1] + v[1])]; + } else { + g = [g[0] + v[0], g[1] + v[1]]; + } + } + let q = [p[0] + g[0], p[1] + g[1]]; + let l0 = &b.levels[0]; + if q[0] < 0.0 || q[1] < 0.0 || q[0] > l0.w as f64 || q[1] > l0.h as f64 { + return None; + } + Some(q) +} + +/// Track level-0 points from pyramid `a` to pyramid `b` (with optional per-point initial +/// displacement guesses). `None` for lost points (untrackable window, left the frame, or failed +/// the forward–backward check). +pub fn track(a: &GrayPyramid, b: &GrayPyramid, pts: &[[f64; 2]], guesses: Option<&[[f64; 2]]>, o: &LkOpts) -> Vec> { + pts.par_iter() + .enumerate() + .map(|(i, p)| { + let g = guesses.and_then(|g| g.get(i).copied()).unwrap_or([0.0; 2]); + let q = lk_point(a, b, *p, g, o)?; + if o.fb_max > 0.0 { + let back = lk_point(b, a, q, [p[0] - q[0], p[1] - q[1]], o)?; + if (back[0] - p[0]).powi(2) + (back[1] - p[1]).powi(2) > o.fb_max * o.fb_max { + return None; + } + } + Some(q) + }) + .collect() +} + +/// Integer downsampling factor so the longer side is at most `max_side` pixels. +pub fn analysis_factor(w: u32, h: u32, max_side: u32) -> usize { + let long = w.max(h).max(1); + long.div_ceil(max_side.max(16)).max(1) as usize +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Value noise: smooth random blobs at two scales. + fn tex(x: f64, y: f64) -> f32 { + fn h(i: i64, j: i64) -> f64 { + let mut v = (i.wrapping_mul(73856093) ^ j.wrapping_mul(19349663)) as u64; + v ^= v >> 13; + v = v.wrapping_mul(0x5bd1e995); + v ^= v >> 15; + (v % 1000) as f64 / 1000.0 + } + fn noise(x: f64, y: f64, cell: f64) -> f64 { + let (fx, fy) = (x / cell, y / cell); + let (i, j) = (fx.floor(), fy.floor()); + let (tx, ty) = (fx - i, fy - j); + let (sx, sy) = (tx * tx * (3.0 - 2.0 * tx), ty * ty * (3.0 - 2.0 * ty)); + let (i, j) = (i as i64, j as i64); + let a = h(i, j) + (h(i + 1, j) - h(i, j)) * sx; + let b = h(i, j + 1) + (h(i + 1, j + 1) - h(i, j + 1)) * sx; + a + (b - a) * sy + } + (0.2 + 0.4 * noise(x + 1000.0, y + 1000.0, 9.0) + 0.3 * noise(x + 1000.0, y + 1000.0, 4.0)) as f32 + } + + fn img(dx: f64, dy: f64) -> Image { + let mut im = Image::new(160, 120); + for y in 0..120 { + for x in 0..160 { + let v = tex(x as f64 + 0.5 - dx, y as f64 + 0.5 - dy); + im.set(x, y, [v, v, v, 1.0]); + } + } + im + } + + #[test] + fn klt_recovers_a_shift() { + let a = GrayPyramid::from_image(&img(0.0, 0.0), [0.0; 2], 1, 3); + let b = GrayPyramid::from_image(&img(6.3, -4.2), [0.0; 2], 1, 3); + let pts = good_features(&a, &CornerOpts { max_features: 60, ..Default::default() }, None); + assert!(pts.len() >= 20, "{}", pts.len()); + let q = track(&a, &b, &pts, None, &LkOpts::default()); + let good: Vec<_> = pts.iter().zip(&q).filter_map(|(p, q)| q.map(|q| [q[0] - p[0], q[1] - p[1]])).collect(); + assert!(good.len() * 2 > pts.len(), "{} of {}", good.len(), pts.len()); + let mut errs: Vec = good.iter().map(|d| ((d[0] - 6.3).powi(2) + (d[1] + 4.2).powi(2)).sqrt()).collect(); + errs.sort_by(f64::total_cmp); + assert!(errs[errs.len() / 2] < 0.1, "median error {}", errs[errs.len() / 2]); + } + + #[test] + fn features_respect_the_region_and_spacing() { + let a = GrayPyramid::from_image(&img(0.0, 0.0), [0.0; 2], 1, 2); + let inside = |p: [f64; 2]| p[0] < 80.0; + let pts = good_features(&a, &CornerOpts { min_distance: 10.0, ..Default::default() }, Some(&inside)); + assert!(!pts.is_empty()); + assert!(pts.iter().all(|p| p[0] < 80.0)); + for (i, p) in pts.iter().enumerate() { + for q in &pts[i + 1..] { + assert!((p[0] - q[0]).hypot(p[1] - q[1]) >= 10.0 - 1e-9); + } + } + assert_eq!(analysis_factor(1920, 1080, 640), 3); + } +} diff --git a/crates/track/src/lib.rs b/crates/track/src/lib.rs index d106b209..d7b239f5 100644 --- a/crates/track/src/lib.rs +++ b/crates/track/src/lib.rs @@ -18,8 +18,12 @@ //! Coordinates are *layer* pixels: the frame image's pixel `(i, j)` covers `[i, i+1) × [j, j+1)` //! after subtracting the frame's `offset` (see [`Frame`]). +pub mod fit; +pub mod klt; +pub mod mask; pub mod plane; pub mod solve; +pub mod stabilize; use effectcraft_raster::Image; use rayon::prelude::*; diff --git a/crates/track/src/mask.rs b/crates/track/src/mask.rs new file mode 100644 index 00000000..e1e13340 --- /dev/null +++ b/crates/track/src/mask.rs @@ -0,0 +1,222 @@ +//! Mask tracking: follow the pixels inside a mask from frame to frame and report the motion as a +//! 2D transform (translation, + scale, + rotation, + skew, or perspective), which the caller +//! applies to the mask's vertices and tangents. +//! +//! Each step detects Shi–Tomasi features inside the current mask outline ([`crate::klt`]), tracks +//! them into the next frame with pyramidal Lucas–Kanade and a forward–backward check, and fits the +//! chosen model robustly with RANSAC ([`crate::fit`]), falling back to simpler models when too few +//! features survive. Only a crop around the mask (plus a margin for the motion) is analysed. + +use effectcraft_raster::Image; + +use crate::Frame; +use crate::fit::{Model, ransac}; +use crate::klt::{CornerOpts, GrayPyramid, LkOpts, analysis_factor, good_features, luma_plane, track}; +use crate::solve::Homography; + +/// Point-in-polygon (even–odd rule). +pub fn inside_polygon(poly: &[[f64; 2]], p: [f64; 2]) -> bool { + let n = poly.len(); + if n < 3 { + return false; + } + let mut c = false; + let mut j = n - 1; + for i in 0..n { + let (a, b) = (poly[i], poly[j]); + if (a[1] > p[1]) != (b[1] > p[1]) && p[0] < (b[0] - a[0]) * (p[1] - a[1]) / (b[1] - a[1]) + a[0] { + c = !c; + } + j = i; + } + c +} + +/// Bounding box `[x0, y0, x1, y1]` of points. +pub fn bounds(p: &[[f64; 2]]) -> [f64; 4] { + let mut b = [f64::MAX, f64::MAX, f64::MIN, f64::MIN]; + for q in p { + b[0] = b[0].min(q[0]); + b[1] = b[1].min(q[1]); + b[2] = b[2].max(q[0]); + b[3] = b[3].max(q[1]); + } + b +} + +/// Mask tracker state. +#[derive(Clone, Debug)] +pub struct MaskTracker { + pub model: Model, + /// The mask outline (layer pixels) on the last tracked frame. + pub outline: Vec<[f64; 2]>, + /// The last frame-to-frame motion (a constant-velocity guess for the next frame). + velocity: Homography, + prev: Option, + prev_offset: [f64; 2], + /// Features used on the last step (layer pixels), for display and tests. + pub last_features: usize, +} + +/// One mask tracking step's outcome. +#[derive(Clone, Debug)] +pub struct MaskStep { + /// Motion from the previous frame to this one (layer pixels). + pub motion: Homography, + /// Correspondences that agreed with the motion. + pub inliers: usize, + /// The model actually fitted (may be simpler than requested when features are scarce). + pub model: Model, +} + +impl MaskTracker { + /// Start on `frame` with the mask `outline` (a closed polyline in layer pixels). + pub fn new(model: Model, outline: Vec<[f64; 2]>, frame: &Frame) -> MaskTracker { + MaskTracker { model, outline, velocity: Homography::IDENTITY, prev: Some(frame.img.clone()), prev_offset: frame.offset, last_features: 0 } + } + + /// Track into `frame` (the next frame in either direction). `None` when the mask's pixels + /// can't be followed (no texture, or the mask left the frame). + pub fn step(&mut self, frame: &Frame) -> Option { + let prev = self.prev.take()?; + let res = self.step_inner(&prev, self.prev_offset, frame); + self.prev = Some(frame.img.clone()); + self.prev_offset = frame.offset; + let r = res?; + self.outline = self.outline.iter().map(|p| r.motion.apply(*p)).collect(); + self.velocity = r.motion; + Some(r) + } + + fn step_inner(&mut self, prev: &Image, prev_off: [f64; 2], frame: &Frame) -> Option { + let b = bounds(&self.outline); + if !(b[0].is_finite() && b[2] > b[0] && b[3] > b[1]) { + return None; + } + let (bw, bh) = (b[2] - b[0], b[3] - b[1]); + // Crop region (layer pixels): the mask plus room for its motion. + let margin = (0.35 * bw.max(bh)).max(32.0); + let (lx0, ly0, lx1, ly1) = (b[0] - margin, b[1] - margin, b[2] + margin, b[3] + margin); + // Both frames share one plane coordinate system: crop in the frames' image pixels. + let crop = |img: &Image, off: [f64; 2]| { + let x0 = (lx0 + off[0]).floor().max(0.0) as i64; + let y0 = (ly0 + off[1]).floor().max(0.0) as i64; + let x1 = ((lx1 + off[0]).ceil() as i64).min(img.width as i64); + let y1 = ((ly1 + off[1]).ceil() as i64).min(img.height as i64); + if x1 <= x0 + 8 || y1 <= y0 + 8 { + return None; + } + Some((img.crop(x0, y0, (x1 - x0) as u32, (y1 - y0) as u32), [off[0] - x0 as f64, off[1] - y0 as f64])) + }; + let (ca, oa) = crop(prev, prev_off)?; + let (cb, ob) = crop(frame.img, frame.offset)?; + let factor = analysis_factor(ca.width.max(cb.width), ca.height.max(cb.height), 720); + // Pyramids must share coordinates: use a common origin (layer pixels → plane). + let levels = 3; + let pa = GrayPyramid::from_plane(luma_plane(&ca, factor), oa, factor as f64, levels); + let pb_raw = luma_plane(&cb, factor); + // Shift frame b's crop into frame a's plane coordinates when the offsets differ. + let pb = if (oa[0] - ob[0]).abs() < 1e-9 && (oa[1] - ob[1]).abs() < 1e-9 && ca.width == cb.width && ca.height == cb.height { + GrayPyramid::from_plane(pb_raw, oa, factor as f64, levels) + } else { + let mut q = crate::plane::Plane::new(pa.levels[0].w, pa.levels[0].h, 1); + let mut v = [0.0f32]; + for y in 0..q.h { + for x in 0..q.w { + // plane a (x, y) → layer → plane b. + let l = pa.to_layer([x as f64 + 0.5, y as f64 + 0.5]); + let pbp = [(l[0] + ob[0]) / factor as f64, (l[1] + ob[1]) / factor as f64]; + pb_raw.sample(pbp[0], pbp[1], &mut v); + q.data[y * q.w + x] = v[0]; + } + } + GrayPyramid::from_plane(q, oa, factor as f64, levels) + }; + let outline: Vec<[f64; 2]> = self.outline.iter().map(|p| pa.to_plane(*p)).collect(); + let area = bw * bh / (factor * factor) as f64; + let md = (area / 500.0).sqrt().clamp(3.0, 12.0); + let inside = |p: [f64; 2]| inside_polygon(&outline, p); + let feats = good_features(&pa, &CornerOpts { max_features: 500, min_distance: md, quality: 0.005, window: 2, border: 3 }, Some(&inside)); + self.last_features = feats.len(); + if feats.len() < 3 { + return None; + } + // Constant-velocity guess. + let guesses: Vec<[f64; 2]> = feats + .iter() + .map(|p| { + let l = pa.to_layer(*p); + let m = self.velocity.apply(l); + [(m[0] - l[0]) / factor as f64, (m[1] - l[1]) / factor as f64] + }) + .collect(); + let tracked = track(&pa, &pb, &feats, Some(&guesses), &LkOpts { radius: 6, fb_max: 0.75, ..Default::default() }); + let (src, dst): (Vec<[f64; 2]>, Vec<[f64; 2]>) = feats.iter().zip(&tracked).filter_map(|(p, q)| q.map(|q| (pa.to_layer(*p), pa.to_layer(q)))).unzip(); + let thr = 1.5 * factor as f64; + let chain: &[Model] = match self.model { + Model::Homography => &[Model::Homography, Model::Affine, Model::Similarity, Model::Translation], + Model::Affine => &[Model::Affine, Model::Similarity, Model::Translation], + Model::Similarity => &[Model::Similarity, Model::Translation], + Model::TranslationScale => &[Model::TranslationScale, Model::Translation], + Model::Translation => &[Model::Translation], + }; + for m in chain { + // Enough support for the model's freedom. + let need = (m.min_points() * 3).max(3); + if src.len() < need { + continue; + } + if let Some(f) = ransac(*m, &src, &dst, thr, 400, 0x5eed + src.len() as u64) { + let c = f.inlier_count(); + if c >= need && c * 3 >= src.len() { + return Some(MaskStep { motion: f.h, inliers: c, model: *m }); + } + } + } + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn tex(x: f64, y: f64) -> f32 { + let v = (x * 0.37).sin() * (y * 0.29).cos() + 0.6 * ((x + 1.7 * y) * 0.21).sin() + 0.4 * ((x * 0.83 - y * 0.61).sin()); + (0.5 + 0.18 * v) as f32 + } + + /// A textured disk of radius 50 posed by `h` (object → layer) over flat grey. + fn frame(h: &Homography) -> Image { + let inv = h.inverse().unwrap(); + let mut im = Image::new(320, 240); + for y in 0..240 { + for x in 0..320 { + let o = inv.apply([x as f64 + 0.5, y as f64 + 0.5]); + let v = if o[0].hypot(o[1]) <= 50.0 { tex(o[0], o[1]) } else { 0.3 }; + im.set(x, y, [v, v, v, 1.0]); + } + } + im + } + + #[test] + fn follows_a_rotating_scaling_disk() { + let pose = |f: usize| { + let a = (f as f64 * 4.0).to_radians(); + let s = 1.0 + 0.03 * f as f64; + Homography([[s * a.cos(), -s * a.sin(), 140.0 + 4.0 * f as f64], [s * a.sin(), s * a.cos(), 110.0 + 2.0 * f as f64], [0.0, 0.0, 1.0]]) + }; + let ring: Vec<[f64; 2]> = (0..24).map(|i| (i as f64 * 15.0).to_radians()).map(|a| [40.0 * a.cos(), 40.0 * a.sin()]).collect(); + let f0 = frame(&pose(0)); + let mut mt = MaskTracker::new(Model::Similarity, ring.iter().map(|p| pose(0).apply(*p)).collect(), &Frame::new(&f0)); + for f in 1..8 { + let img = frame(&pose(f)); + let st = mt.step(&Frame::new(&img)).expect("tracked"); + assert!(st.inliers > 10); + let want: Vec<[f64; 2]> = ring.iter().map(|p| pose(f).apply(*p)).collect(); + let err = mt.outline.iter().zip(&want).map(|(a, b)| (a[0] - b[0]).hypot(a[1] - b[1])).fold(0.0, f64::max); + assert!(err < 1.5, "frame {f}: max vertex error {err}"); + } + } +} diff --git a/crates/track/src/solve.rs b/crates/track/src/solve.rs index bf3f2eb5..6c267721 100644 --- a/crates/track/src/solve.rs +++ b/crates/track/src/solve.rs @@ -47,6 +47,89 @@ impl Homography { [(m[0][0] * p[0] + m[0][1] * p[1] + m[0][2]) / w, (m[1][0] * p[0] + m[1][1] * p[1] + m[1][2]) / w] } + /// `self ∘ other` (apply `other` first). + pub fn then_after(&self, other: &Homography) -> Homography { + Homography(mul(self.0, other.0)).normalized() + } + + /// Composition `b ∘ a`: apply `a`, then `b`. + pub fn compose(a: &Homography, b: &Homography) -> Homography { + b.then_after(a) + } + + /// Scale so that `h33 = 1` (when it isn't ≈ 0). + pub fn normalized(self) -> Homography { + let s = self.0[2][2]; + if s.abs() < 1e-12 || (s - 1.0).abs() < 1e-15 { + return self; + } + Homography(self.0.map(|r| r.map(|v| v / s))) + } + + /// The inverse map (`None` when singular). + pub fn inverse(&self) -> Option { + let m = &self.0; + let c00 = m[1][1] * m[2][2] - m[1][2] * m[2][1]; + let c01 = m[1][2] * m[2][0] - m[1][0] * m[2][2]; + let c02 = m[1][0] * m[2][1] - m[1][1] * m[2][0]; + let det = m[0][0] * c00 + m[0][1] * c01 + m[0][2] * c02; + if det.abs() < 1e-18 || !det.is_finite() { + return None; + } + let inv = [ + [c00, m[0][2] * m[2][1] - m[0][1] * m[2][2], m[0][1] * m[1][2] - m[0][2] * m[1][1]], + [c01, m[0][0] * m[2][2] - m[0][2] * m[2][0], m[0][2] * m[1][0] - m[0][0] * m[1][2]], + [c02, m[0][1] * m[2][0] - m[0][0] * m[2][1], m[0][0] * m[1][1] - m[0][1] * m[1][0]], + ]; + Some(Homography(inv.map(|r| r.map(|v| v / det))).normalized()) + } + + /// Translation by `t`. + pub fn translation(t: [f64; 2]) -> Homography { + Homography([[1.0, 0.0, t[0]], [0.0, 1.0, t[1]], [0.0, 0.0, 1.0]]) + } + + /// Uniform scale `k` about `c`. + pub fn scale_about(k: f64, c: [f64; 2]) -> Homography { + Homography([[k, 0.0, c[0] * (1.0 - k)], [0.0, k, c[1] * (1.0 - k)], [0.0, 0.0, 1.0]]) + } + + /// `(1 - t) · self + t · other` element-wise (both normalised to `h33 = 1`): blends between + /// nearby transforms. + pub fn lerp(&self, other: &Homography, t: f64) -> Homography { + let (a, b) = (self.normalized().0, other.normalized().0); + let mut o = [[0.0; 3]; 3]; + for i in 0..3 { + for j in 0..3 { + o[i][j] = a[i][j] + (b[i][j] - a[i][j]) * t; + } + } + Homography(o) + } + + /// Map a Bezier tangent (relative to vertex `v`) through the transform: the tangent's end + /// point is mapped and made relative to the mapped vertex. + pub fn apply_tangent(&self, v: [f64; 2], tangent: [f64; 2]) -> [f64; 2] { + if tangent == [0.0, 0.0] { + return tangent; + } + let a = self.apply(v); + let b = self.apply([v[0] + tangent[0], v[1] + tangent[1]]); + [b[0] - a[0], b[1] - a[1]] + } + + /// Largest absolute difference of the matrices (normalised). + pub fn max_diff(&self, other: &Homography) -> f64 { + let (a, b) = (self.normalized().0, other.normalized().0); + let mut d: f64 = 0.0; + for i in 0..3 { + for j in 0..3 { + d = d.max((a[i][j] - b[i][j]).abs()); + } + } + d + } + /// The homography taking `src[i]` to `dst[i]` (four points, no three collinear). pub fn from_points(src: [[f64; 2]; 4], dst: [[f64; 2]; 4]) -> Option { // Normalise both sets (centroid at 0, mean distance √2) for conditioning. @@ -114,7 +197,7 @@ fn sub(a: [f64; 2], b: [f64; 2]) -> [f64; 2] { [a[0] - b[0], a[1] - b[1]] } -fn mul(a: [[f64; 3]; 3], b: [[f64; 3]; 3]) -> [[f64; 3]; 3] { +pub(crate) fn mul(a: [[f64; 3]; 3], b: [[f64; 3]; 3]) -> [[f64; 3]; 3] { let mut o = [[0.0; 3]; 3]; for i in 0..3 { for j in 0..3 { diff --git a/crates/track/src/stabilize.rs b/crates/track/src/stabilize.rs new file mode 100644 index 00000000..1abe79f9 --- /dev/null +++ b/crates/track/src/stabilize.rs @@ -0,0 +1,576 @@ +//! Video stabilization (the Warp Stabilizer's analysis and stabilization steps), clean-room from +//! the published approach of 2D stabilizers: +//! +//! 1. **Analysis** ([`Analyzer`]): Shi–Tomasi features are tracked between consecutive frames +//! with pyramidal Lucas–Kanade ([`crate::klt`]), and the dominant (background) motion between +//! each pair of frames is fitted robustly with RANSAC ([`crate::fit`]) for three models at +//! once: translation, similarity and homography. The result ([`WarpAnalysis`]) is small and +//! serialisable; it is stored in the effect instance. +//! 2. **Stabilization** ([`plan`]): the camera path is smoothed and per-frame corrective +//! transforms are derived. For *Smooth Motion* each frame is warped to the Gaussian-weighted +//! average of its neighbours' viewpoints — the motion-smoothing scheme of Matsushita et al., +//! "Full-Frame Video Stabilization with Motion Inpainting" (PAMI 2006) — so the low-frequency +//! camera move stays and the jitter goes; *No Motion* maps every frame onto one reference +//! frame. Framing then crops to the region valid on every frame (a centred rectangle inside +//! every warped frame outline) and optionally scales it up to fill the frame; when that would +//! exceed Maximum Scale, the correction is relaxed towards identity on the offending frames — +//! trading smoothness for less crop, the constraint idea of Grundmann et al., "Auto-Directed +//! Video Stabilization with Robust L1 Optimal Camera Paths" (CVPR 2011), applied here +//! per frame rather than through a linear program. +//! +//! Coordinates are layer pixels; transforms map *source* frame pixels to *stabilized* pixels. + +use serde::{Deserialize, Serialize}; + +use crate::Frame; +use crate::fit::{Model, ransac}; +use crate::klt::{CornerOpts, GrayPyramid, LkOpts, analysis_factor, good_features, track}; +use crate::solve::Homography; + +/// Inter-frame motion of one frame (from the previous frame to this one) for each model. +#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct FrameMotion { + /// Translation `[tx, ty]`. + pub t: [f64; 2], + /// Similarity `[a, b, tx, ty]` (`x' = a x − b y + tx`, `y' = b x + a y + ty`). + pub s: [f64; 4], + /// Homography, row-major without `h33 = 1`. + pub h: [f64; 8], + /// Correspondences that agreed with the homography / tracked in total. + pub inliers: u32, + pub features: u32, + /// A subset of this frame's tracked features (layer pixels), for Show Track Points. + #[serde(skip_serializing_if = "Vec::is_empty")] + pub points: Vec<[f32; 2]>, +} + +impl Default for FrameMotion { + fn default() -> Self { + FrameMotion { t: [0.0; 2], s: [1.0, 0.0, 0.0, 0.0], h: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], inliers: 0, features: 0, points: vec![] } + } +} + +impl FrameMotion { + /// The motion under a stabilization method. + pub fn motion(&self, method: Method) -> Homography { + match method { + Method::Position => Homography::translation(self.t), + Method::Similarity => { + let s = self.s; + Homography([[s[0], -s[1], s[2]], [s[1], s[0], s[3]], [0.0, 0.0, 1.0]]) + } + Method::Perspective | Method::SubspaceWarp => { + let h = self.h; + Homography([[h[0], h[1], h[2]], [h[3], h[4], h[5]], [h[6], h[7], 1.0]]) + } + } + } +} + +/// The stored analysis of a clip. +#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct WarpAnalysis { + pub version: u32, + /// Layer time (seconds) of frame 0, and the frame duration. + pub start: f64, + pub frame_duration: f64, + /// Layer size (pixels). + pub size: [f64; 2], + /// Detailed Analysis was on. + pub detailed: bool, + /// Per frame; frame 0's motion is the identity. + pub frames: Vec, +} + +impl WarpAnalysis { + pub fn is_empty(&self) -> bool { + self.frames.is_empty() + } + /// Frame index at a layer time (held at the ends). + pub fn frame_at(&self, layer_time: f64) -> Option { + if self.frames.is_empty() || self.frame_duration <= 0.0 { + return None; + } + let i = ((layer_time - self.start) / self.frame_duration).round(); + Some(i.clamp(0.0, (self.frames.len() - 1) as f64) as usize) + } + pub fn to_json(&self) -> String { + serde_json::to_string(self).unwrap_or_default() + } + pub fn from_json(s: &str) -> Option { + if s.trim().is_empty() { + return None; + } + serde_json::from_str(s).ok() + } +} + +/// Analysis options. +#[derive(Clone, Copy, Debug, Default)] +pub struct AnalyzeOpts { + /// Detailed Analysis: twice the resolution and more features. + pub detailed: bool, +} + +/// Incremental analysis: push the clip's frames in order. +pub struct Analyzer { + opts: AnalyzeOpts, + prev: Option, + frames: Vec, + size: [f64; 2], +} + +fn round_pts(p: &[[f64; 2]], n: usize) -> Vec<[f32; 2]> { + let step = (p.len() / n.max(1)).max(1); + p.iter().step_by(step).take(n).map(|q| [((q[0] * 10.0).round() / 10.0) as f32, ((q[1] * 10.0).round() / 10.0) as f32]).collect() +} + +impl Analyzer { + pub fn new(size: [f64; 2], opts: AnalyzeOpts) -> Analyzer { + Analyzer { opts, prev: None, frames: vec![], size } + } + + pub fn len(&self) -> usize { + self.frames.len() + } + pub fn is_empty(&self) -> bool { + self.frames.is_empty() + } + + /// Analyse the next frame. + pub fn push(&mut self, frame: &Frame) { + let (max_side, max_feat) = if self.opts.detailed { (960, 700) } else { (480, 350) }; + let factor = analysis_factor(frame.img.width, frame.img.height, max_side); + let pyr = GrayPyramid::from_image(frame.img, frame.offset, factor, 3); + let mut fm = FrameMotion::default(); + if let Some(prev) = &self.prev + && prev.width() == pyr.width() + && prev.height() == pyr.height() + && prev.offset == pyr.offset + { + let md = (prev.width().max(prev.height()) as f64 / 40.0).clamp(4.0, 16.0); + let feats = good_features(prev, &CornerOpts { max_features: max_feat, min_distance: md, quality: 0.01, window: 2, border: 4 }, None); + let q = track(prev, &pyr, &feats, None, &LkOpts { radius: 5, fb_max: 0.75, ..Default::default() }); + let (src, dst): (Vec<[f64; 2]>, Vec<[f64; 2]>) = feats.iter().zip(&q).filter_map(|(p, q)| q.map(|q| (prev.to_layer(*p), pyr.to_layer(q)))).unzip(); + fm.features = src.len() as u32; + let thr = 1.0 * factor as f64; + let seed = self.frames.len() as u64 + 1; + if let Some(f) = ransac(Model::Translation, &src, &dst, thr * 1.5, 200, seed) { + let m = f.h.0; + fm.t = [m[0][2], m[1][2]]; + } + if let Some(f) = ransac(Model::Similarity, &src, &dst, thr * 1.5, 300, seed) { + let m = f.h.0; + fm.s = [m[0][0], m[1][0], m[0][2], m[1][2]]; + } + if let Some(f) = ransac(Model::Homography, &src, &dst, thr, 500, seed) { + let m = f.h.normalized().0; + // Reject wild projective fits (few features): keep the similarity then. + let sane = m[2][0].abs() < 0.01 && m[2][1].abs() < 0.01; + fm.h = if sane { + [m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1]] + } else { + [fm.s[0], -fm.s[1], fm.s[2], fm.s[1], fm.s[0], fm.s[3], 0.0, 0.0] + }; + fm.inliers = f.inlier_count() as u32; + let inl: Vec<[f64; 2]> = dst.iter().zip(&f.inliers).filter(|(_, b)| **b).map(|(p, _)| *p).collect(); + fm.points = round_pts(&inl, 48); + } + } + self.frames.push(fm); + self.prev = Some(pyr); + } + + pub fn finish(self, start: f64, frame_duration: f64) -> WarpAnalysis { + WarpAnalysis { version: 1, start, frame_duration, size: self.size, detailed: self.opts.detailed, frames: self.frames } + } +} + +// ---------------------------------------------------------------- stabilization + +/// Stabilization ▸ Result. +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] +pub enum StabResult { + #[default] + SmoothMotion, + NoMotion, +} + +/// Stabilization ▸ Method. +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] +pub enum Method { + Position, + /// Position, Scale, Rotation. + Similarity, + Perspective, + /// Approximated by Perspective (see the module docs of the effect). + #[default] + SubspaceWarp, +} + +/// Borders ▸ Framing. +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] +pub enum Framing { + StabilizeOnly, + StabilizeCrop, + #[default] + StabilizeCropAutoScale, + SynthesizeEdges, +} + +/// Effect parameters that shape the stabilization. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct StabSettings { + pub result: StabResult, + /// Smoothness, percent (50 = default). + pub smoothness: f64, + pub method: Method, + pub preserve_scale: bool, + pub framing: Framing, + /// Maximum Scale, percent. + pub max_scale: f64, + /// Action Safe Margin, percent of the frame on each side. + pub action_safe: f64, + /// Additional Scale, percent. + pub additional_scale: f64, + /// Crop Less ↔ Smooth More (−100…100). + pub crop_less_smooth_more: f64, + /// Frames per second (for Smoothness). + pub fps: f64, +} + +impl Default for StabSettings { + fn default() -> Self { + StabSettings { + result: StabResult::SmoothMotion, + smoothness: 50.0, + method: Method::SubspaceWarp, + preserve_scale: false, + framing: Framing::StabilizeCropAutoScale, + max_scale: 150.0, + action_safe: 0.0, + additional_scale: 100.0, + crop_less_smooth_more: 0.0, + fps: 30.0, + } + } +} + +impl StabSettings { + /// Gaussian smoothing sigma in frames. + pub fn sigma(&self) -> f64 { + let base = self.smoothness.max(0.0) / 100.0 * self.fps.max(1.0) * 0.6; + base * 2f64.powf(self.crop_less_smooth_more.clamp(-100.0, 100.0) / 100.0) + } +} + +/// The per-frame stabilization of a clip. +#[derive(Clone, Debug, Default)] +pub struct Plan { + /// Final source → output transform per frame (layer pixels), framing scale included. + pub warps: Vec, + /// The visible region in output pixels `[x0, y0, x1, y1]` (Stabilize, Crop / Auto-scale); + /// `None` = everything the warp produces. + pub crop: Option<[f64; 4]>, + /// Auto-scale factor (1 = none). + pub auto_scale: f64, + /// Fraction of the frame (per side length) valid on every frame before scaling. + pub valid_fraction: f64, +} + +fn convex_contains(q: &[[f64; 2]; 4], p: [f64; 2]) -> bool { + let mut sign = 0.0; + for i in 0..4 { + let (a, b) = (q[i], q[(i + 1) % 4]); + let c = (b[0] - a[0]) * (p[1] - a[1]) - (b[1] - a[1]) * (p[0] - a[0]); + if c.abs() < 1e-12 { + continue; + } + if sign == 0.0 { + sign = c.signum(); + } else if c.signum() != sign { + return false; + } + } + true +} + +/// Largest `s ∈ [0, 1]` such that the centred `s·W × s·H` rectangle lies inside the frame +/// outline mapped by `b`. +pub fn valid_fraction(b: &Homography, size: [f64; 2]) -> f64 { + let [w, h] = size; + let quad = [b.apply([0.0, 0.0]), b.apply([w, 0.0]), b.apply([w, h]), b.apply([0.0, h])]; + // A fold or a point at infinity: nothing is valid. + let m = b.normalized().0; + for p in [[0.0, 0.0], [w, 0.0], [w, h], [0.0, h]] { + if m[2][0] * p[0] + m[2][1] * p[1] + m[2][2] <= 1e-9 { + return 0.0; + } + } + let c = [w / 2.0, h / 2.0]; + let fits = |s: f64| { + let (hw, hh) = (s * w / 2.0, s * h / 2.0); + [[c[0] - hw, c[1] - hh], [c[0] + hw, c[1] - hh], [c[0] + hw, c[1] + hh], [c[0] - hw, c[1] + hh]].iter().all(|p| convex_contains(&quad, *p)) + }; + if fits(1.0) { + return 1.0; + } + if !fits(0.0) { + return 0.0; + } + let (mut lo, mut hi) = (0.0, 1.0); + for _ in 0..30 { + let mid = 0.5 * (lo + hi); + if fits(mid) { + lo = mid; + } else { + hi = mid; + } + } + lo +} + +fn remove_scale(b: &Homography, c: [f64; 2]) -> Homography { + // Local scale at the frame centre from the Jacobian of the map. + let e = 1.0; + let p0 = b.apply(c); + let px = b.apply([c[0] + e, c[1]]); + let py = b.apply([c[0], c[1] + e]); + let det = ((px[0] - p0[0]) * (py[1] - p0[1]) - (px[1] - p0[1]) * (py[0] - p0[0])).abs(); + let s = det.sqrt(); + if s < 1e-9 { + return *b; + } + Homography::scale_about(1.0 / s, p0).then_after(b) +} + +/// Corrective transforms (source → stabilized, before framing) for every frame. +pub fn corrections(a: &WarpAnalysis, s: &StabSettings) -> Vec { + let n = a.frames.len(); + if n == 0 { + return vec![]; + } + let method = s.method; + let inter: Vec = a.frames.iter().map(|f| f.motion(method)).collect(); + let inv: Vec = inter.iter().map(|h| h.inverse().unwrap_or(Homography::IDENTITY)).collect(); + let c = [a.size[0] / 2.0, a.size[1] / 2.0]; + let mut out = Vec::with_capacity(n); + match s.result { + StabResult::NoMotion => { + // Cumulative path C_t (frame 0 → frame t); every frame maps onto the middle frame. + let mut cum = Vec::with_capacity(n); + let mut cur = Homography::IDENTITY; + for (k, h) in inter.iter().enumerate() { + if k > 0 { + cur = h.then_after(&cur); + } + cum.push(cur); + } + let r = n / 2; + for c_t in &cum { + let ci = c_t.inverse().unwrap_or(Homography::IDENTITY); + out.push(cum[r].then_after(&ci)); + } + } + StabResult::SmoothMotion => { + let sigma = s.sigma(); + let rad = ((3.0 * sigma).ceil() as usize).min(n.saturating_sub(1)); + for t in 0..n { + if sigma < 0.3 || rad == 0 { + out.push(Homography::IDENTITY); + continue; + } + let mut acc = [[0.0; 3]; 3]; + let mut wsum = 0.0; + let mut add = |h: &Homography, d: f64| { + let w = (-(d * d) / (2.0 * sigma * sigma)).exp(); + let m = h.normalized().0; + for i in 0..3 { + for j in 0..3 { + acc[i][j] += w * m[i][j]; + } + } + wsum += w; + }; + add(&Homography::IDENTITY, 0.0); + // Forward: T_t^{i+1} = inter[i+1] ∘ T_t^i. + let mut tf = Homography::IDENTITY; + for i in t + 1..=(t + rad).min(n - 1) { + tf = inter[i].then_after(&tf); + add(&tf, (i - t) as f64); + } + // Backward: T_t^{i-1} = inter[i]⁻¹ ∘ T_t^i. + let mut tb = Homography::IDENTITY; + for i in (t.saturating_sub(rad)..t).rev() { + tb = inv[i + 1].then_after(&tb); + add(&tb, (t - i) as f64); + } + let m = acc.map(|r| r.map(|v| v / wsum)); + out.push(Homography(m).normalized()); + } + } + } + if s.preserve_scale && method != Method::Position { + out = out.iter().map(|b| remove_scale(b, c)).collect(); + } + out +} + +/// Stabilization plan: corrections plus framing. +pub fn plan(a: &WarpAnalysis, s: &StabSettings) -> Plan { + let mut b = corrections(a, s); + let size = a.size; + let c = [size[0] / 2.0, size[1] / 2.0]; + let add = (s.additional_scale / 100.0).max(0.01); + let z_add = Homography::scale_about(add, c); + let fracs = |b: &[Homography]| b.iter().map(|h| valid_fraction(h, size)).collect::>(); + let mut fr = fracs(&b); + let mut vmin = fr.iter().copied().fold(1.0, f64::min); + let margin = (s.action_safe / 100.0).clamp(0.0, 0.45); + let need_cover = 1.0 - 2.0 * margin; + let max_scale = (s.max_scale / 100.0).max(1.0); + if s.framing == Framing::StabilizeCropAutoScale && vmin * max_scale < need_cover && !b.is_empty() { + // Relax the correction on frames that need more than Maximum Scale. + let target = need_cover / max_scale; + let n = b.len(); + let mut lam = vec![1.0f64; n]; + for t in 0..n { + if fr[t] >= target { + continue; + } + let (mut lo, mut hi) = (0.0, 1.0); + for _ in 0..24 { + let mid = 0.5 * (lo + hi); + if valid_fraction(&Homography::IDENTITY.lerp(&b[t], mid), size) >= target { + lo = mid; + } else { + hi = mid; + } + } + lam[t] = lo; + } + // Spread the relaxation smoothly (min filter, then a Gaussian of the same radius) so + // the camera doesn't jump. + let r = ((s.sigma() * 0.5).ceil() as usize).max(1); + let minf: Vec = (0..n).map(|t| lam[t.saturating_sub(r)..=(t + r).min(n - 1)].iter().copied().fold(1.0, f64::min)).collect(); + let sig = r as f64 / 2.0; + let smooth: Vec = (0..n) + .map(|t| { + let (mut acc, mut ws) = (0.0, 0.0); + for i in t.saturating_sub(r)..=(t + r).min(n - 1) { + let d = i as f64 - t as f64; + let w = (-(d * d) / (2.0 * sig * sig)).exp(); + acc += w * minf[i]; + ws += w; + } + (acc / ws).min(lam[t]) + }) + .collect(); + b = b.iter().zip(&smooth).map(|(h, l)| if *l >= 1.0 { *h } else { Homography::IDENTITY.lerp(h, *l) }).collect(); + fr = fracs(&b); + vmin = fr.iter().copied().fold(1.0, f64::min); + } + let rect = |k: f64| [c[0] - k * size[0] / 2.0, c[1] - k * size[1] / 2.0, c[0] + k * size[0] / 2.0, c[1] + k * size[1] / 2.0]; + let (auto, crop) = match s.framing { + Framing::StabilizeOnly | Framing::SynthesizeEdges => (1.0, None), + Framing::StabilizeCrop => (1.0, Some(rect(vmin * add))), + Framing::StabilizeCropAutoScale => { + let k = if vmin > 1e-6 { (need_cover / vmin).clamp(1.0, max_scale) } else { max_scale }; + let vis = vmin * k * add; + (k, if vis >= 1.0 - 1e-9 { None } else { Some(rect(vis)) }) + } + }; + let z = Homography::scale_about(auto, c); + let warps = b.iter().map(|h| z_add.then_after(&z.then_after(h))).collect(); + Plan { warps, crop, auto_scale: auto, valid_fraction: vmin } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// A synthetic analysis: a slow pan plus per-frame jitter. + fn shaky(n: usize) -> (WarpAnalysis, Vec<[f64; 2]>) { + let pos = |k: usize| { + let j = [((k * 7919) % 13) as f64 - 6.0, ((k * 104729) % 11) as f64 - 5.0]; + [2.0 * k as f64 + j[0], 0.5 * k as f64 + j[1]] + }; + let mut a = WarpAnalysis { version: 1, start: 0.0, frame_duration: 1.0 / 30.0, size: [640.0, 360.0], detailed: false, frames: vec![] }; + let path: Vec<[f64; 2]> = (0..n).map(pos).collect(); + for k in 0..n { + let mut f = FrameMotion::default(); + if k > 0 { + // Scene points move opposite to the camera. + let d = [path[k - 1][0] - path[k][0], path[k - 1][1] - path[k][1]]; + f.t = d; + f.s = [1.0, 0.0, d[0], d[1]]; + f.h = [1.0, 0.0, d[0], 0.0, 1.0, d[1], 0.0, 0.0]; + } + a.frames.push(f); + } + (a, path) + } + + fn jitter(p: &[[f64; 2]]) -> f64 { + // Mean magnitude of the second difference (acceleration). + let mut s = 0.0; + for k in 1..p.len() - 1 { + s += (p[k + 1][0] - 2.0 * p[k][0] + p[k - 1][0]).hypot(p[k + 1][1] - 2.0 * p[k][1] + p[k - 1][1]); + } + s / (p.len() - 2) as f64 + } + + #[test] + fn smoothing_removes_jitter_and_no_motion_locks() { + let (a, path) = shaky(90); + // A scene point at (320, 180) on frame 0 appears at (320, 180) - path[k] on frame k. + let scene = |k: usize| [320.0 - path[k][0] + path[0][0], 180.0 - path[k][1] + path[0][1]]; + let before: Vec<[f64; 2]> = (0..90).map(scene).collect(); + for method in [Method::Position, Method::Similarity, Method::Perspective] { + let s = StabSettings { method, framing: Framing::StabilizeOnly, ..Default::default() }; + let p = plan(&a, &s); + let after: Vec<[f64; 2]> = (0..90).map(|k| p.warps[k].apply(scene(k))).collect(); + let (j0, j1) = (jitter(&before), jitter(&after)); + assert!(j1 < 0.2 * j0, "{method:?}: jitter {j0} → {j1}"); + } + let s = StabSettings { result: StabResult::NoMotion, framing: Framing::StabilizeOnly, ..Default::default() }; + let p = plan(&a, &s); + let fixed: Vec<[f64; 2]> = (0..90).map(|k| p.warps[k].apply(scene(k))).collect(); + for q in &fixed { + assert!((q[0] - fixed[45][0]).abs() < 1e-6 && (q[1] - fixed[45][1]).abs() < 1e-6); + } + } + + #[test] + fn framing_crops_and_scales() { + let (a, _) = shaky(60); + let p = plan(&a, &StabSettings { framing: Framing::StabilizeCrop, ..Default::default() }); + let crop = p.crop.unwrap(); + assert!(p.auto_scale == 1.0 && crop[0] > 0.0 && crop[2] < 640.0); + // Every frame covers the crop rectangle. + for w in &p.warps { + let inv = w.inverse().unwrap(); + for q in [[crop[0], crop[1]], [crop[2], crop[1]], [crop[2], crop[3]], [crop[0], crop[3]]] { + let s = inv.apply(q); + assert!(s[0] >= -1e-6 && s[1] >= -1e-6 && s[0] <= 640.0 + 1e-6 && s[1] <= 360.0 + 1e-6, "{s:?}"); + } + } + let p = plan(&a, &StabSettings::default()); + assert!(p.auto_scale > 1.0 && p.crop.is_none(), "{} {:?}", p.auto_scale, p.crop); + for w in &p.warps { + let inv = w.inverse().unwrap(); + for q in [[0.0, 0.0], [640.0, 0.0], [640.0, 360.0], [0.0, 360.0]] { + let s = inv.apply(q); + assert!(s[0] >= -1e-3 && s[1] >= -1e-3 && s[0] <= 640.001 && s[1] <= 360.001, "{s:?}"); + } + } + // A tight Maximum Scale relaxes the correction instead of showing borders. + let p = plan(&a, &StabSettings { max_scale: 101.0, ..Default::default() }); + assert!(p.auto_scale <= 1.01 + 1e-9); + assert!(p.valid_fraction * p.auto_scale >= 0.999 || p.crop.is_some()); + let j = WarpAnalysis::from_json(&a.to_json()).unwrap(); + assert_eq!(j, a); + } +}