From 0bce3f2e4938eec54ef0036ef5c8c3a36c542649 Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Sat, 3 Oct 2026 04:08:23 -0400 Subject: [PATCH] M13.3: Subspace Warp content-preserving mesh warp, face tracker library Co-Authored-By: Claude Opus 5.5 --- crates/effects/src/warp_stab.rs | 40 +- crates/track/src/camtrack/features.rs | 13 +- crates/track/src/face.rs | 1142 +++++++++++++++++++++++++ crates/track/src/lib.rs | 2 + crates/track/src/solve.rs | 6 + crates/track/src/stabilize.rs | 358 +++++++- crates/track/src/subspace.rs | 447 ++++++++++ 7 files changed, 1971 insertions(+), 37 deletions(-) create mode 100644 crates/track/src/face.rs create mode 100644 crates/track/src/subspace.rs diff --git a/crates/effects/src/warp_stab.rs b/crates/effects/src/warp_stab.rs index c0bbced8..ff535c41 100644 --- a/crates/effects/src/warp_stab.rs +++ b/crates/effects/src/warp_stab.rs @@ -17,8 +17,11 @@ //! Edge Cropping ignores bad edge pixels of the input frames; //! - **Show Track Points** draws the analysed background features. //! -//! *Subspace Warp* is approximated by a per-frame perspective warp (no content-preserving -//! mesh warp yet), and *Rolling Shutter Ripple* is stored but has no effect. +//! *Subspace Warp* warps each frame with a content-preserving mesh fitted to the analysis' +//! subspace-smoothed feature trajectories ([`effectcraft_track::subspace`]); analyses made +//! before trajectories were stored fall back to a perspective warp. *Rolling Shutter Ripple* +//! picks the mesh: Automatic Reduction (12 × 8, stiffer) or Enhanced Reduction (twice the rows, +//! softer), which follows the row-wise wobble of rolling-shutter shake more closely. //! //! Without an analysis (or with one made for another layer size) the input passes through. @@ -30,7 +33,7 @@ use effectcraft_keyframe::Value; use effectcraft_project::ParamUi; use effectcraft_raster::{Image, Px}; use effectcraft_track::Homography; -use effectcraft_track::stabilize::{Framing, Method, Plan, StabResult, StabSettings, WarpAnalysis, plan}; +use effectcraft_track::stabilize::{Framing, Method, Plan, Ripple, StabResult, StabSettings, WarpAnalysis, plan}; use rayon::prelude::*; use crate::{Buf, EffectCtx, EffectSpec, Params, num, p, popup, slider}; @@ -145,6 +148,7 @@ pub fn settings(params: &Params, fps: f64) -> StabSettings { // AE's 0…100 slider (50 = balanced) onto the planner's −100…100. crop_less_smooth_more: (f(params, "advanced/cropLessSmoothMore") - 50.0) * 2.0, fps, + ripple: if e(params, "advanced/rollingShutterRipple") == 1 { Ripple::Enhanced } else { Ripple::Automatic }, } } @@ -175,7 +179,7 @@ fn settings_key(s: &StabSettings) -> u64 { for v in [s.smoothness, s.max_scale, s.action_safe, s.additional_scale, s.crop_less_smooth_more, s.fps] { h.write_u64(v.to_bits()); } - h.write(format!("{:?}{:?}{:?}{:?}", s.result, s.method, s.preserve_scale, s.framing).as_bytes()); + h.write(format!("{:?}{:?}{:?}{:?}{:?}", s.result, s.method, s.preserve_scale, s.framing, s.ripple).as_bytes()); h.finish() } @@ -275,6 +279,13 @@ fn render(ctx: &EffectCtx, buf: Buf) -> Buf { } let sc = if buf.scale > 0.0 { buf.scale } else { 1.0 }; let Some(inv) = pl.warps[k].inverse() else { return buf }; + // Output point → source point of this frame (a mesh lookup for Subspace Warp). + let mesh = pl.meshes.get(k); + let finv = pl.framing.inverse().unwrap_or(Homography::IDENTITY); + let src_of = |p: [f64; 2]| match mesh { + Some(m) => m.sample(finv.apply(p)), + None => inv.apply(p), + }; let (ow, oh) = (((size[0] * sc).ceil() as u32).max(1), ((size[1] * sc).ceil() as u32).max(1)); let synth = matches!(settings(ctx.params, 30.0).framing, Framing::SynthesizeEdges); let crop = [ @@ -306,7 +317,7 @@ fn render(ctx: &EffectCtx, buf: Buf) -> Buf { { continue; } - let q = inv.apply(p); + let q = src_of(p); if !synth { *px = sample(src, q); continue; @@ -337,12 +348,12 @@ fn render(ctx: &EffectCtx, buf: Buf) -> Buf { }); let need = need_m.into_inner().unwrap_or_else(|e| e.into_inner()); if synth && need.iter().any(|w| *w > 0.0) { - synthesize(ctx, &a, k, &inv, &crop, sc, &mut out, &need); + synthesize(ctx, &a, k, &src_of, &crop, sc, &mut out, &need); } if b(ctx.params, "advanced/showTrackPoints") { let r = (2.5 * f(ctx.params, "advanced/trackPointSize") / 100.0 * sc).max(0.5); for q in &a.frames[k].points { - let o = pl.warps[k].apply([q[0] as f64, q[1] as f64]); + let Some(o) = pl.output_of(k, [q[0] as f64, q[1] as f64]) else { continue }; dot(&mut out, [o[0] * sc, o[1] * sc], r, [0.25, 0.75, 1.0, 1.0]); } } @@ -352,7 +363,16 @@ fn render(ctx: &EffectCtx, buf: Buf) -> Buf { /// Fill the pixels flagged in `need` (weight = how much synthesized content they take) from /// neighbouring frames, nearest first. #[allow(clippy::too_many_arguments)] -fn synthesize(ctx: &EffectCtx, a: &WarpAnalysis, k: usize, inv: &Homography, crop: &[f64; 4], sc: f64, out: &mut Image, need: &[f32]) { +fn synthesize( + ctx: &EffectCtx, + a: &WarpAnalysis, + k: usize, + src_of: &(dyn Fn([f64; 2]) -> [f64; 2] + Sync), + crop: &[f64; 4], + sc: f64, + out: &mut Image, + need: &[f32], +) { let Some(host) = ctx.env.host else { return }; let n = a.frames.len(); let range = ((f(ctx.params, "advanced/synthesizeInputRange").max(0.0) / a.frame_duration.max(1e-6)).round() as usize).min(60); @@ -376,7 +396,7 @@ fn synthesize(ctx: &EffectCtx, a: &WarpAnalysis, k: usize, inv: &Homography, cro .filter(|i| !filled[**i]) .filter_map(|i| { let (x, y) = (i % ow, i / ow); - let q = inv.apply([(x as f64 + 0.5) / sc, (y as f64 + 0.5) / sc]); + let q = src_of([(x as f64 + 0.5) / sc, (y as f64 + 0.5) / sc]); let qj = m.apply(q); inside(crop, qj).then(|| (*i, nb.img.sample_bilinear_clamped(qj[0] * nb.scale + nb.offset[0], qj[1] * nb.scale + nb.offset[1]))) }) @@ -412,7 +432,7 @@ mod tests { use effectcraft_track::stabilize::FrameMotion; fn analysis_json(n: usize, w: f64, h: f64, shift: impl Fn(usize) -> [f64; 2]) -> String { - let mut a = WarpAnalysis { version: 1, start: 0.0, frame_duration: 1.0 / 25.0, size: [w, h], detailed: false, frames: vec![] }; + let mut a = WarpAnalysis { version: 1, start: 0.0, frame_duration: 1.0 / 25.0, size: [w, h], detailed: false, frames: vec![], tracks: vec![] }; for k in 0..n { let mut fm = FrameMotion::default(); if k > 0 { diff --git a/crates/track/src/camtrack/features.rs b/crates/track/src/camtrack/features.rs index 30b019e5..1775a304 100644 --- a/crates/track/src/camtrack/features.rs +++ b/crates/track/src/camtrack/features.rs @@ -28,11 +28,19 @@ pub struct TrackAnalyzer { frame: u32, size: [f64; 2], factor: f64, + /// (analysis long side, wanted features) overriding the options' defaults. + budget: Option<(u32, usize)>, } impl TrackAnalyzer { pub fn new(size: [f64; 2], opts: AnalyzeOpts) -> TrackAnalyzer { - TrackAnalyzer { opts, prev: None, active: vec![], tracks: vec![], frame: 0, size, factor: 1.0 } + TrackAnalyzer { opts, prev: None, active: vec![], tracks: vec![], frame: 0, size, factor: 1.0, budget: None } + } + + /// An analyzer working at most at `max_side` pixels with about `want` live features (the + /// Warp Stabilizer's trajectories for Subspace Warp). + pub fn with_budget(size: [f64; 2], max_side: u32, want: usize) -> TrackAnalyzer { + TrackAnalyzer { budget: Some((max_side, want.max(8))), ..TrackAnalyzer::new(size, AnalyzeOpts::default()) } } pub fn frames(&self) -> u32 { @@ -45,6 +53,9 @@ impl TrackAnalyzer { } fn params(&self) -> (u32, usize) { + if let Some(b) = self.budget { + return b; + } if self.opts.detailed { (1280, 900) } else { (720, 500) } } diff --git a/crates/track/src/face.rs b/crates/track/src/face.rs new file mode 100644 index 00000000..78344a14 --- /dev/null +++ b/crates/track/src/face.rs @@ -0,0 +1,1142 @@ +//! Face tracking (Tracker panel ▸ Method ▸ Face Tracking (Outline Only) / (Detailed Features)), +//! a classical pipeline with no learned weights: +//! +//! 1. **Skin model**: on the first frame, the colour of the middle of the mask (luma and +//! normalised r, g chromaticity: median and median absolute deviation) defines skin; every +//! later frame is classified against it inside a search window around the previous face. +//! 2. **Face region and outline**: the skin component containing the face centre is flood-filled; +//! its outer boundary is found along 48 rays from its centroid (so the non-skin holes of eyes, +//! brows, nostrils and mouth don't matter), median-smoothed, and an ellipse is fitted to the +//! outline polygon from its area moments (centre, half-height, roll). The outline is what +//! *Outline Only* keys into the mask. +//! 3. **Facial features**: the non-skin connected components inside the face are matched to the +//! eyes, brows, nostrils and mouth by their position in the face frame (predicted from the +//! previous frame); landmark points are measured on each component (extremes along the face +//! axes for corners and lids, a darkness-weighted centroid for the pupil). Chin and jaw points +//! come from the outline. +//! 4. **Shape model**: an active-shape point distribution model (Cootes, Taylor, Cooper & Graham, +//! "Active Shape Models — Their Training and Application", CVIU 61(1), 1995) regularises the +//! landmarks: a weighted similarity alignment and shape-parameter fit (parameters clamped to +//! ±3 standard deviations) fills landmarks whose feature was not found (a closed eye, a +//! merged component) and replaces detections that disagree with the model. The model is +//! *trained on synthetic shapes* produced by our own parametric face generator +//! ([`synth_shape`]: eye and mouth openness, mouth width, brow raise, head yaw and pitch) by +//! principal component analysis — no external data or weights. +//! +//! Coordinates are layer pixels; the face frame is centred on the face ellipse, with the +//! half-height as unit, `u` right and `v` down (image-left is "Left"). + +use std::sync::OnceLock; + +use effectcraft_raster::Image; + +use crate::Frame; +use crate::camtrack::linalg::{cholesky_solve, sym_eigen}; + +/// Landmarks: (id, display name, canonical position in the face frame). +pub const LANDMARKS: [(&str, &str, [f64; 2]); 26] = [ + ("leftEyebrowInner", "Left Eyebrow Inner", [-0.16, -0.44]), + ("leftEyebrowMiddle", "Left Eyebrow Middle", [-0.32, -0.50]), + ("leftEyebrowOuter", "Left Eyebrow Outer", [-0.48, -0.44]), + ("rightEyebrowInner", "Right Eyebrow Inner", [0.16, -0.44]), + ("rightEyebrowMiddle", "Right Eyebrow Middle", [0.32, -0.50]), + ("rightEyebrowOuter", "Right Eyebrow Outer", [0.48, -0.44]), + ("leftEyeInner", "Left Eye Inner", [-0.17, -0.22]), + ("leftEyeOuter", "Left Eye Outer", [-0.45, -0.22]), + ("leftEyeTop", "Left Eye Top", [-0.31, -0.292]), + ("leftEyeBottom", "Left Eye Bottom", [-0.31, -0.148]), + ("leftPupil", "Left Pupil", [-0.31, -0.22]), + ("rightEyeInner", "Right Eye Inner", [0.17, -0.22]), + ("rightEyeOuter", "Right Eye Outer", [0.45, -0.22]), + ("rightEyeTop", "Right Eye Top", [0.31, -0.292]), + ("rightEyeBottom", "Right Eye Bottom", [0.31, -0.148]), + ("rightPupil", "Right Pupil", [0.31, -0.22]), + ("noseTip", "Nose Tip", [0.0, 0.14]), + ("leftNostril", "Left Nostril", [-0.08, 0.20]), + ("rightNostril", "Right Nostril", [0.08, 0.20]), + ("mouthLeft", "Mouth Left", [-0.24, 0.50]), + ("mouthRight", "Mouth Right", [0.24, 0.50]), + ("mouthTop", "Mouth Top", [0.0, 0.435]), + ("mouthBottom", "Mouth Bottom", [0.0, 0.565]), + ("chin", "Chin", [0.0, 1.0]), + ("leftJaw", "Left Jaw", [-0.614, 0.574]), + ("rightJaw", "Right Jaw", [0.614, 0.574]), +]; + +/// Number of landmarks. +pub const N: usize = LANDMARKS.len(); + +/// Landmark index by id. +pub fn landmark(id: &str) -> Option { + LANDMARKS.iter().position(|l| l.0 == id) +} + +const BROW: [[usize; 3]; 2] = [[0, 1, 2], [3, 4, 5]]; +/// Eye landmarks: inner, outer, top, bottom, pupil. +const EYE: [[usize; 5]; 2] = [[6, 7, 8, 9, 10], [11, 12, 13, 14, 15]]; +const NOSE_TIP: usize = 16; +const NOSTRIL: [usize; 2] = [17, 18]; +/// Mouth: left, right, top, bottom. +const MOUTH: [usize; 4] = [19, 20, 21, 22]; +const CHIN: usize = 23; +const JAW: [usize; 2] = [24, 25]; +/// Half-width of the face relative to its half-height. +const ASPECT: f64 = 0.75; + +// ---------------------------------------------------------------- synthetic faces + +/// Parameters of a synthetic face (the shape model's training data and the tests' footage). +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct FaceParams { + /// Face centre (layer pixels) and half-height (pixels). + pub center: [f64; 2], + pub height: f64, + /// Head rotation in degrees: roll (in the image plane), yaw (turn), pitch (nod). + pub roll: f64, + pub yaw: f64, + pub pitch: f64, + /// Eye openness (0 closed … 1 open), left and right. + pub eye_open: [f64; 2], + /// Mouth openness (0 … 1) and width (1 = neutral). + pub mouth_open: f64, + pub mouth_width: f64, + /// Eyebrow raise (−1 … 1), left and right. + pub brow_raise: [f64; 2], +} + +impl Default for FaceParams { + fn default() -> Self { + FaceParams { + center: [0.0; 2], + height: 60.0, + roll: 0.0, + yaw: 0.0, + pitch: 0.0, + eye_open: [1.0; 2], + mouth_open: 0.2, + mouth_width: 1.0, + brow_raise: [0.0; 2], + } + } +} + +fn eye_half_height(open: f64) -> f64 { + 0.012 + 0.06 * open.clamp(0.0, 1.0) +} +fn mouth_half_height(open: f64) -> f64 { + 0.015 + 0.11 * open.clamp(0.0, 1.0) +} + +/// The landmarks of a synthetic face in the face frame (before roll, translation and scale). +pub fn synth_shape(p: &FaceParams) -> [[f64; 2]; N] { + let mut s = [[0.0; 2]; N]; + let (sy, sp) = (p.yaw.to_radians().sin(), p.pitch.to_radians().sin()); + // Inner features slide with the head's turn; the nose (closer to the camera) slides more. + let shift = |depth: f64| [0.35 * sy * depth, 0.3 * sp * depth]; + for (i, l) in LANDMARKS.iter().enumerate() { + s[i] = l.2; + } + for side in 0..2 { + let sg = if side == 0 { -1.0 } else { 1.0 }; + let raise = -0.06 * p.brow_raise[side].clamp(-1.0, 1.0); + for &i in &BROW[side] { + s[i][1] += raise; + } + let h = eye_half_height(p.eye_open[side]); + let e = EYE[side]; + s[e[2]] = [sg * 0.31, -0.22 - h]; + s[e[3]] = [sg * 0.31, -0.22 + h]; + } + let mw = 0.24 * p.mouth_width.clamp(0.5, 1.6); + let mh = mouth_half_height(p.mouth_open); + s[MOUTH[0]] = [-mw, 0.5]; + s[MOUTH[1]] = [mw, 0.5]; + s[MOUTH[2]] = [0.0, 0.5 - mh]; + s[MOUTH[3]] = [0.0, 0.5 + mh]; + for (i, q) in s.iter_mut().enumerate() { + if i == CHIN || JAW.contains(&i) { + continue; + } + let d = if i == NOSE_TIP { + 1.3 + } else if NOSTRIL.contains(&i) { + 1.15 + } else { + 1.0 + }; + let o = shift(d); + q[0] += o[0]; + q[1] += o[1]; + } + s +} + +/// Face frame → layer pixels. +fn to_layer(c: [f64; 2], b: f64, roll_deg: f64, q: [f64; 2]) -> [f64; 2] { + let (sn, cs) = roll_deg.to_radians().sin_cos(); + [c[0] + b * (cs * q[0] - sn * q[1]), c[1] + b * (sn * q[0] + cs * q[1])] +} + +/// Layer pixels → face frame. +fn to_face(c: [f64; 2], b: f64, roll_deg: f64, p: [f64; 2]) -> [f64; 2] { + let (sn, cs) = roll_deg.to_radians().sin_cos(); + let d = [(p[0] - c[0]) / b, (p[1] - c[1]) / b]; + [cs * d[0] + sn * d[1], -sn * d[0] + cs * d[1]] +} + +/// A synthetic face's landmarks in layer pixels. +pub fn synth_landmarks(p: &FaceParams) -> [[f64; 2]; N] { + synth_shape(p).map(|q| to_layer(p.center, p.height, p.roll, q)) +} + +fn hash(i: i64, j: i64) -> f64 { + let mut v = (i.wrapping_mul(73_856_093) ^ j.wrapping_mul(19_349_663)) as u64; + v ^= v >> 13; + v = v.wrapping_mul(0x5bd1_e995); + v ^= v >> 15; + (v % 10_000) as f64 / 10_000.0 +} + +fn seg_dist(p: [f64; 2], a: [f64; 2], b: [f64; 2]) -> f64 { + let d = [b[0] - a[0], b[1] - a[1]]; + let l2 = d[0] * d[0] + d[1] * d[1]; + let t = if l2 > 0.0 { (((p[0] - a[0]) * d[0] + (p[1] - a[1]) * d[1]) / l2).clamp(0.0, 1.0) } else { 0.0 }; + (p[0] - a[0] - t * d[0]).hypot(p[1] - a[1] - t * d[1]) +} + +/// Colour of a synthetic face at face-frame point `q` (`None` outside the face). +fn face_color(p: &FaceParams, s: &[[f64; 2]; N], q: [f64; 2]) -> Option<[f32; 3]> { + if (q[0] / ASPECT).powi(2) + q[1] * q[1] > 1.0 { + return None; + } + let shade = (0.95 - 0.08 * q[1]) as f32; + let mut col = [0.86 * shade, 0.64 * shade, 0.52 * shade]; + for side in 0..2 { + let b = BROW[side]; + let d = seg_dist(q, s[b[0]], s[b[1]]).min(seg_dist(q, s[b[1]], s[b[2]])); + if d < 0.035 { + col = [0.28, 0.18, 0.12]; + } + let e = EYE[side]; + let ec = [(s[e[0]][0] + s[e[1]][0]) / 2.0, (s[e[2]][1] + s[e[3]][1]) / 2.0]; + let (hw, hh) = ((s[e[1]][0] - s[e[0]][0]).abs() / 2.0, (s[e[3]][1] - s[e[2]][1]).abs() / 2.0); + if ((q[0] - ec[0]) / hw).powi(2) + ((q[1] - ec[1]) / hh).powi(2) <= 1.0 { + let pupil = s[e[4]]; + col = if (q[0] - pupil[0]).hypot(q[1] - pupil[1]) < 0.055 { [0.12, 0.08, 0.06] } else { [0.95, 0.95, 0.93] }; + } + } + let _ = p; + for &n in &NOSTRIL { + if ((q[0] - s[n][0]) / 0.035).powi(2) + ((q[1] - s[n][1]) / 0.022).powi(2) <= 1.0 { + col = [0.3, 0.13, 0.1]; + } + } + let m = MOUTH; + let mc = [(s[m[0]][0] + s[m[1]][0]) / 2.0, (s[m[2]][1] + s[m[3]][1]) / 2.0]; + let (mw, mh) = ((s[m[1]][0] - s[m[0]][0]) / 2.0, (s[m[3]][1] - s[m[2]][1]) / 2.0); + if ((q[0] - mc[0]) / mw).powi(2) + ((q[1] - mc[1]) / mh).powi(2) <= 1.0 { + col = [0.42, 0.08, 0.1]; + } + Some(col) +} + +/// Render a synthetic face over a textured blue-green background (2 × 2 supersampled): the +/// test footage of the face tracker. +pub fn render_synthetic(w: u32, h: u32, p: &FaceParams, background_seed: i64) -> Image { + use rayon::prelude::*; + let s = synth_shape(p); + let mut img = Image::new(w, h); + img.rows_mut().for_each(|(y, row)| { + for (x, px) in row.iter_mut().enumerate() { + let mut acc = [0.0f32; 3]; + for sy in 0..2 { + for sx in 0..2 { + let l = [x as f64 + 0.25 + 0.5 * sx as f64, y as f64 + 0.25 + 0.5 * sy as f64]; + let q = to_face(p.center, p.height, p.roll, l); + let c = face_color(p, &s, q).unwrap_or_else(|| { + let n = hash((l[0] / 9.0) as i64 + background_seed, (l[1] / 9.0) as i64) as f32; + [0.15 + 0.1 * n, 0.35 + 0.2 * n, 0.45 + 0.25 * n] + }); + for k in 0..3 { + acc[k] += c[k] * 0.25; + } + } + } + *px = [acc[0], acc[1], acc[2], 1.0]; + } + }); + img +} + +// ---------------------------------------------------------------- shape model + +/// Point distribution model: mean shape and principal modes (face frame). +pub struct Pdm { + pub mean: Vec, + /// Modes as rows of length `2N`. + pub modes: Vec>, + /// Variance of each mode. + pub var: Vec, +} + +/// The shape model, trained once on synthetic shapes. +pub fn pdm() -> &'static Pdm { + static P: OnceLock = OnceLock::new(); + P.get_or_init(|| { + let samples = 600; + let dim = 2 * N; + let mut rng = 0x2545_f491_4f6c_dd1du64; + let mut u = || { + rng ^= rng << 13; + rng ^= rng >> 7; + rng ^= rng << 17; + (rng % 1_000_000) as f64 / 1_000_000.0 + }; + let mut data = Vec::with_capacity(samples); + for _ in 0..samples { + let p = FaceParams { + yaw: (u() - 0.5) * 60.0, + pitch: (u() - 0.5) * 40.0, + eye_open: [u(), u()], + mouth_open: u(), + mouth_width: 0.8 + 0.45 * u(), + brow_raise: [u() * 2.0 - 1.0, u() * 2.0 - 1.0], + ..Default::default() + }; + data.push(synth_shape(&p).iter().flat_map(|q| [q[0], q[1]]).collect::>()); + } + let mut mean = vec![0.0; dim]; + for d in &data { + for (m, v) in mean.iter_mut().zip(d) { + *m += v / samples as f64; + } + } + let mut cov = vec![0.0; dim * dim]; + for d in &data { + for a in 0..dim { + let da = d[a] - mean[a]; + for b in 0..dim { + cov[a * dim + b] += da * (d[b] - mean[b]) / samples as f64; + } + } + } + let (vals, vecs) = sym_eigen(&cov, dim); + let total: f64 = vals.iter().map(|v| v.max(0.0)).sum(); + let mut modes = vec![]; + let mut var = vec![]; + let mut acc = 0.0; + for k in (0..dim).rev() { + if acc >= 0.995 * total || vals[k] <= 1e-12 { + break; + } + acc += vals[k]; + modes.push((0..dim).map(|r| vecs[r * dim + k]).collect()); + var.push(vals[k]); + } + Pdm { mean, modes, var } + }) +} + +/// A similarity `p ↦ s R q + t` as `[a, b, tx, ty]` (`x = a u − b v + tx`, `y = b u + a v + ty`). +fn fit_similarity(src: &[[f64; 2]], dst: &[[f64; 2]], w: &[f64]) -> Option<[f64; 4]> { + let ws: f64 = w.iter().sum(); + if ws < 2.0 - 1e-9 { + return None; + } + let mut cs = [0.0; 2]; + let mut cd = [0.0; 2]; + for i in 0..src.len() { + for k in 0..2 { + cs[k] += w[i] * src[i][k] / ws; + cd[k] += w[i] * dst[i][k] / ws; + } + } + let (mut ss, mut dt, mut cr) = (0.0, 0.0, 0.0); + for i in 0..src.len() { + let a = [src[i][0] - cs[0], src[i][1] - cs[1]]; + let b = [dst[i][0] - cd[0], dst[i][1] - cd[1]]; + ss += w[i] * (a[0] * a[0] + a[1] * a[1]); + dt += w[i] * (a[0] * b[0] + a[1] * b[1]); + cr += w[i] * (a[0] * b[1] - a[1] * b[0]); + } + if ss < 1e-12 { + return None; + } + let (a, b) = (dt / ss, cr / ss); + Some([a, b, cd[0] - (a * cs[0] - b * cs[1]), cd[1] - (b * cs[0] + a * cs[1])]) +} + +fn sim_apply(t: &[f64; 4], q: [f64; 2]) -> [f64; 2] { + [t[0] * q[0] - t[1] * q[1] + t[2], t[1] * q[0] + t[0] * q[1] + t[3]] +} + +fn sim_inverse(t: &[f64; 4], p: [f64; 2]) -> [f64; 2] { + let d = [p[0] - t[2], p[1] - t[3]]; + let s2 = t[0] * t[0] + t[1] * t[1]; + [(t[0] * d[0] + t[1] * d[1]) / s2, (-t[1] * d[0] + t[0] * d[1]) / s2] +} + +/// Fit the shape model to detected landmarks (`w` = 0 for missing ones), starting from the +/// face-frame similarity `init`. Returns (model landmarks in layer pixels, similarity). +pub fn fit_shape(detected: &[[f64; 2]; N], w: &[f64; N], init: [f64; 4]) -> ([[f64; 2]; N], [f64; 4]) { + let m = pdm(); + let k = m.modes.len(); + let mut b = vec![0.0; k]; + let mut t = init; + let shape = |b: &[f64]| -> [[f64; 2]; N] { + let mut s = [[0.0; 2]; N]; + for i in 0..N { + for d in 0..2 { + let j = 2 * i + d; + s[i][d] = m.mean[j] + m.modes.iter().zip(b).map(|(row, bk)| row[j] * bk).sum::(); + } + } + s + }; + for _ in 0..10 { + let x = shape(&b); + if let Some(nt) = fit_similarity(&x, detected, w) { + t = nt; + } + // Detections in the model frame. + let y: Vec<[f64; 2]> = detected.iter().map(|p| sim_inverse(&t, *p)).collect(); + let mut ata = vec![0.0; k * k]; + let mut atb = vec![0.0; k]; + for i in 0..N { + if w[i] <= 0.0 { + continue; + } + for d in 0..2 { + let j = 2 * i + d; + let r = y[i][d] - m.mean[j]; + for a in 0..k { + atb[a] += w[i] * m.modes[a][j] * r; + for c in 0..k { + ata[a * k + c] += w[i] * m.modes[a][j] * m.modes[c][j]; + } + } + } + } + for a in 0..k { + // A weak prior towards the mean (Tikhonov, scaled by the mode's variance). + ata[a * k + a] += 1e-3 / m.var[a].max(1e-9) * 1e-3 + 1e-9; + } + if cholesky_solve(&mut ata, k, &mut atb) { + for a in 0..k { + let lim = 3.0 * m.var[a].sqrt(); + b[a] = atb[a].clamp(-lim, lim); + } + } + } + let x = shape(&b); + (x.map(|q| sim_apply(&t, q)), t) +} + +// ---------------------------------------------------------------- tracking + +/// The skin colour model (luma, r and g chromaticity: median and spread). +#[derive(Clone, Copy, Debug, PartialEq)] +struct Skin { + med: [f32; 3], + tol: [f32; 3], +} + +fn features(px: [f32; 4]) -> [f32; 3] { + let s = (px[0] + px[1] + px[2]).max(1e-4); + [0.2126 * px[0] + 0.7152 * px[1] + 0.0722 * px[2], px[0] / s, px[1] / s] +} + +impl Skin { + fn is_skin(&self, px: [f32; 4]) -> bool { + let f = features(px); + (0..3).all(|k| (f[k] - self.med[k]).abs() <= self.tol[k]) + } +} + +/// One frame's face fit. +#[derive(Clone, Debug, PartialEq)] +pub struct FaceFit { + /// Landmarks ([`LANDMARKS`] order), layer pixels. + pub landmarks: [[f64; 2]; N], + /// Which landmarks were measured on this frame (the others come from the shape model). + pub detected: [bool; N], + /// The face outline: a closed polygon (layer pixels). + pub outline: Vec<[f64; 2]>, + /// Face ellipse centre, half-height (pixels) and roll (degrees). + pub center: [f64; 2], + pub height: f64, + pub roll: f64, +} + +/// Follows one face through a clip. +#[derive(Clone, Debug)] +pub struct FaceTracker { + skin: Skin, + prev: FaceFit, + /// Previous landmarks in the face frame (the expected feature positions). + expect: [[f64; 2]; N], +} + +/// A pixel window of a frame in layer coordinates. +struct Window { + x0: i64, + y0: i64, + w: usize, + h: usize, +} + +impl Window { + fn around(frame: &Frame, c: [f64; 2], rx: f64, ry: f64) -> Option { + let (ox, oy) = (frame.offset[0], frame.offset[1]); + let x0 = ((c[0] - rx + ox).floor() as i64).max(0); + let y0 = ((c[1] - ry + oy).floor() as i64).max(0); + let x1 = ((c[0] + rx + ox).ceil() as i64).min(frame.img.width as i64); + let y1 = ((c[1] + ry + oy).ceil() as i64).min(frame.img.height as i64); + (x1 > x0 + 4 && y1 > y0 + 4).then(|| Window { x0, y0, w: (x1 - x0) as usize, h: (y1 - y0) as usize }) + } + fn px(&self, frame: &Frame, i: usize, j: usize) -> [f32; 4] { + frame.img.get(self.x0 + i as i64, self.y0 + j as i64) + } + /// Layer position of the centre of window pixel `(i, j)`. + fn layer(&self, frame: &Frame, i: usize, j: usize) -> [f64; 2] { + [(self.x0 + i as i64) as f64 + 0.5 - frame.offset[0], (self.y0 + j as i64) as f64 + 0.5 - frame.offset[1]] + } + fn cell(&self, frame: &Frame, p: [f64; 2]) -> Option<(usize, usize)> { + let x = (p[0] + frame.offset[0]).floor() as i64 - self.x0; + let y = (p[1] + frame.offset[1]).floor() as i64 - self.y0; + (x >= 0 && y >= 0 && (x as usize) < self.w && (y as usize) < self.h).then_some((x as usize, y as usize)) + } +} + +fn median(v: &mut [f32]) -> f32 { + v.sort_by(f32::total_cmp); + v[v.len() / 2] +} + +/// Area-moment ellipse of a closed polygon: (centroid, half-height along the major axis, +/// half-width, major axis angle from vertical in degrees). +fn polygon_ellipse(poly: &[[f64; 2]]) -> Option<([f64; 2], f64, f64, f64)> { + let n = poly.len(); + let (mut a, mut cx, mut cy) = (0.0, 0.0, 0.0); + let (mut ixx, mut iyy, mut ixy) = (0.0, 0.0, 0.0); + for i in 0..n { + let (p, q) = (poly[i], poly[(i + 1) % n]); + let cr = p[0] * q[1] - q[0] * p[1]; + a += cr; + cx += (p[0] + q[0]) * cr; + cy += (p[1] + q[1]) * cr; + ixx += (p[1] * p[1] + p[1] * q[1] + q[1] * q[1]) * cr; + iyy += (p[0] * p[0] + p[0] * q[0] + q[0] * q[0]) * cr; + ixy += (p[0] * q[1] + 2.0 * p[0] * p[1] + 2.0 * q[0] * q[1] + q[0] * p[1]) * cr; + } + a *= 0.5; + if a.abs() < 1e-9 { + return None; + } + cx /= 6.0 * a; + cy /= 6.0 * a; + // Central second moments (per unit area): var x, var y, cov xy. + let vxx = iyy / (12.0 * a) - cx * cx; + let vyy = ixx / (12.0 * a) - cy * cy; + let vxy = ixy / (24.0 * a) - cx * cy; + let tr = vxx + vyy; + let det = vxx * vyy - vxy * vxy; + let disc = (tr * tr / 4.0 - det).max(0.0).sqrt(); + let (l1, l2) = (tr / 2.0 + disc, (tr / 2.0 - disc).max(0.0)); + // Major axis direction. + let ang = 0.5 * (2.0 * vxy).atan2(vxx - vyy); // from +x + let mut from_vertical = ang.to_degrees() - 90.0; + while from_vertical > 90.0 { + from_vertical -= 180.0; + } + while from_vertical < -90.0 { + from_vertical += 180.0; + } + Some(([cx, cy], 2.0 * l1.sqrt(), 2.0 * l2.sqrt(), from_vertical)) +} + +/// A non-skin component inside the face. +struct Blob { + /// Face-frame positions of its pixels and their darkness (skin luma − luma). + pts: Vec<([f64; 2], f64)>, + centroid: [f64; 2], +} + +impl Blob { + fn extreme(&self, dir: [f64; 2]) -> [f64; 2] { + self.pts.iter().map(|p| p.0).max_by(|a, b| (a[0] * dir[0] + a[1] * dir[1]).total_cmp(&(b[0] * dir[0] + b[1] * dir[1]))).unwrap_or(self.centroid) + } + /// Extreme along `v` (`sign` −1 = top) among the pixels within `band` of the column `u`. + fn lid(&self, u: f64, band: f64, sign: f64) -> [f64; 2] { + let c = self.pts.iter().filter(|p| (p.0[0] - u).abs() <= band).map(|p| p.0); + let best = c.max_by(|a, b| (sign * a[1]).total_cmp(&(sign * b[1]))); + best.map(|p| [u, p[1]]).unwrap_or(self.centroid) + } + fn dark_centroid(&self) -> [f64; 2] { + let (mut s, mut w) = ([0.0; 2], 0.0); + for (q, d) in &self.pts { + let k = d.max(0.0).powi(2); + s[0] += k * q[0]; + s[1] += k * q[1]; + w += k; + } + if w > 1e-12 { [s[0] / w, s[1] / w] } else { self.centroid } + } +} + +impl FaceTracker { + /// Start on `frame` with the face inside `region` (`[x0, y0, x1, y1]`, layer pixels: the + /// mask's bounds). `None` when no face-like skin region is found. + pub fn new(frame: &Frame, region: [f64; 4]) -> Option<(FaceTracker, FaceFit)> { + let c = [(region[0] + region[2]) / 2.0, (region[1] + region[3]) / 2.0]; + let (hw, hh) = ((region[2] - region[0]) / 2.0, (region[3] - region[1]) / 2.0); + if !(hw > 4.0 && hh > 4.0) { + return None; + } + // Skin colour: the middle of the region (cheeks and nose). + let win = Window::around(frame, c, hw * 0.35, hh * 0.3)?; + let mut f: [Vec; 3] = [vec![], vec![], vec![]]; + for j in 0..win.h { + for i in 0..win.w { + let v = features(win.px(frame, i, j)); + for k in 0..3 { + f[k].push(v[k]); + } + } + } + let mut med = [0.0f32; 3]; + let mut tol = [0.0f32; 3]; + let floors = [0.12f32, 0.025, 0.025]; + for k in 0..3 { + med[k] = median(&mut f[k]); + let mut dev: Vec = f[k].iter().map(|v| (v - med[k]).abs()).collect(); + tol[k] = (4.0 * median(&mut dev)).max(floors[k]); + } + let skin = Skin { med, tol }; + let canon: [[f64; 2]; N] = LANDMARKS.map(|l| l.2); + let guess = FaceFit { + landmarks: canon.map(|q| to_layer(c, hh, 0.0, q)), + detected: [false; N], + outline: vec![], + center: c, + height: hh.max(hw / ASPECT), + roll: 0.0, + }; + let mut t = FaceTracker { skin, prev: guess, expect: canon }; + let fit = t.fit(frame)?; + t.accept(&fit); + Some((t, fit)) + } + + /// The fit of the last tracked frame. + pub fn last(&self) -> &FaceFit { + &self.prev + } + + fn accept(&mut self, fit: &FaceFit) { + self.expect = fit.landmarks.map(|p| to_face(fit.center, fit.height, fit.roll, p)); + self.prev = fit.clone(); + } + + /// Track into the next frame (either direction). `None` when the face is lost. + pub fn step(&mut self, frame: &Frame) -> Option { + let fit = self.fit(frame)?; + self.accept(&fit); + Some(fit) + } + + fn fit(&self, frame: &Frame) -> Option { + let pv = &self.prev; + let b0 = pv.height; + let reach = 1.7 * b0; + let win = Window::around(frame, pv.center, reach, reach)?; + let (w, h) = (win.w, win.h); + let mut skin = vec![false; w * h]; + let mut luma = vec![0.0f32; w * h]; + for j in 0..h { + for i in 0..w { + let px = win.px(frame, i, j); + skin[j * w + i] = self.skin.is_skin(px); + luma[j * w + i] = features(px)[0]; + } + } + // Seed: the skin pixel nearest the predicted centre. + let (ci, cj) = win.cell(frame, pv.center)?; + let mut seed = None; + 'search: for r in 0..(0.6 * b0).ceil() as i64 { + for dj in -r..=r { + for di in -r..=r { + if di.abs() != r && dj.abs() != r { + continue; + } + let (i, j) = (ci as i64 + di, cj as i64 + dj); + if i >= 0 && j >= 0 && (i as usize) < w && (j as usize) < h && skin[j as usize * w + i as usize] { + seed = Some((i as usize, j as usize)); + break 'search; + } + } + } + } + let (si, sj) = seed?; + // Flood fill (4-connected). + let mut comp = vec![false; w * h]; + let mut stack = vec![(si, sj)]; + comp[sj * w + si] = true; + let mut count = 0usize; + let (mut mx, mut my) = (0.0, 0.0); + while let Some((i, j)) = stack.pop() { + count += 1; + let p = win.layer(frame, i, j); + mx += p[0]; + my += p[1]; + let nb = [(i.wrapping_sub(1), j), (i + 1, j), (i, j.wrapping_sub(1)), (i, j + 1)]; + for (a, b) in nb { + if a < w && b < h && !comp[b * w + a] && skin[b * w + a] { + comp[b * w + a] = true; + stack.push((a, b)); + } + } + } + if (count as f64) < 0.15 * std::f64::consts::PI * ASPECT * b0 * b0 { + return None; + } + let c0 = [mx / count as f64, my / count as f64]; + // Outline along rays: the farthest component pixel. + let rays = 48; + let mut radii = vec![0.0f64; rays]; + for (k, r) in radii.iter_mut().enumerate() { + let a = k as f64 / rays as f64 * std::f64::consts::TAU; + let d = [a.cos(), a.sin()]; + let mut t = 0.0; + while t < reach * 1.4 { + let p = [c0[0] + d[0] * t, c0[1] + d[1] * t]; + match win.cell(frame, p) { + Some((i, j)) => { + if comp[j * w + i] { + *r = t + 0.5; + } + } + None => break, + } + t += 0.5; + } + } + // Median of 5, circularly. + let sm: Vec = (0..rays) + .map(|k| { + let mut v: Vec = (0..5).map(|d| radii[(k + rays + d - 2) % rays]).collect(); + v.sort_by(f64::total_cmp); + v[2] + }) + .collect(); + let outline: Vec<[f64; 2]> = (0..rays) + .map(|k| { + let a = k as f64 / rays as f64 * std::f64::consts::TAU; + [c0[0] + a.cos() * sm[k], c0[1] + a.sin() * sm[k]] + }) + .collect(); + let (center, major, _minor, ang) = polygon_ellipse(&outline)?; + if !(major > 0.3 * b0 && major < 2.5 * b0) { + return None; + } + // Roll: the ellipse axis nearest the previous roll (it is ambiguous by 180°). + let mut roll = ang; + while roll - pv.roll > 90.0 { + roll -= 180.0; + } + while roll - pv.roll < -90.0 { + roll += 180.0; + } + let b = major; + // Non-skin components inside the face outline (shrunk a little: anti-aliased edges). + let inner: Vec<[f64; 2]> = outline.iter().map(|p| [center[0] + 0.9 * (p[0] - center[0]), center[1] + 0.9 * (p[1] - center[1])]).collect(); + let mut feat = vec![false; w * h]; + for j in 0..h { + for i in 0..w { + if !skin[j * w + i] && crate::mask::inside_polygon(&inner, win.layer(frame, i, j)) { + feat[j * w + i] = true; + } + } + } + let min_area = (0.0006 * b * b).max(2.0) as usize; + let mut seen = vec![false; w * h]; + let mut blobs: Vec = vec![]; + let skin_luma = self.skin.med[0] as f64; + for j0 in 0..h { + for i0 in 0..w { + if !feat[j0 * w + i0] || seen[j0 * w + i0] { + continue; + } + let mut st = vec![(i0, j0)]; + seen[j0 * w + i0] = true; + let mut pts = vec![]; + while let Some((i, j)) = st.pop() { + let q = to_face(center, b, roll, win.layer(frame, i, j)); + pts.push((q, skin_luma - luma[j * w + i] as f64)); + for dj in -1i64..=1 { + for di in -1i64..=1 { + let (a, c) = (i as i64 + di, j as i64 + dj); + if a < 0 || c < 0 || a as usize >= w || c as usize >= h { + continue; + } + let (a, c) = (a as usize, c as usize); + if feat[c * w + a] && !seen[c * w + a] { + seen[c * w + a] = true; + st.push((a, c)); + } + } + } + } + if pts.len() >= min_area { + let n = pts.len() as f64; + let centroid = [pts.iter().map(|p| p.0[0]).sum::() / n, pts.iter().map(|p| p.0[1]).sum::() / n]; + blobs.push(Blob { pts, centroid }); + } + } + } + // Match components to features near their expected positions. + let ex = &self.expect; + let mean_of = |ids: &[usize]| { + let n = ids.len() as f64; + [ids.iter().map(|i| ex[*i][0]).sum::() / n, ids.iter().map(|i| ex[*i][1]).sum::() / n] + }; + let mut used = vec![false; blobs.len()]; + let take = |target: [f64; 2], radius: f64, prefer_large: bool, used: &mut Vec| -> Option { + let mut best: Option<(usize, f64)> = None; + for (k, bl) in blobs.iter().enumerate() { + if used[k] { + continue; + } + let d = (bl.centroid[0] - target[0]).hypot(bl.centroid[1] - target[1]); + if d > radius { + continue; + } + let score = if prefer_large { -(bl.pts.len() as f64) } else { d }; + if best.is_none_or(|b| score < b.1) { + best = Some((k, score)); + } + } + if let Some((k, _)) = best { + used[k] = true; + } + best.map(|b| b.0) + }; + let mut det = [[0.0; 2]; N]; + let mut wgt = [0.0; N]; + let set = |i: usize, q: [f64; 2], det: &mut [[f64; 2]; N], wgt: &mut [f64; N]| { + det[i] = to_layer(center, b, roll, q); + wgt[i] = 1.0; + }; + let eye_ids = [EYE[0].to_vec(), EYE[1].to_vec()]; + let mouth = take(mean_of(&MOUTH), 0.25, true, &mut used); + let eyes = [take(mean_of(&eye_ids[0]), 0.15, false, &mut used), take(mean_of(&eye_ids[1]), 0.15, false, &mut used)]; + let brows = [take(mean_of(&BROW[0]), 0.15, false, &mut used), take(mean_of(&BROW[1]), 0.15, false, &mut used)]; + let nostrils = [take(ex[NOSTRIL[0]], 0.07, false, &mut used), take(ex[NOSTRIL[1]], 0.07, false, &mut used)]; + for side in 0..2 { + let sg = if side == 0 { -1.0 } else { 1.0 }; + if let Some(k) = eyes[side] { + let bl = &blobs[k]; + let e = EYE[side]; + let inner = bl.extreme([-sg, 0.0]); + let outer = bl.extreme([sg, 0.0]); + let mid = 0.5 * (inner[0] + outer[0]); + let band = 0.15 * (outer[0] - inner[0]).abs(); + let px = 0.5 / b; + set(e[0], [inner[0] - sg * px, inner[1]], &mut det, &mut wgt); + set(e[1], [outer[0] + sg * px, outer[1]], &mut det, &mut wgt); + let top = bl.lid(mid, band, -1.0); + let bot = bl.lid(mid, band, 1.0); + set(e[2], [top[0], top[1] - px], &mut det, &mut wgt); + set(e[3], [bot[0], bot[1] + px], &mut det, &mut wgt); + set(e[4], bl.dark_centroid(), &mut det, &mut wgt); + } + if let Some(k) = brows[side] { + let bl = &blobs[k]; + let br = BROW[side]; + let inner = bl.extreme([-sg, 0.0]); + let outer = bl.extreme([sg, 0.0]); + set(br[0], inner, &mut det, &mut wgt); + set(br[2], outer, &mut det, &mut wgt); + // The middle: the brow's centre line at its mid column. + let mid = 0.5 * (inner[0] + outer[0]); + let band = 0.1 * (outer[0] - inner[0]).abs().max(0.05); + let col: Vec = bl.pts.iter().filter(|p| (p.0[0] - mid).abs() <= band).map(|p| p.0[1]).collect(); + if !col.is_empty() { + set(br[1], [mid, col.iter().sum::() / col.len() as f64], &mut det, &mut wgt); + } + } + if let Some(k) = nostrils[side] { + set(NOSTRIL[side], blobs[k].centroid, &mut det, &mut wgt); + } + } + if let (Some(a), Some(c)) = (nostrils[0], nostrils[1]) { + let (p, q) = (blobs[a].centroid, blobs[c].centroid); + set(NOSE_TIP, [0.5 * (p[0] + q[0]), 0.5 * (p[1] + q[1]) - 0.06], &mut det, &mut wgt); + } + if let Some(k) = mouth { + let bl = &blobs[k]; + let l = bl.extreme([-1.0, 0.0]); + let r = bl.extreme([1.0, 0.0]); + let px = 0.5 / b; + set(MOUTH[0], [l[0] - px, l[1]], &mut det, &mut wgt); + set(MOUTH[1], [r[0] + px, r[1]], &mut det, &mut wgt); + let mid = 0.5 * (l[0] + r[0]); + let band = 0.12 * (r[0] - l[0]).abs(); + let top = bl.lid(mid, band, -1.0); + let bot = bl.lid(mid, band, 1.0); + set(MOUTH[2], [top[0], top[1] - px], &mut det, &mut wgt); + set(MOUTH[3], [bot[0], bot[1] + px], &mut det, &mut wgt); + } + // Chin and jaw from the outline, along their canonical directions. + for i in [CHIN, JAW[0], JAW[1]] { + let q = LANDMARKS[i].2; + let dir = to_layer([0.0; 2], 1.0, roll, q); + let ang = dir[1].atan2(dir[0]); + let rad = ray_radius(&outline, center, ang); + let len = dir[0].hypot(dir[1]); + let p = [center[0] + dir[0] / len * rad, center[1] + dir[1] / len * rad]; + det[i] = p; + wgt[i] = 1.0; + } + // Shape model: fill the missing landmarks and replace inconsistent detections. + let (sn, cs) = roll.to_radians().sin_cos(); + let init = [b * cs, b * sn, center[0], center[1]]; + let (model, _) = fit_shape(&det, &wgt, init); + let mut w2 = wgt; + for i in 0..N { + if wgt[i] > 0.0 && (det[i][0] - model[i][0]).hypot(det[i][1] - model[i][1]) > 0.12 * b { + w2[i] = 0.0; + } + } + let (model, sim) = if w2 != wgt { fit_shape(&det, &w2, init) } else { (model, init) }; + let _ = sim; + let mut landmarks = model; + let mut detected = [false; N]; + for i in 0..N { + if w2[i] > 0.0 { + landmarks[i] = det[i]; + detected[i] = true; + } + } + // Roll from the jaw line (it does not move with yaw); fall back to the ellipse. + let jl = landmarks[JAW[0]]; + let jr = landmarks[JAW[1]]; + let roll_jaw = (jr[1] - jl[1]).atan2(jr[0] - jl[0]).to_degrees(); + let roll = if (roll_jaw - roll).abs() < 20.0 { roll_jaw } else { roll }; + Some(FaceFit { landmarks, detected, outline, center, height: b, roll }) + } +} + +/// Distance from `c` to the polygon `poly` (star-shaped around `c`) along angle `ang`. +fn ray_radius(poly: &[[f64; 2]], c: [f64; 2], ang: f64) -> f64 { + let d = [ang.cos(), ang.sin()]; + let mut best = 0.0f64; + let n = poly.len(); + for i in 0..n { + let (p, q) = (poly[i], poly[(i + 1) % n]); + let e = [q[0] - p[0], q[1] - p[1]]; + let den = d[0] * e[1] - d[1] * e[0]; + if den.abs() < 1e-12 { + continue; + } + let w = [p[0] - c[0], p[1] - c[1]]; + let t = (w[0] * e[1] - w[1] * e[0]) / den; + let s = (w[0] * d[1] - w[1] * d[0]) / den; + if t > 0.0 && (-1e-9..=1.0 + 1e-9).contains(&s) { + best = best.max(t); + } + } + best +} + +// ---------------------------------------------------------------- measurements + +/// Face measurements (Extract & Copy Face Measurements): (id, display name). +pub const MEASUREMENTS: [(&str, &str); 14] = [ + ("headPositionX", "Head Position X"), + ("headPositionY", "Head Position Y"), + ("headScale", "Head Scale"), + ("headOrientationX", "Head Orientation X"), + ("headOrientationY", "Head Orientation Y"), + ("headOrientationZ", "Head Orientation Z"), + ("leftEyeOpenness", "Left Eye Openness"), + ("rightEyeOpenness", "Right Eye Openness"), + ("leftEyebrowRaise", "Left Eyebrow Distance from Eye"), + ("rightEyebrowRaise", "Right Eyebrow Distance from Eye"), + ("mouthOpenness", "Mouth Openness"), + ("mouthWidth", "Mouth Width"), + ("mouthOffsetX", "Mouth Offset X"), + ("mouthOffsetY", "Mouth Offset Y"), +]; + +/// Face centre, half-height and roll from the chin and jaw landmarks. +pub fn face_frame(l: &[[f64; 2]; N]) -> ([f64; 2], f64, f64) { + let (ch, jl, jr) = (l[CHIN], l[JAW[0]], l[JAW[1]]); + let jm = [0.5 * (jl[0] + jr[0]), 0.5 * (jl[1] + jr[1])]; + let k = 1.0 - LANDMARKS[JAW[0]].2[1]; + let c = [ch[0] + (jm[0] - ch[0]) / k, ch[1] + (jm[1] - ch[1]) / k]; + // Half-height from the chin and from the jaw width (averaged: less noise). + let jw = (jr[0] - jl[0]).hypot(jr[1] - jl[1]) / (2.0 * LANDMARKS[JAW[1]].2[0]); + let b = (0.5 * ((ch[0] - c[0]).hypot(ch[1] - c[1]) + jw)).max(1e-9); + let roll = (jr[1] - jl[1]).atan2(jr[0] - jl[0]).to_degrees(); + (c, b, roll) +} + +/// The measurements of a frame's landmarks ([`MEASUREMENTS`] order); `reference_height`: the +/// face half-height that is Head Scale 100 %. +pub fn measure(l: &[[f64; 2]; N], reference_height: f64) -> [f64; 14] { + let (c, b, roll) = face_frame(l); + let q = l.map(|p| to_face(c, b, roll, p)); + let canon = LANDMARKS.map(|x| x.2); + // Inner features slide with yaw and pitch (see `synth_shape`). + let inner = [EYE[0][4], EYE[1][4], MOUTH[0], MOUTH[1], EYE[0][0], EYE[1][0], EYE[0][1], EYE[1][1]]; + let n = inner.len() as f64; + let du = inner.iter().map(|i| q[*i][0] - canon[*i][0]).sum::() / n; + let dv = inner.iter().map(|i| q[*i][1] - canon[*i][1]).sum::() / n; + let yaw = (du / 0.35).clamp(-1.0, 1.0).asin().to_degrees(); + let pitch = (dv / 0.3).clamp(-1.0, 1.0).asin().to_degrees(); + let open = |e: [usize; 5]| { + let wdt = (q[e[1]][0] - q[e[0]][0]).abs().max(1e-9); + (q[e[3]][1] - q[e[2]][1]).max(0.0) / wdt * 100.0 + }; + let brow = |side: usize| { + let e = EYE[side]; + let eye_v = 0.5 * (q[e[2]][1] + q[e[3]][1]); + (eye_v - q[BROW[side][1]][1]) * 100.0 + }; + let mw = (q[MOUTH[1]][0] - q[MOUTH[0]][0]).abs(); + let mc = [0.5 * (q[MOUTH[0]][0] + q[MOUTH[1]][0]), 0.5 * (q[MOUTH[2]][1] + q[MOUTH[3]][1])]; + [ + c[0], + c[1], + b / reference_height.max(1e-9) * 100.0, + pitch, + yaw, + roll, + open(EYE[0]), + open(EYE[1]), + brow(0), + brow(1), + (q[MOUTH[3]][1] - q[MOUTH[2]][1]).max(0.0) / mw.max(1e-9) * 100.0, + mw / (2.0 * ASPECT) * 100.0, + (mc[0] - q[NOSE_TIP][0]) * 100.0, + (mc[1] - q[NOSE_TIP][1]) * 100.0, + ] +} + +#[cfg(test)] +mod tests { + use super::*; + + fn frame_params(t: usize) -> FaceParams { + let tf = t as f64; + FaceParams { + center: [160.0 + 30.0 * (tf * 0.15).sin(), 120.0 + 12.0 * (tf * 0.1).cos()], + height: 62.0 * (1.0 + 0.08 * (tf * 0.07).sin()), + roll: 14.0 * (tf * 0.12).sin(), + yaw: 12.0 * (tf * 0.09).sin(), + pitch: 6.0 * (tf * 0.11).cos(), + eye_open: [0.8 + 0.2 * (tf * 0.3).sin(), 0.8 + 0.2 * (tf * 0.3).sin()], + mouth_open: 0.5 + 0.45 * (tf * 0.2).sin(), + mouth_width: 1.0 + 0.1 * (tf * 0.13).cos(), + brow_raise: [0.4 * (tf * 0.17).sin(), 0.4 * (tf * 0.17).sin()], + } + } + + #[test] + fn shape_model_spans_the_synthetic_shapes() { + let m = pdm(); + assert!(m.modes.len() >= 5 && m.modes.len() <= 14, "{} modes", m.modes.len()); + // A synthetic shape is reproduced from half its landmarks. + let p = FaceParams { yaw: 15.0, mouth_open: 0.8, eye_open: [0.3, 0.9], center: [100.0, 80.0], height: 50.0, roll: 10.0, ..Default::default() }; + let truth = synth_landmarks(&p); + let mut w = [0.0; N]; + for (i, wi) in w.iter_mut().enumerate() { + *wi = if i % 2 == 0 || i >= 19 { 1.0 } else { 0.0 }; + } + let (sn, cs) = 10f64.to_radians().sin_cos(); + let (fit, _) = fit_shape(&truth, &w, [50.0 * cs, 50.0 * sn, 100.0, 80.0]); + let err = (0..N).map(|i| (fit[i][0] - truth[i][0]).hypot(fit[i][1] - truth[i][1])).fold(0.0, f64::max); + assert!(err < 0.06 * 50.0, "max error {err}"); + } + + #[test] + fn tracks_a_moving_rotating_synthetic_face() { + let (w, h) = (320, 240); + let p0 = frame_params(0); + let img0 = render_synthetic(w, h, &p0, 3); + // The user's mask: a loose box around the face. + let region = [p0.center[0] - 60.0, p0.center[1] - 75.0, p0.center[0] + 60.0, p0.center[1] + 75.0]; + let (mut tr, fit0) = FaceTracker::new(&Frame { img: &img0, offset: [0.0; 2] }, region).expect("face found"); + let mut fits = vec![fit0]; + let n = 40; + for t in 1..n { + let img = render_synthetic(w, h, &frame_params(t), 3); + fits.push(tr.step(&Frame { img: &img, offset: [0.0; 2] }).unwrap_or_else(|| panic!("lost at {t}"))); + } + let mut per = [0.0f64; N]; + let mut worst = 0.0f64; + let mut outline_err = 0.0; + let mut meas = vec![]; + for (t, f) in fits.iter().enumerate() { + let p = frame_params(t); + let truth = synth_landmarks(&p); + for i in 0..N { + let e = (f.landmarks[i][0] - truth[i][0]).hypot(f.landmarks[i][1] - truth[i][1]) / p.height; + per[i] += e / n as f64; + worst = worst.max(e); + } + // Outline: on the face ellipse. + let mut oe = 0.0; + for q in &f.outline { + let u = to_face(p.center, p.height, p.roll, *q); + oe += ((u[0] / ASPECT).powi(2) + u[1] * u[1]).sqrt() - 1.0; + } + outline_err += (oe / f.outline.len() as f64).abs() / n as f64; + meas.push((p, measure(&f.landmarks, fits[0].height))); + assert!((f.roll - p.roll).abs() < 3.0, "frame {t}: roll {} vs {}", f.roll, p.roll); + } + let mean = per.iter().sum::() / N as f64; + eprintln!("face landmarks: mean error {mean:.4}, worst {worst:.4} face heights; outline {outline_err:.4}"); + for (i, e) in per.iter().enumerate() { + assert!(*e < 0.04, "{}: mean error {e:.3} (face heights)", LANDMARKS[i].0); + } + assert!(mean < 0.02, "mean landmark error {mean:.4}"); + assert!(worst < 0.08, "worst landmark error {worst:.3}"); + assert!(outline_err < 0.04, "outline error {outline_err:.3}"); + // Measurements follow the generator: mouth openness and yaw correlate strongly. + let corr = |a: &[f64], b: &[f64]| { + let n = a.len() as f64; + let (ma, mb) = (a.iter().sum::() / n, b.iter().sum::() / n); + let (mut sab, mut saa, mut sbb) = (0.0, 0.0, 0.0); + for (x, y) in a.iter().zip(b) { + sab += (x - ma) * (y - mb); + saa += (x - ma).powi(2); + sbb += (y - mb).powi(2); + } + sab / (saa * sbb).sqrt() + }; + let mo: Vec = meas.iter().map(|m| m.1[10]).collect(); + let mt: Vec = meas.iter().map(|m| m.0.mouth_open).collect(); + assert!(corr(&mo, &mt) > 0.95, "mouth openness r = {}", corr(&mo, &mt)); + let yw: Vec = meas.iter().map(|m| m.1[4]).collect(); + let yt: Vec = meas.iter().map(|m| m.0.yaw).collect(); + assert!(corr(&yw, &yt) > 0.9, "yaw r = {}", corr(&yw, &yt)); + let rl: Vec = meas.iter().map(|m| m.1[5]).collect(); + let rt: Vec = meas.iter().map(|m| m.0.roll).collect(); + assert!(corr(&rl, &rt) > 0.98, "roll r = {}", corr(&rl, &rt)); + let sc: Vec = meas.iter().map(|m| m.1[2]).collect(); + let st: Vec = meas.iter().map(|m| m.0.height).collect(); + assert!(corr(&sc, &st) > 0.95, "scale r = {}", corr(&sc, &st)); + } + + #[test] + fn measurements_of_the_synthetic_model_are_exact() { + let p = FaceParams { center: [50.0, 60.0], height: 40.0, roll: 20.0, yaw: 10.0, pitch: -5.0, ..Default::default() }; + let m = measure(&synth_landmarks(&p), 40.0); + assert!((m[0] - 50.0).abs() < 1e-6 && (m[1] - 60.0).abs() < 1e-6, "{m:?}"); + assert!((m[2] - 100.0).abs() < 1e-6); + assert!((m[5] - 20.0).abs() < 1e-6); + assert!((m[4] - 10.0).abs() < 0.5 && (m[3] + 5.0).abs() < 0.5, "{m:?}"); + } +} diff --git a/crates/track/src/lib.rs b/crates/track/src/lib.rs index 1c6e6a15..f4a7b88f 100644 --- a/crates/track/src/lib.rs +++ b/crates/track/src/lib.rs @@ -19,6 +19,7 @@ //! after subtracting the frame's `offset` (see [`Frame`]). pub mod camtrack; +pub mod face; pub mod fit; pub mod klt; pub mod mask; @@ -26,6 +27,7 @@ pub mod plane; pub mod roto; pub mod solve; pub mod stabilize; +pub mod subspace; use effectcraft_raster::Image; use rayon::prelude::*; diff --git a/crates/track/src/solve.rs b/crates/track/src/solve.rs index 6c267721..693c52f7 100644 --- a/crates/track/src/solve.rs +++ b/crates/track/src/solve.rs @@ -37,6 +37,12 @@ pub fn solve_linear(mut a: [[f64; N]; N], mut b: [f64; N]) -> Op #[derive(Clone, Copy, Debug, PartialEq)] pub struct Homography(pub [[f64; 3]; 3]); +impl Default for Homography { + fn default() -> Self { + Homography::IDENTITY + } +} + impl Homography { pub const IDENTITY: Homography = Homography([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]); diff --git a/crates/track/src/stabilize.rs b/crates/track/src/stabilize.rs index 1abe79f9..9769fcbd 100644 --- a/crates/track/src/stabilize.rs +++ b/crates/track/src/stabilize.rs @@ -18,14 +18,23 @@ //! Video Stabilization with Robust L1 Optimal Camera Paths" (CVPR 2011), applied here //! per frame rather than through a linear program. //! +//! +//! *Subspace Warp* additionally follows long feature trajectories through the clip +//! ([`crate::camtrack::TrackAnalyzer`]) and stabilises each frame with a content-preserving mesh +//! warp fitted to the subspace-smoothed trajectories ([`crate::subspace`]): scenes with depth +//! (parallax) that one homography per frame cannot hold still are stabilised locally. The +//! framing (crop, auto-scale, Maximum Scale relaxation) works the same on meshes. +//! //! 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::camtrack::{Track2D, TrackAnalyzer}; +use crate::fit::{Model, least_squares, ransac}; use crate::klt::{CornerOpts, GrayPyramid, LkOpts, analysis_factor, good_features, track}; use crate::solve::Homography; +use crate::subspace::{Mesh, MeshOpts, smooth_trajectories, solve_mesh}; /// Inter-frame motion of one frame (from the previous frame to this one) for each model. #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] @@ -82,6 +91,10 @@ pub struct WarpAnalysis { pub detailed: bool, /// Per frame; frame 0's motion is the identity. pub frames: Vec, + /// Long feature trajectories (layer pixels, 0.1 px) for Subspace Warp; empty in analyses + /// made before it (which then stabilise like Perspective). + #[serde(skip_serializing_if = "Vec::is_empty")] + pub tracks: Vec, } impl WarpAnalysis { @@ -120,6 +133,7 @@ pub struct Analyzer { prev: Option, frames: Vec, size: [f64; 2], + trajectories: TrackAnalyzer, } fn round_pts(p: &[[f64; 2]], n: usize) -> Vec<[f32; 2]> { @@ -129,7 +143,8 @@ fn round_pts(p: &[[f64; 2]], n: usize) -> Vec<[f32; 2]> { impl Analyzer { pub fn new(size: [f64; 2], opts: AnalyzeOpts) -> Analyzer { - Analyzer { opts, prev: None, frames: vec![], size } + let (side, want) = if opts.detailed { (640, 260) } else { (480, 160) }; + Analyzer { opts, prev: None, frames: vec![], size, trajectories: TrackAnalyzer::with_budget(size, side, want) } } pub fn len(&self) -> usize { @@ -144,6 +159,7 @@ impl Analyzer { 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); + self.trajectories.push(frame); let mut fm = FrameMotion::default(); if let Some(prev) = &self.prev && prev.width() == pyr.width() @@ -184,7 +200,17 @@ impl Analyzer { } 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 } + let mut tracks = self.trajectories.finish(start, frame_duration).tracks; + // Short trajectories hardly constrain the subspace: keep those of at least 6 frames, at + // 0.1 px (smaller JSON). + tracks.retain(|t| t.pts.len() >= 6); + for (i, t) in tracks.iter_mut().enumerate() { + t.id = i as u32; + for p in t.pts.iter_mut() { + *p = [(p[0] * 10.0).round() / 10.0, (p[1] * 10.0).round() / 10.0]; + } + } + WarpAnalysis { version: 2, start, frame_duration, size: self.size, detailed: self.opts.detailed, frames: self.frames, tracks } } } @@ -205,11 +231,31 @@ pub enum Method { /// Position, Scale, Rotation. Similarity, Perspective, - /// Approximated by Perspective (see the module docs of the effect). + /// Content-preserving mesh warps fitted to subspace-smoothed trajectories. #[default] SubspaceWarp, } +/// Advanced ▸ Rolling Shutter Ripple: how freely Subspace Warp's mesh may bend row by row (a +/// rolling-shutter camera exposes rows at different times, so shake wobbles the frame). +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] +pub enum Ripple { + /// A 12 × 8 mesh. + #[default] + Automatic, + /// Twice the rows and a softer shape term: follows row-wise wobble more closely. + Enhanced, +} + +impl Ripple { + pub fn mesh(self) -> MeshOpts { + match self { + Ripple::Automatic => MeshOpts { cols: 12, rows: 8, alpha: 1.0, prior: 0.02 }, + Ripple::Enhanced => MeshOpts { cols: 12, rows: 16, alpha: 0.5, prior: 0.02 }, + } + } +} + /// Borders ▸ Framing. #[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] pub enum Framing { @@ -239,6 +285,7 @@ pub struct StabSettings { pub crop_less_smooth_more: f64, /// Frames per second (for Smoothness). pub fps: f64, + pub ripple: Ripple, } impl Default for StabSettings { @@ -254,6 +301,7 @@ impl Default for StabSettings { additional_scale: 100.0, crop_less_smooth_more: 0.0, fps: 30.0, + ripple: Ripple::Automatic, } } } @@ -269,8 +317,15 @@ impl StabSettings { /// 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. + /// Final source → output transform per frame (layer pixels), framing scale included. For + /// mesh warps, the closest homography (for overlays and edge synthesis). pub warps: Vec, + /// Subspace Warp: per frame, the mesh (stabilised pixels before framing → source pixels); + /// empty for the other methods. + pub meshes: Vec, + /// The framing transform (auto-scale and Additional Scale about the centre) after the + /// stabilisation. + pub framing: Homography, /// The visible region in output pixels `[x0, y0, x1, y1]` (Stabilize, Crop / Auto-scale); /// `None` = everything the warp produces. pub crop: Option<[f64; 4]>, @@ -280,6 +335,24 @@ pub struct Plan { pub valid_fraction: f64, } +impl Plan { + /// The source position (frame `k`'s layer pixels) shown at output point `p`. + pub fn source_of(&self, k: usize, p: [f64; 2]) -> Option<[f64; 2]> { + if let Some(m) = self.meshes.get(k) { + let inv = self.framing.inverse()?; + return Some(m.sample(inv.apply(p))); + } + Some(self.warps.get(k)?.inverse()?.apply(p)) + } + /// Where source point `q` of frame `k` lands in the output. + pub fn output_of(&self, k: usize, q: [f64; 2]) -> Option<[f64; 2]> { + if let Some(m) = self.meshes.get(k) { + return m.forward(q).map(|p| self.framing.apply(p)); + } + Some(self.warps.get(k)?.apply(q)) + } +} + fn convex_contains(q: &[[f64; 2]; 4], p: [f64; 2]) -> bool { let mut sign = 0.0; for i in 0..4 { @@ -418,14 +491,100 @@ pub fn corrections(a: &WarpAnalysis, s: &StabSettings) -> Vec { out } +/// One frame's stabilisation before framing. +#[derive(Clone, Debug)] +enum Warp { + H(Homography), + M(Mesh), +} + +impl Warp { + fn valid(&self, size: [f64; 2]) -> f64 { + match self { + Warp::H(h) => valid_fraction(h, size), + Warp::M(m) => m.valid_fraction(), + } + } + /// Blend towards the identity (`l` = 1 keeps the warp). + fn relax(&self, l: f64) -> Warp { + match self { + Warp::H(h) => Warp::H(Homography::IDENTITY.lerp(h, l)), + Warp::M(m) => Warp::M(m.lerp_identity(l)), + } + } + fn homography(&self) -> Homography { + match self { + Warp::H(h) => *h, + Warp::M(m) => m.homography(), + } + } +} + +/// Remove the uniform scale of the correspondences' best similarity (Preserve Scale): the +/// wanted positions are scaled back about their centroid. +fn unscale(pairs: &mut [([f64; 2], [f64; 2])]) { + let (src, dst): (Vec<[f64; 2]>, Vec<[f64; 2]>) = pairs.iter().copied().unzip(); + let Some(h) = least_squares(Model::Similarity, &src, &dst) else { return }; + let m = h.0; + let k = (m[0][0] * m[1][1] - m[0][1] * m[1][0]).abs().sqrt(); + if k < 1e-6 { + return; + } + let n = dst.len() as f64; + let c = [dst.iter().map(|q| q[0]).sum::() / n, dst.iter().map(|q| q[1]).sum::() / n]; + for (_, d) in pairs.iter_mut() { + *d = [c[0] + (d[0] - c[0]) / k, c[1] + (d[1] - c[1]) / k]; + } +} + +/// Subspace Warp's meshes (stabilised → source, before framing) for every frame; `None` for the +/// other methods and for analyses without trajectories. `base`: the perspective corrections +/// ([`corrections`]), the meshes' prior. +pub fn meshes(a: &WarpAnalysis, s: &StabSettings, base: &[Homography]) -> Option> { + if s.method != Method::SubspaceWarp || a.tracks.is_empty() || base.len() != a.frames.len() { + return None; + } + let n = a.frames.len(); + let o = s.ripple.mesh(); + let size = a.size; + let pairs: Vec> = match s.result { + StabResult::NoMotion => (0..n).map(|t| a.tracks.iter().filter_map(|tr| tr.at(t as u32)).map(|p| (p, base[t].apply(p))).collect()).collect(), + StabResult::SmoothMotion => { + let sigma = s.sigma(); + if sigma < 0.3 { + return Some((0..n).map(|_| Mesh::identity(o.cols, o.rows, size)).collect()); + } + smooth_trajectories(&a.tracks, n, sigma, 9) + } + }; + use rayon::prelude::*; + Some( + pairs + .into_par_iter() + .enumerate() + .map(|(t, mut pr)| { + if s.preserve_scale && pr.len() >= 2 { + unscale(&mut pr); + } + if pr.len() < 6 { Mesh::from_homography(o.cols, o.rows, size, &base[t]) } else { solve_mesh(size, &o, &pr, &base[t]) } + }) + .collect(), + ) +} + /// Stabilization plan: corrections plus framing. pub fn plan(a: &WarpAnalysis, s: &StabSettings) -> Plan { - let mut b = corrections(a, s); + use rayon::prelude::*; + let base = corrections(a, s); + let mut b: Vec = match meshes(a, s, &base) { + Some(m) => m.into_iter().map(Warp::M).collect(), + None => base.into_iter().map(Warp::H).collect(), + }; 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 fracs = |b: &[Warp]| b.par_iter().map(|w| w.valid(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); @@ -435,22 +594,24 @@ pub fn plan(a: &WarpAnalysis, s: &StabSettings) -> Plan { // 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; + let lam: Vec = (0..n) + .into_par_iter() + .map(|t| { + if fr[t] >= target { + return 1.0; } - } - lam[t] = lo; - } + let (mut lo, mut hi) = (0.0, 1.0); + for _ in 0..24 { + let mid = 0.5 * (lo + hi); + if b[t].relax(mid).valid(size) >= target { + lo = mid; + } else { + hi = mid; + } + } + lo + }) + .collect(); // 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); @@ -468,7 +629,7 @@ pub fn plan(a: &WarpAnalysis, s: &StabSettings) -> Plan { (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(); + b = b.iter().zip(&smooth).map(|(w, l)| if *l >= 1.0 { w.clone() } else { w.relax(*l) }).collect(); fr = fracs(&b); vmin = fr.iter().copied().fold(1.0, f64::min); } @@ -483,13 +644,23 @@ pub fn plan(a: &WarpAnalysis, s: &StabSettings) -> Plan { } }; 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 } + let framing = z_add.then_after(&z); + let warps = b.par_iter().map(|w| framing.then_after(&w.homography())).collect(); + let meshes = b + .into_iter() + .filter_map(|w| match w { + Warp::M(m) => Some(m), + Warp::H(_) => None, + }) + .collect(); + Plan { warps, meshes, framing, crop, auto_scale: auto, valid_fraction: vmin } } #[cfg(test)] mod tests { use super::*; + use effectcraft_raster::Image; + use rayon::prelude::*; /// A synthetic analysis: a slow pan plus per-frame jitter. fn shaky(n: usize) -> (WarpAnalysis, Vec<[f64; 2]>) { @@ -497,7 +668,7 @@ mod tests { 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 mut a = WarpAnalysis { version: 1, start: 0.0, frame_duration: 1.0 / 30.0, size: [640.0, 360.0], detailed: false, frames: vec![], tracks: vec![] }; let path: Vec<[f64; 2]> = (0..n).map(pos).collect(); for k in 0..n { let mut f = FrameMotion::default(); @@ -573,4 +744,139 @@ mod tests { let j = WarpAnalysis::from_json(&a.to_json()).unwrap(); assert_eq!(j, a); } + + // ------------------------------------------------------------ synthetic shaky footage + + fn hash(i: i64, j: i64, k: i64) -> f64 { + let mut v = (i.wrapping_mul(73_856_093) ^ j.wrapping_mul(19_349_663) ^ k.wrapping_mul(83_492_791)) as u64; + v ^= v >> 13; + v = v.wrapping_mul(0x5bd1_e995); + v ^= v >> 15; + (v % 10_000) as f64 / 10_000.0 + } + + fn value_noise(x: f64, y: f64, cell: f64, seed: i64) -> f64 { + let (fx, fy) = (x / cell, y / cell); + let (i, j) = (fx.floor() as i64, fy.floor() as i64); + let (u, v) = (fx - i as f64, fy - j as f64); + let (u, v) = (u * u * (3.0 - 2.0 * u), v * v * (3.0 - 2.0 * v)); + let a = hash(i, j, seed) * (1.0 - u) + hash(i + 1, j, seed) * u; + let b = hash(i, j + 1, seed) * (1.0 - u) + hash(i + 1, j + 1, seed) * u; + a * (1.0 - v) + b * v + } + + fn texture(x: f64, y: f64) -> f32 { + (0.55 * value_noise(x, y, 5.0, 1) + 0.45 * value_noise(x, y, 13.0, 2)) as f32 + } + + /// A scene with depth: a near bump in the middle moves `1 + depth` times as much as the + /// background when the camera shakes (parallax a homography cannot model). + struct Scene { + size: [f64; 2], + cams: Vec<[f64; 2]>, + } + + impl Scene { + fn new(n: usize) -> Scene { + let cams = (0..n) + .map(|t| { + let j = [5.0 * (hash(t as i64, 7, 3) - 0.5), 5.0 * (hash(t as i64, 11, 5) - 0.5)]; + [0.4 * t as f64 + j[0], 0.15 * t as f64 + j[1]] + }) + .collect(); + Scene { size: [320.0, 180.0], cams } + } + fn depth(&self, s: [f64; 2]) -> f64 { + 1.0 + 1.2 * (-((s[0] - 160.0).powi(2) + (s[1] - 90.0).powi(2)) / (2.0 * 38.0f64.powi(2))).exp() + } + /// Where scene point `s` appears on frame `t`. + fn image_of(&self, t: usize, s: [f64; 2]) -> [f64; 2] { + let (c, d) = (self.cams[t], self.depth(s)); + [s[0] + c[0] * d, s[1] + c[1] * d] + } + fn frame(&self, t: usize) -> Image { + let (w, h) = (self.size[0] as u32, self.size[1] as u32); + let mut im = Image::new(w, h); + let c = self.cams[t]; + im.rows_mut().for_each(|(y, row)| { + for (x, px) in row.iter_mut().enumerate() { + let p = [x as f64 + 0.5, y as f64 + 0.5]; + let mut s = p; + for _ in 0..10 { + let d = self.depth(s); + s = [p[0] - c[0] * d, p[1] - c[1] * d]; + } + let v = texture(s[0], s[1]); + *px = [v, v, v, 1.0]; + } + }); + im + } + } + + fn residual_jitter(scene: &Scene, out: impl Fn(usize, [f64; 2]) -> Option<[f64; 2]>) -> f64 { + let n = scene.cams.len(); + let mut tot = 0.0; + let mut cnt = 0; + for gy in 0..5 { + for gx in 0..7 { + let s = [70.0 + gx as f64 * 30.0, 45.0 + gy as f64 * 22.0]; + let ys: Vec> = (0..n).map(|t| out(t, scene.image_of(t, s))).collect(); + for t in 6..n - 6 { + if let (Some(a), Some(b), Some(c)) = (ys[t - 1], ys[t], ys[t + 1]) { + tot += (c[0] - 2.0 * b[0] + a[0]).hypot(c[1] - 2.0 * b[1] + a[1]); + cnt += 1; + } + } + } + } + tot / cnt.max(1) as f64 + } + + #[test] + fn subspace_warp_beats_perspective_on_parallax() { + let n = 48; + let scene = Scene::new(n); + let mut an = Analyzer::new(scene.size, AnalyzeOpts::default()); + for t in 0..n { + let img = scene.frame(t); + an.push(&Frame { img: &img, offset: [0.0; 2] }); + } + let a = an.finish(0.0, 1.0 / 24.0); + assert!(a.tracks.len() >= 40, "trajectories: {}", a.tracks.len()); + // The analysis round-trips with its trajectories. + assert_eq!(WarpAnalysis::from_json(&a.to_json()).unwrap(), a); + let base = StabSettings { framing: Framing::StabilizeOnly, fps: 24.0, ..Default::default() }; + let before = residual_jitter(&scene, |_, x| Some(x)); + let ppl = plan(&a, &StabSettings { method: Method::Perspective, ..base }); + assert!(ppl.meshes.is_empty()); + let persp = residual_jitter(&scene, |t, x| ppl.output_of(t, x)); + let spl = plan(&a, &StabSettings { method: Method::SubspaceWarp, ..base }); + assert_eq!(spl.meshes.len(), n); + let sub = residual_jitter(&scene, |t, x| spl.output_of(t, x)); + let epl = plan(&a, &StabSettings { method: Method::SubspaceWarp, ripple: Ripple::Enhanced, ..base }); + let enh = residual_jitter(&scene, |t, x| epl.output_of(t, x)); + eprintln!("jitter: before {before:.3}, perspective {persp:.3}, subspace {sub:.3}, enhanced {enh:.3}"); + assert!(persp < 0.8 * before, "perspective {persp} vs {before}"); + assert!(sub < 0.6 * persp, "subspace {sub} vs perspective {persp}"); + assert!(enh < 0.6 * persp, "enhanced {enh} vs perspective {persp}"); + // source_of and output_of agree. + let q = [150.0, 80.0]; + let o = spl.output_of(10, q).unwrap(); + let back = spl.source_of(10, o).unwrap(); + assert!((back[0] - q[0]).abs() < 1e-3 && (back[1] - q[1]).abs() < 1e-3); + // Auto-scale framing covers the frame with mesh warps too. + let fpl = plan(&a, &StabSettings { method: Method::SubspaceWarp, fps: 24.0, ..Default::default() }); + assert!(fpl.auto_scale >= 1.0); + for k in [0, n / 2, n - 1] { + for p in [[1.0, 1.0], [319.0, 1.0], [319.0, 179.0], [1.0, 179.0]] { + let q = fpl.source_of(k, p).unwrap(); + let inside = q[0] >= -0.5 && q[1] >= -0.5 && q[0] <= 320.5 && q[1] <= 180.5; + assert!(inside || fpl.crop.is_some(), "frame {k}: {p:?} → {q:?}"); + } + } + // Preserve Scale keeps working with meshes. + let psp = plan(&a, &StabSettings { method: Method::SubspaceWarp, preserve_scale: true, ..base }); + assert_eq!(psp.meshes.len(), n); + } } diff --git a/crates/track/src/subspace.rs b/crates/track/src/subspace.rs new file mode 100644 index 00000000..339f9708 --- /dev/null +++ b/crates/track/src/subspace.rs @@ -0,0 +1,447 @@ +//! Warp Stabilizer ▸ Method ▸ **Subspace Warp**: content-preserving warps fitted to smoothed +//! feature trajectories, clean-room from the published method: +//! +//! - Liu, Gleicher, Wang, Jin & Agarwala, "Subspace Video Stabilization", ACM TOG 30(1), 2011: +//! the feature trajectories of a short window form a matrix `M` (two rows per trajectory, one +//! column per frame) that is close to low rank (rank 9 for a moving camera over a static +//! scene); its factorisation `M ≈ W C` gives *eigen-trajectories* `C`, which are smoothed (here +//! with a Gaussian) instead of the trajectories themselves. Every trajectory's smoothed +//! position is `W_i Ĉ_t`; trajectories that do not span the whole window get their +//! coefficients `W_i` by least squares over the frames they cover (the paper's handling of +//! incomplete trajectories in a moving factorisation). Smoothing in the subspace keeps the +//! smoothed trajectories consistent with one another, so the warp does not tear the scene. +//! - Liu, Gleicher, Jin & Agarwala, "Content-Preserving Warps for 3D Video Stabilization", ACM +//! TOG 28(3), 2009: each frame is warped by a grid mesh fitted to the feature correspondences +//! (original → smoothed position) in least squares, with a **similarity term** asking every +//! grid triangle to keep its shape (`V1 = V2 + u (V3 − V2) + v R90 (V3 − V2)` with `u, v` from +//! the undeformed grid) so the warp bends where the scene's depth demands it but stays locally +//! rigid elsewhere. +//! +//! The mesh is stored *inversely*: a regular grid over the stabilised frame whose vertices hold +//! the source position they show ([`Mesh`]), so rendering is a bilinear lookup per output pixel. +//! The data term is the same linear constraint read the other way (the smoothed position's grid +//! cell, bilinearly, must land on the original position). A weak prior ties every vertex to the +//! frame's perspective correction, which fills feature-less regions and keeps the system well +//! posed. + +use rayon::prelude::*; + +use crate::camtrack::Track2D; +use crate::camtrack::linalg::{cholesky_solve, sym_eigen}; +use crate::fit::{Model, least_squares}; +use crate::solve::Homography; + +/// A warp mesh: a regular `cols × rows` grid over the stabilised frame (`size`), each vertex +/// holding the source-frame position it shows. +#[derive(Clone, Debug, PartialEq)] +pub struct Mesh { + pub cols: usize, + pub rows: usize, + pub size: [f64; 2], + /// `(rows + 1) × (cols + 1)` source positions, row-major. + pub verts: Vec<[f64; 2]>, +} + +impl Mesh { + /// The identity mesh. + pub fn identity(cols: usize, rows: usize, size: [f64; 2]) -> Mesh { + let mut m = Mesh { cols: cols.max(1), rows: rows.max(1), size, verts: vec![] }; + m.verts = (0..=m.rows).flat_map(|j| (0..=m.cols).map(move |i| (i, j))).map(|(i, j)| m.grid(i, j)).collect(); + m + } + + /// The mesh of a homography (`h`: source → stabilised). + pub fn from_homography(cols: usize, rows: usize, size: [f64; 2], h: &Homography) -> Mesh { + let inv = h.inverse().unwrap_or(Homography::IDENTITY); + let mut m = Mesh::identity(cols, rows, size); + for v in m.verts.iter_mut() { + *v = inv.apply(*v); + } + m + } + + fn cell(&self) -> [f64; 2] { + [self.size[0] / self.cols as f64, self.size[1] / self.rows as f64] + } + + /// The undeformed position of vertex `(i, j)` (column, row). + pub fn grid(&self, i: usize, j: usize) -> [f64; 2] { + let c = self.cell(); + [i as f64 * c[0], j as f64 * c[1]] + } + + fn idx(&self, i: usize, j: usize) -> usize { + j * (self.cols + 1) + i + } + + /// Cell and bilinear coordinates of a stabilised point (the outer cells extend beyond the + /// frame, so points outside it extrapolate). + fn locate(&self, p: [f64; 2]) -> (usize, usize, f64, f64) { + let c = self.cell(); + let fx = p[0] / c[0]; + let fy = p[1] / c[1]; + let i = (fx.floor().max(0.0) as usize).min(self.cols - 1); + let j = (fy.floor().max(0.0) as usize).min(self.rows - 1); + (i, j, fx - i as f64, fy - j as f64) + } + + /// The source position shown at stabilised point `p`. + pub fn sample(&self, p: [f64; 2]) -> [f64; 2] { + let (i, j, u, v) = self.locate(p); + let a = self.verts[self.idx(i, j)]; + let b = self.verts[self.idx(i + 1, j)]; + let c = self.verts[self.idx(i, j + 1)]; + let d = self.verts[self.idx(i + 1, j + 1)]; + let w = [(1.0 - u) * (1.0 - v), u * (1.0 - v), (1.0 - u) * v, u * v]; + [0, 1].map(|k| w[0] * a[k] + w[1] * b[k] + w[2] * c[k] + w[3] * d[k]) + } + + /// Where source point `q` lands in the stabilised frame (Newton iterations on + /// [`Mesh::sample`]); `None` when it does not converge (a folded mesh). + pub fn forward(&self, q: [f64; 2]) -> Option<[f64; 2]> { + // Start from the mesh's own offset at q. + let s = self.sample(q); + let mut p = [2.0 * q[0] - s[0], 2.0 * q[1] - s[1]]; + for _ in 0..30 { + let f = self.sample(p); + let e = [f[0] - q[0], f[1] - q[1]]; + if e[0].hypot(e[1]) < 1e-6 { + return Some(p); + } + let h = 0.5; + let fx = self.sample([p[0] + h, p[1]]); + let fy = self.sample([p[0], p[1] + h]); + let j = [[(fx[0] - f[0]) / h, (fy[0] - f[0]) / h], [(fx[1] - f[1]) / h, (fy[1] - f[1]) / h]]; + let det = j[0][0] * j[1][1] - j[0][1] * j[1][0]; + if det.abs() < 1e-12 { + return None; + } + p[0] -= (j[1][1] * e[0] - j[0][1] * e[1]) / det; + p[1] -= (-j[1][0] * e[0] + j[0][0] * e[1]) / det; + } + let f = self.sample(p); + ((f[0] - q[0]).hypot(f[1] - q[1]) < 1e-3).then_some(p) + } + + /// `(1 − t) · identity + t · self` per vertex. + pub fn lerp_identity(&self, t: f64) -> Mesh { + let mut m = self.clone(); + for j in 0..=self.rows { + for i in 0..=self.cols { + let g = self.grid(i, j); + let k = self.idx(i, j); + m.verts[k] = [g[0] + (self.verts[k][0] - g[0]) * t, g[1] + (self.verts[k][1] - g[1]) * t]; + } + } + m + } + + /// Largest `s ∈ [0, 1]` such that the centred `s·W × s·H` rectangle of the stabilised frame + /// shows only source pixels (its outline, sampled densely, maps inside the source frame). + pub fn valid_fraction(&self) -> f64 { + let [w, h] = self.size; + let c = [w / 2.0, h / 2.0]; + let tol = 1e-6; + let inside = |q: [f64; 2]| q[0] >= -tol && q[1] >= -tol && q[0] <= w + tol && q[1] <= h + tol; + let fits = |s: f64| { + let (hw, hh) = (s * w / 2.0, s * h / 2.0); + let n = 48; + (0..=n).all(|k| { + let t = k as f64 / n as f64; + let x = c[0] - hw + 2.0 * hw * t; + let y = c[1] - hh + 2.0 * hh * t; + inside(self.sample([x, c[1] - hh])) + && inside(self.sample([x, c[1] + hh])) + && inside(self.sample([c[0] - hw, y])) + && inside(self.sample([c[0] + hw, y])) + }) + }; + 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..24 { + let mid = 0.5 * (lo + hi); + if fits(mid) { + lo = mid; + } else { + hi = mid; + } + } + lo + } + + /// The homography (source → stabilised) closest to the mesh in least squares. + pub fn homography(&self) -> Homography { + let mut src = vec![]; + let mut dst = vec![]; + for j in 0..=self.rows { + for i in 0..=self.cols { + src.push(self.verts[self.idx(i, j)]); + dst.push(self.grid(i, j)); + } + } + least_squares(Model::Homography, &src, &dst).unwrap_or(Homography::IDENTITY) + } +} + +/// Mesh resolution and rigidity. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct MeshOpts { + pub cols: usize, + pub rows: usize, + /// Weight of the similarity (shape-preserving) term relative to one feature. + pub alpha: f64, + /// Weight of the prior towards the frame's perspective correction, per vertex. + pub prior: f64, +} + +impl Default for MeshOpts { + fn default() -> Self { + MeshOpts { cols: 12, rows: 8, alpha: 1.0, prior: 0.02 } + } +} + +/// Normal equations `AᵀA x = Aᵀb`, accumulated row by row. +struct Normal { + n: usize, + a: Vec, + b: Vec, +} + +impl Normal { + fn row(&mut self, coefs: &[(usize, f64)], rhs: f64, w: f64) { + for (ia, ca) in coefs { + self.b[*ia] += w * ca * rhs; + for (ib, cb) in coefs { + self.a[ia * self.n + ib] += w * ca * cb; + } + } + } +} + +/// Fit the content-preserving warp of one frame: `pairs` are (source position, wanted +/// stabilised position) of its features; `base` is the frame's perspective correction (source → +/// stabilised), the prior. +pub fn solve_mesh(size: [f64; 2], o: &MeshOpts, pairs: &[([f64; 2], [f64; 2])], base: &Homography) -> Mesh { + let mut m = Mesh::from_homography(o.cols, o.rows, size, base); + let nv = (m.cols + 1) * (m.rows + 1); + let n = 2 * nv; + let mut ne = Normal { n, a: vec![0.0; n * n], b: vec![0.0; n] }; + // Prior. + for k in 0..nv { + let v = m.verts[k]; + ne.row(&[(2 * k, 1.0)], v[0], o.prior); + ne.row(&[(2 * k + 1, 1.0)], v[1], o.prior); + } + // Data: the bilinear combination of the wanted position's cell lands on the source position. + let mut used = 0; + for (src, dst) in pairs { + if !(dst[0] >= 0.0 && dst[1] >= 0.0 && dst[0] <= size[0] && dst[1] <= size[1] && src[0].is_finite() && src[1].is_finite()) { + continue; + } + let (i, j, u, v) = m.locate(*dst); + let ks = [m.idx(i, j), m.idx(i + 1, j), m.idx(i, j + 1), m.idx(i + 1, j + 1)]; + let w = [(1.0 - u) * (1.0 - v), u * (1.0 - v), (1.0 - u) * v, u * v]; + for d in 0..2 { + let coefs: Vec<(usize, f64)> = ks.iter().zip(w).map(|(k, w)| (2 * k + d, w)).collect(); + ne.row(&coefs, src[d], 1.0); + } + used += 1; + } + if used == 0 { + return m; + } + // Similarity: four triangles per cell, each corner against its two cell neighbours. + let a2 = o.alpha * o.alpha; + for j in 0..m.rows { + for i in 0..m.cols { + let (a, b, c, d) = ((i, j), (i + 1, j), (i + 1, j + 1), (i, j + 1)); + for (v1, v2, v3) in [(a, b, d), (b, c, a), (c, d, b), (d, a, c)] { + let (g1, g2, g3) = (m.grid(v1.0, v1.1), m.grid(v2.0, v2.1), m.grid(v3.0, v3.1)); + let dv = [g3[0] - g2[0], g3[1] - g2[1]]; + let e = [g1[0] - g2[0], g1[1] - g2[1]]; + let l2 = dv[0] * dv[0] + dv[1] * dv[1]; + // R90(d) = (−d.y, d.x). + let u = (e[0] * dv[0] + e[1] * dv[1]) / l2; + let v = (e[0] * -dv[1] + e[1] * dv[0]) / l2; + let (k1, k2, k3) = (m.idx(v1.0, v1.1), m.idx(v2.0, v2.1), m.idx(v3.0, v3.1)); + // x: V1x − V2x − u (V3x − V2x) + v (V3y − V2y) = 0 + ne.row(&[(2 * k1, 1.0), (2 * k2, u - 1.0), (2 * k3, -u), (2 * k3 + 1, v), (2 * k2 + 1, -v)], 0.0, a2); + // y: V1y − V2y − u (V3y − V2y) − v (V3x − V2x) = 0 + ne.row(&[(2 * k1 + 1, 1.0), (2 * k2 + 1, u - 1.0), (2 * k3 + 1, -u), (2 * k3, -v), (2 * k2, v)], 0.0, a2); + } + } + } + let mut x = ne.b; + if cholesky_solve(&mut ne.a, n, &mut x) { + for k in 0..nv { + m.verts[k] = [x[2 * k], x[2 * k + 1]]; + } + } + m +} + +/// Gaussian weights of the frames `lo..=hi` around `t`. +fn gauss(lo: usize, hi: usize, t: usize, sigma: f64) -> Vec { + let g: Vec = (lo..=hi).map(|j| (-((j as f64 - t as f64).powi(2)) / (2.0 * sigma * sigma)).exp()).collect(); + let s: f64 = g.iter().sum(); + g.into_iter().map(|v| v / s.max(1e-300)).collect() +} + +/// Subspace-smoothed feature positions: for every frame `t < frames`, the (original, smoothed) +/// positions of the trajectories seen on it. `sigma`: Gaussian smoothing in frames; `rank`: the +/// subspace dimension (9 in the paper). Frames without enough trajectories get no pairs. +pub fn smooth_trajectories(tracks: &[Track2D], frames: usize, sigma: f64, rank: usize) -> Vec> { + let max_r = ((3.0 * sigma).ceil() as usize).clamp(1, 30); + (0..frames) + .into_par_iter() + .map(|t| { + let t32 = t as u32; + let mut r = max_r; + // The window around t: shrink it until enough trajectories span it. + let (lo, hi, complete) = loop { + let lo = t.saturating_sub(r); + let hi = (t + r).min(frames.saturating_sub(1)); + let complete: Vec<&Track2D> = tracks.iter().filter(|tr| tr.start as usize <= lo && tr.end() as usize > hi).collect(); + if complete.len() >= 8 || r <= 2 { + break (lo, hi, complete); + } + r = (r * 2 / 3).max(2); + }; + if complete.len() < 4 { + return vec![]; + } + let m = hi - lo + 1; + let rows = 2 * complete.len(); + let k = rank.min(m).min(rows).max(1); + // M: rows × m (row-major). + let mut mat = vec![0.0; rows * m]; + for (i, tr) in complete.iter().enumerate() { + for c in 0..m { + let p = tr.at((lo + c) as u32).unwrap_or([0.0; 2]); + mat[(2 * i) * m + c] = p[0]; + mat[(2 * i + 1) * m + c] = p[1]; + } + } + // Gram MᵀM and its top eigenvectors: the eigen-trajectories (orthonormal rows of C). + let mut gram = vec![0.0; m * m]; + for a in 0..m { + for b in a..m { + let s: f64 = (0..rows).map(|r| mat[r * m + a] * mat[r * m + b]).sum(); + gram[a * m + b] = s; + gram[b * m + a] = s; + } + } + let (_, vecs) = sym_eigen(&gram, m); + // C[q][c] = vecs[c][m − 1 − q] (largest first). + let cmat: Vec> = (0..k).map(|q| (0..m).map(|c| vecs[c * m + (m - 1 - q)]).collect()).collect(); + let g = gauss(lo, hi, t, sigma.max(1e-6)); + let chat: Vec = cmat.iter().map(|row| row.iter().zip(&g).map(|(a, b)| a * b).sum()).collect(); + let tc = t - lo; + let mut out = Vec::new(); + // Complete trajectories: W = M Cᵀ. + for (i, _) in complete.iter().enumerate() { + let src = [mat[(2 * i) * m + tc], mat[(2 * i + 1) * m + tc]]; + let mut dst = [0.0; 2]; + for (d, slot) in dst.iter_mut().enumerate() { + let row = &mat[(2 * i + d) * m..(2 * i + d + 1) * m]; + for q in 0..k { + let w: f64 = row.iter().zip(&cmat[q]).map(|(a, b)| a * b).sum(); + *slot += w * chat[q]; + } + } + out.push((src, dst)); + } + // Incomplete trajectories seen on t: coefficients by least squares over the frames + // they cover in the window. + let need = (k + 3).max(m / 2); + for tr in tracks.iter().filter(|tr| tr.at(t32).is_some() && !(tr.start as usize <= lo && tr.end() as usize > hi)) { + let cols: Vec = (0..m).filter(|c| tr.at((lo + c) as u32).is_some()).collect(); + if cols.len() < need { + continue; + } + let mut ata = vec![0.0; k * k]; + let mut atb = [vec![0.0; k], vec![0.0; k]]; + for &c in &cols { + let p = tr.at((lo + c) as u32).expect("covered"); + for a in 0..k { + for d in 0..2 { + atb[d][a] += cmat[a][c] * p[d]; + } + for b in 0..k { + ata[a * k + b] += cmat[a][c] * cmat[b][c]; + } + } + } + for a in 0..k { + ata[a * k + a] += 1e-9; + } + let mut dst = [0.0; 2]; + let mut ok = true; + for d in 0..2 { + let mut x = atb[d].clone(); + let mut aa = ata.clone(); + if !cholesky_solve(&mut aa, k, &mut x) { + ok = false; + break; + } + dst[d] = x.iter().zip(&chat).map(|(a, b)| a * b).sum(); + } + if ok { + out.push((tr.at(t32).expect("seen"), dst)); + } + } + out + }) + .collect() +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn mesh_identity_sample_and_forward() { + let m = Mesh::identity(4, 3, [100.0, 60.0]); + assert_eq!(m.sample([37.0, 21.0]), [37.0, 21.0]); + assert_eq!(m.valid_fraction(), 1.0); + let h = Homography([[1.0, 0.02, 5.0], [-0.01, 1.0, -3.0], [0.0, 0.0, 1.0]]); + let mh = Mesh::from_homography(4, 3, [100.0, 60.0], &h); + // Affine maps are exact on a bilinear mesh. + let q = [40.0, 30.0]; + let p = mh.forward(q).unwrap(); + let e = h.apply(q); + assert!((p[0] - e[0]).abs() < 1e-6 && (p[1] - e[1]).abs() < 1e-6, "{p:?} {e:?}"); + assert!(mh.homography().max_diff(&h) < 1e-6); + assert!(mh.valid_fraction() < 1.0); + } + + #[test] + fn mesh_fits_a_translation_and_bends_for_local_motion() { + let size = [200.0, 120.0]; + let mut pairs = vec![]; + for y in 0..12 { + for x in 0..20 { + let p = [5.0 + x as f64 * 10.0, 5.0 + y as f64 * 10.0]; + // Everything moves by (3, −2); a blob near the centre by (8, −2). + let bump = (-((p[0] - 100.0).powi(2) + (p[1] - 60.0).powi(2)) / (2.0 * 20.0f64.powi(2))).exp(); + pairs.push((p, [p[0] + 3.0 + 5.0 * bump, p[1] - 2.0])); + } + } + let base = Homography::translation([3.0, -2.0]); + let m = solve_mesh(size, &MeshOpts { cols: 10, rows: 6, alpha: 0.5, prior: 0.01 }, &pairs, &base); + let mut err_mesh = 0.0; + let mut err_h = 0.0; + for (s, d) in &pairs { + let q = m.sample(*d); + err_mesh += (q[0] - s[0]).hypot(q[1] - s[1]); + let qh = base.inverse().unwrap().apply(*d); + err_h += (qh[0] - s[0]).hypot(qh[1] - s[1]); + } + assert!(err_mesh < 0.35 * err_h, "{err_mesh} vs {err_h}"); + } +}