mirror of
https://github.com/storytold/effectcraft.git
synced 2026-10-25 18:47:07 -06:00
646 lines
24 KiB
Rust
646 lines
24 KiB
Rust
//! Motion tracking, clean-room from textbook methods:
|
||
//!
|
||
//! - each **track point** has a *feature region* (the pattern to follow) and a *search region*
|
||
//! (where to look for it in the next frame), as in After Effects' Tracker;
|
||
//! - the pattern is located by **normalized cross-correlation** (Lewis, "Fast Normalized
|
||
//! Cross-Correlation", 1995) searched **coarse to fine over an image pyramid**;
|
||
//! - the integer match is refined to **sub-pixel** precision (and, optionally, the feature's
|
||
//! rotation and scale) by **Lucas–Kanade** Gauss–Newton image alignment (Lucas & Kanade 1981;
|
||
//! Baker & Matthews, "Lucas-Kanade 20 Years On", 2004) with a gain/bias photometric model;
|
||
//! - **confidence** is the correlation of the template with the matched region, in percent;
|
||
//! when it drops below a threshold the tracker continues, stops, extrapolates the motion or
|
||
//! adapts the feature (re-samples the template), like AE's Motion Tracker Options.
|
||
//!
|
||
//! Only the pixels around each search region are converted and pre-processed, so tracking cost is
|
||
//! independent of the frame size. Points are tracked in parallel and the coarse correlation search
|
||
//! is parallel over candidate rows (rayon).
|
||
//!
|
||
//! 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 camtrack;
|
||
pub mod fit;
|
||
pub mod klt;
|
||
pub mod mask;
|
||
pub mod plane;
|
||
pub mod roto;
|
||
pub mod solve;
|
||
pub mod stabilize;
|
||
|
||
use effectcraft_raster::Image;
|
||
use rayon::prelude::*;
|
||
use serde::{Deserialize, Serialize};
|
||
|
||
pub use plane::{Channel, Plane};
|
||
pub use solve::{Homography, angle_and_length, unwrap_degrees};
|
||
|
||
/// What to do when a frame's confidence is below the threshold.
|
||
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
|
||
#[serde(rename_all = "camelCase")]
|
||
pub enum ConfidenceAction {
|
||
/// Keep the match anyway.
|
||
Continue,
|
||
/// Stop the analysis before this frame.
|
||
Stop,
|
||
/// Ignore the match and continue the previous motion.
|
||
Extrapolate,
|
||
/// Keep the match and re-sample the feature from this frame.
|
||
#[default]
|
||
Adapt,
|
||
}
|
||
|
||
impl ConfidenceAction {
|
||
pub const ALL: [ConfidenceAction; 4] = [ConfidenceAction::Continue, ConfidenceAction::Stop, ConfidenceAction::Extrapolate, ConfidenceAction::Adapt];
|
||
pub fn label(self) -> &'static str {
|
||
match self {
|
||
ConfidenceAction::Continue => "Continue Tracking",
|
||
ConfidenceAction::Stop => "Stop Tracking",
|
||
ConfidenceAction::Extrapolate => "Extrapolate Motion",
|
||
ConfidenceAction::Adapt => "Adapt Feature",
|
||
}
|
||
}
|
||
pub fn from_name(s: &str) -> Option<ConfidenceAction> {
|
||
let n = s.to_ascii_lowercase().replace([' ', '_', '-'], "");
|
||
ConfidenceAction::ALL.into_iter().find(|a| {
|
||
format!("{a:?}").to_ascii_lowercase() == n
|
||
|| a.label().to_ascii_lowercase().replace(' ', "") == n
|
||
|| a.label().to_ascii_lowercase().replace(' ', "").starts_with(&n)
|
||
})
|
||
}
|
||
}
|
||
|
||
/// Motion Tracker Options that affect matching.
|
||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||
#[serde(default, rename_all = "camelCase")]
|
||
pub struct TrackOptions {
|
||
pub channel: Channel,
|
||
/// Pre-process blur radius in pixels (0 = off).
|
||
pub blur: f64,
|
||
/// Pre-process edge enhancement.
|
||
pub enhance: bool,
|
||
/// Sub-pixel positioning (else whole-pixel matches).
|
||
pub subpixel: bool,
|
||
/// Re-sample the feature on every frame.
|
||
pub adapt_every_frame: bool,
|
||
/// Confidence threshold in percent for `action`.
|
||
pub threshold: f64,
|
||
pub action: ConfidenceAction,
|
||
/// Also estimate each feature's rotation and scale while matching (follows rotating or
|
||
/// zooming features without adapting).
|
||
pub track_shape: bool,
|
||
}
|
||
|
||
impl Default for TrackOptions {
|
||
fn default() -> Self {
|
||
TrackOptions {
|
||
channel: Channel::Luminance,
|
||
blur: 0.0,
|
||
enhance: false,
|
||
subpixel: true,
|
||
adapt_every_frame: false,
|
||
threshold: 80.0,
|
||
action: ConfidenceAction::Adapt,
|
||
track_shape: true,
|
||
}
|
||
}
|
||
}
|
||
|
||
/// One track point's regions (layer pixels).
|
||
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
|
||
#[serde(rename_all = "camelCase")]
|
||
pub struct PointSpec {
|
||
pub center: [f64; 2],
|
||
/// Feature region width and height.
|
||
pub feature_size: [f64; 2],
|
||
/// Search region centre relative to the feature centre.
|
||
pub search_offset: [f64; 2],
|
||
/// Search region width and height.
|
||
pub search_size: [f64; 2],
|
||
}
|
||
|
||
/// A frame to track in: layer pixels map to image pixels by adding `offset`.
|
||
#[derive(Clone, Copy)]
|
||
pub struct Frame<'a> {
|
||
pub img: &'a Image,
|
||
pub offset: [f64; 2],
|
||
}
|
||
|
||
impl<'a> Frame<'a> {
|
||
pub fn new(img: &'a Image) -> Frame<'a> {
|
||
Frame { img, offset: [0.0; 2] }
|
||
}
|
||
}
|
||
|
||
/// How a point's position on a frame was obtained.
|
||
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
|
||
#[serde(rename_all = "camelCase")]
|
||
pub enum Status {
|
||
Tracked,
|
||
/// Tracked, and the feature was re-sampled from this frame.
|
||
Adapted,
|
||
/// Low confidence: the previous motion was continued.
|
||
Extrapolated,
|
||
/// Low confidence with the Stop action: no result for this frame.
|
||
Stopped,
|
||
}
|
||
|
||
/// A point's result on one frame.
|
||
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
|
||
#[serde(rename_all = "camelCase")]
|
||
pub struct PointResult {
|
||
/// Feature centre (layer pixels).
|
||
pub center: [f64; 2],
|
||
/// Match confidence, 0–100 %.
|
||
pub confidence: f64,
|
||
/// Feature rotation (degrees) and scale (1 = start) relative to the start frame (when
|
||
/// `track_shape` is on).
|
||
pub rotation: f64,
|
||
pub scale: f64,
|
||
pub status: Status,
|
||
}
|
||
|
||
/// A template sampled on a unit grid around the feature centre at one pyramid level.
|
||
#[derive(Clone, Debug)]
|
||
struct Patch {
|
||
r: [i32; 2],
|
||
ch: usize,
|
||
raw: Vec<f32>,
|
||
/// Zero-mean values and their sum of squares.
|
||
zm: Vec<f32>,
|
||
ss: f64,
|
||
}
|
||
|
||
impl Patch {
|
||
fn sample(plane: &Plane, c: [f64; 2], a: [f64; 2], r: [i32; 2]) -> Patch {
|
||
let ch = plane.ch;
|
||
let mut raw = Vec::with_capacity(((2 * r[0] + 1) * (2 * r[1] + 1)) as usize * ch);
|
||
let mut v = [0.0f32; 3];
|
||
for gy in -r[1]..=r[1] {
|
||
for gx in -r[0]..=r[0] {
|
||
let (ux, uy) = (gx as f64, gy as f64);
|
||
plane.sample(c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy, &mut v);
|
||
raw.extend_from_slice(&v[..ch]);
|
||
}
|
||
}
|
||
let mean = raw.iter().map(|x| *x as f64).sum::<f64>() / raw.len().max(1) as f64;
|
||
let zm: Vec<f32> = raw.iter().map(|x| (*x as f64 - mean) as f32).collect();
|
||
let ss = zm.iter().map(|x| (*x as f64) * (*x as f64)).sum();
|
||
Patch { r, ch, raw, zm, ss }
|
||
}
|
||
}
|
||
|
||
/// Normalized cross-correlation of `patch` with `plane` sampled at `c + A u` (A = similarity
|
||
/// `[[a0, -a1], [a1, a0]]`). Pure translations share one set of bilinear weights.
|
||
fn ncc(plane: &Plane, p: &Patch, c: [f64; 2], a: [f64; 2]) -> f64 {
|
||
if p.ss <= 1e-12 {
|
||
return 0.0;
|
||
}
|
||
let ch = p.ch;
|
||
let (mut s, mut s2, mut st) = (0.0f64, 0.0f64, 0.0f64);
|
||
let identity = (a[0] - 1.0).abs() < 1e-9 && a[1].abs() < 1e-9;
|
||
let fx = c[0] - 0.5;
|
||
let fy = c[1] - 0.5;
|
||
let (bx, by) = (fx.floor(), fy.floor());
|
||
let inside = bx as i64 - p.r[0] as i64 >= 0
|
||
&& by as i64 - p.r[1] as i64 >= 0
|
||
&& (bx as i64 + p.r[0] as i64 + 1) < plane.w as i64
|
||
&& (by as i64 + p.r[1] as i64 + 1) < plane.h as i64;
|
||
if identity && inside {
|
||
let (tx, ty) = ((fx - bx) as f32, (fy - by) as f32);
|
||
let (w00, w10, w01, w11) = ((1.0 - tx) * (1.0 - ty), tx * (1.0 - ty), (1.0 - tx) * ty, tx * ty);
|
||
let row = plane.w * ch;
|
||
let mut k = 0;
|
||
for gy in -p.r[1]..=p.r[1] {
|
||
let y = (by as i64 + gy as i64) as usize;
|
||
let base = y * row;
|
||
for gx in -p.r[0]..=p.r[0] {
|
||
let x = (bx as i64 + gx as i64) as usize;
|
||
let i00 = base + x * ch;
|
||
for cc in 0..ch {
|
||
let v =
|
||
w00 * plane.data[i00 + cc] + w10 * plane.data[i00 + ch + cc] + w01 * plane.data[i00 + row + cc] + w11 * plane.data[i00 + row + ch + cc];
|
||
let v = v as f64;
|
||
s += v;
|
||
s2 += v * v;
|
||
st += v * p.zm[k] as f64;
|
||
k += 1;
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
let mut v = [0.0f32; 3];
|
||
let mut k = 0;
|
||
for gy in -p.r[1]..=p.r[1] {
|
||
for gx in -p.r[0]..=p.r[0] {
|
||
let (ux, uy) = (gx as f64, gy as f64);
|
||
plane.sample(c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy, &mut v);
|
||
for cc in 0..ch {
|
||
let x = v[cc] as f64;
|
||
s += x;
|
||
s2 += x * x;
|
||
st += x * p.zm[k] as f64;
|
||
k += 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
let n = p.zm.len() as f64;
|
||
let var = s2 - s * s / n;
|
||
if var <= 1e-12 {
|
||
return 0.0;
|
||
}
|
||
(st / (var * p.ss).sqrt()).clamp(-1.0, 1.0)
|
||
}
|
||
|
||
/// The pre-processed pixels around one point: a pyramid of a crop. Plane coordinates are image
|
||
/// pixels minus `origin`, divided by `2^level`.
|
||
struct Crop {
|
||
origin: [f64; 2],
|
||
levels: Vec<Plane>,
|
||
grad0: (Plane, Plane),
|
||
}
|
||
|
||
impl Crop {
|
||
/// Crop image pixels `[c - ext, c + ext]` (image pixel coordinates), pre-processed.
|
||
fn new(frame: &Frame, opts: &TrackOptions, c: [f64; 2], ext: [f64; 2], levels: usize) -> Crop {
|
||
let sigma = opts.blur.max(0.0) / 2.0;
|
||
let margin = (sigma * 3.0).ceil() + 3.0 + if opts.enhance { 6.0 } else { 0.0 } + (1 << levels) as f64;
|
||
let x0 = (c[0] - ext[0] - margin).floor();
|
||
let y0 = (c[1] - ext[1] - margin).floor();
|
||
let w = ((2.0 * (ext[0] + margin)).ceil() as usize + 2).max(4);
|
||
let h = ((2.0 * (ext[1] + margin)).ceil() as usize + 2).max(4);
|
||
let mut p = Plane::from_image(frame.img, opts.channel, x0 as i64, y0 as i64, w, h);
|
||
if sigma > 0.05 {
|
||
p = p.gaussian(sigma);
|
||
}
|
||
if opts.enhance {
|
||
p = p.enhance();
|
||
}
|
||
let grad0 = p.gradients();
|
||
let mut lv = vec![p];
|
||
for _ in 0..levels {
|
||
let d = lv.last().map(Plane::downsample).unwrap_or_default();
|
||
lv.push(d);
|
||
}
|
||
Crop { origin: [x0, y0], levels: lv, grad0 }
|
||
}
|
||
fn to_level(&self, p: [f64; 2], k: usize) -> [f64; 2] {
|
||
let s = (1u32 << k) as f64;
|
||
[(p[0] - self.origin[0]) / s, (p[1] - self.origin[1]) / s]
|
||
}
|
||
fn level_to_image(&self, p: [f64; 2], k: usize) -> [f64; 2] {
|
||
let s = (1u32 << k) as f64;
|
||
[p[0] * s + self.origin[0], p[1] * s + self.origin[1]]
|
||
}
|
||
}
|
||
|
||
/// Per-point tracking state.
|
||
#[derive(Clone, Debug)]
|
||
struct PointState {
|
||
spec: PointSpec,
|
||
/// Current centre in image pixels.
|
||
center: [f64; 2],
|
||
/// Current similarity `[s cos θ, s sin θ]` relative to the template's frame.
|
||
shape: [f64; 2],
|
||
velocity: [f64; 2],
|
||
templates: Vec<Patch>,
|
||
levels: usize,
|
||
}
|
||
|
||
fn half(v: [f64; 2]) -> [f64; 2] {
|
||
[(v[0] / 2.0).max(1.0), (v[1] / 2.0).max(1.0)]
|
||
}
|
||
|
||
impl PointState {
|
||
fn feature_r(&self) -> [i32; 2] {
|
||
let h = half(self.spec.feature_size);
|
||
[h[0].round().max(2.0) as i32, h[1].round().max(2.0) as i32]
|
||
}
|
||
fn level_r(&self, k: usize) -> [i32; 2] {
|
||
let r = self.feature_r();
|
||
let s = (1 << k) as f64;
|
||
[((r[0] as f64 / s).ceil() as i32).max(2), ((r[1] as f64 / s).ceil() as i32).max(2)]
|
||
}
|
||
/// Search range around the predicted centre (pixels, per axis).
|
||
fn range(&self) -> [f64; 2] {
|
||
let f = half(self.spec.feature_size);
|
||
let s = half(self.spec.search_size);
|
||
[(s[0] - f[0]).max(2.0), (s[1] - f[1]).max(2.0)]
|
||
}
|
||
fn extent(&self) -> f64 {
|
||
let r = self.feature_r();
|
||
let s = (self.shape[0] * self.shape[0] + self.shape[1] * self.shape[1]).sqrt().max(1.0);
|
||
((r[0] * r[0] + r[1] * r[1]) as f64).sqrt() * s + 2.0
|
||
}
|
||
|
||
fn new(spec: PointSpec, frame: &Frame, opts: &TrackOptions) -> PointState {
|
||
let c = [spec.center[0] + frame.offset[0], spec.center[1] + frame.offset[1]];
|
||
let mut st = PointState { spec, center: c, shape: [1.0, 0.0], velocity: [0.0; 2], templates: vec![], levels: 0 };
|
||
let range = st.range();
|
||
let fr = st.feature_r();
|
||
let mut l = 0;
|
||
while l < 4 && range[0].min(range[1]) / (2u32 << l) as f64 >= 3.0 && (fr[0].min(fr[1]) as f64) / (2u32 << l) as f64 >= 3.0 {
|
||
l += 1;
|
||
}
|
||
st.levels = l;
|
||
let e = st.extent();
|
||
let crop = Crop::new(frame, opts, c, [e, e], l);
|
||
st.resample(&crop);
|
||
st
|
||
}
|
||
|
||
/// Re-sample the templates from `crop` at the current centre and shape.
|
||
fn resample(&mut self, crop: &Crop) {
|
||
self.templates = (0..=self.levels).map(|k| Patch::sample(&crop.levels[k], crop.to_level(self.center, k), self.shape, self.level_r(k))).collect();
|
||
}
|
||
|
||
fn step(&mut self, frame: &Frame, opts: &TrackOptions) -> PointResult {
|
||
let prev = self.center;
|
||
let predicted = [prev[0] + self.velocity[0], prev[1] + self.velocity[1]];
|
||
let sc = [predicted[0] + self.spec.search_offset[0], predicted[1] + self.spec.search_offset[1]];
|
||
let range = self.range();
|
||
let e = self.extent();
|
||
let crop = Crop::new(frame, opts, sc, [range[0] + e, range[1] + e], self.levels);
|
||
let top = self.levels;
|
||
// Coarse: exhaustive correlation over the search range at the top level.
|
||
let s = (1u32 << top) as f64;
|
||
let ctop = crop.to_level(sc, top);
|
||
let (rx, ry) = ((range[0] / s).ceil() as i32, (range[1] / s).ceil() as i32);
|
||
let plane = &crop.levels[top];
|
||
let tpl = &self.templates[top];
|
||
let shape = self.shape;
|
||
let mut cands: Vec<(f64, [f64; 2])> = (-ry..=ry)
|
||
.into_par_iter()
|
||
.flat_map_iter(|j| {
|
||
(-rx..=rx).map(move |i| {
|
||
let c = [ctop[0] + i as f64, ctop[1] + j as f64];
|
||
(ncc(plane, tpl, c, shape), c)
|
||
})
|
||
})
|
||
.collect();
|
||
// Best score first; ties (flat or repetitive regions) go to the candidate nearest the
|
||
// prediction so featureless areas don't drift.
|
||
let cp = crop.to_level(predicted, top);
|
||
let d2 = |c: &[f64; 2]| (c[0] - cp[0]).powi(2) + (c[1] - cp[1]).powi(2);
|
||
cands.sort_by(|a, b| if (a.0 - b.0).abs() < 1e-9 { d2(&a.1).total_cmp(&d2(&b.1)) } else { b.0.total_cmp(&a.0) });
|
||
let mut seeds: Vec<[f64; 2]> = vec![];
|
||
for (_, c) in &cands {
|
||
if seeds.iter().all(|q| (q[0] - c[0]).abs() > 1.5 || (q[1] - c[1]).abs() > 1.5) {
|
||
seeds.push(*c);
|
||
}
|
||
if seeds.len() == 3 {
|
||
break;
|
||
}
|
||
}
|
||
// Fine: refine each seed down the pyramid in a 5 × 5 neighbourhood.
|
||
let mut best = (f64::MIN, sc);
|
||
for seed in seeds {
|
||
let mut c = seed;
|
||
let mut score = f64::MIN;
|
||
for k in (0..top).rev() {
|
||
c = [c[0] * 2.0, c[1] * 2.0];
|
||
let (pl, tp) = (&crop.levels[k], &self.templates[k]);
|
||
// Start from the projected match; neighbours must beat it (no drift on ties).
|
||
let mut lb = (ncc(pl, tp, c, shape), c);
|
||
for j in -2..=2 {
|
||
for i in -2..=2 {
|
||
if i == 0 && j == 0 {
|
||
continue;
|
||
}
|
||
let q = [c[0] + i as f64, c[1] + j as f64];
|
||
let v = ncc(pl, tp, q, shape);
|
||
if v > lb.0 + 1e-9 {
|
||
lb = (v, q);
|
||
}
|
||
}
|
||
}
|
||
(score, c) = lb;
|
||
}
|
||
if top == 0 {
|
||
score = ncc(&crop.levels[0], &self.templates[0], c, shape);
|
||
}
|
||
if score > best.0 + 1e-9 {
|
||
best = (score, crop.level_to_image(c, 0));
|
||
}
|
||
}
|
||
let c0 = crop.to_level(best.1, 0);
|
||
// Sub-pixel (and shape) refinement.
|
||
let (mut c, mut a) = (c0, shape);
|
||
if opts.subpixel {
|
||
let mut ok = false;
|
||
if opts.track_shape
|
||
&& let Some((cr, ar)) = lk_refine(&crop, &self.templates[0], c0, shape, true)
|
||
{
|
||
(c, a) = (cr, ar);
|
||
ok = true;
|
||
}
|
||
if !ok {
|
||
if let Some((cr, _)) = lk_refine(&crop, &self.templates[0], c0, shape, false) {
|
||
c = cr;
|
||
} else {
|
||
c = parabolic(&crop.levels[0], &self.templates[0], c0, shape);
|
||
}
|
||
}
|
||
}
|
||
// A featureless template can't be matched: stay put.
|
||
if self.templates[0].ss < 1e-6 {
|
||
(c, a) = (crop.to_level(predicted, 0), shape);
|
||
}
|
||
let mut conf = ncc(&crop.levels[0], &self.templates[0], c, a);
|
||
// Keep the coarse match if refinement made it worse.
|
||
let coarse = ncc(&crop.levels[0], &self.templates[0], c0, shape);
|
||
if coarse > conf + 0.02 {
|
||
(c, a, conf) = (c0, shape, coarse);
|
||
}
|
||
let confidence = (conf.max(0.0) * 100.0).min(100.0);
|
||
let found = crop.level_to_image(c, 0);
|
||
let low = confidence < opts.threshold;
|
||
let mut status = Status::Tracked;
|
||
if low {
|
||
match opts.action {
|
||
ConfidenceAction::Stop => {
|
||
return self.result(prev, frame, confidence, Status::Stopped);
|
||
}
|
||
ConfidenceAction::Extrapolate => {
|
||
self.center = predicted;
|
||
return self.result(predicted, frame, confidence, Status::Extrapolated);
|
||
}
|
||
ConfidenceAction::Continue => {}
|
||
ConfidenceAction::Adapt => status = Status::Adapted,
|
||
}
|
||
}
|
||
// Only confident matches predict the next search position.
|
||
self.velocity = if low { [0.0; 2] } else { [found[0] - prev[0], found[1] - prev[1]] };
|
||
self.center = found;
|
||
self.shape = a;
|
||
if opts.adapt_every_frame || status == Status::Adapted {
|
||
status = Status::Adapted;
|
||
self.resample(&crop);
|
||
}
|
||
self.result(found, frame, confidence, status)
|
||
}
|
||
|
||
fn result(&self, c: [f64; 2], frame: &Frame, confidence: f64, status: Status) -> PointResult {
|
||
PointResult {
|
||
center: [c[0] - frame.offset[0], c[1] - frame.offset[1]],
|
||
confidence,
|
||
rotation: self.shape[1].atan2(self.shape[0]).to_degrees(),
|
||
scale: (self.shape[0] * self.shape[0] + self.shape[1] * self.shape[1]).sqrt(),
|
||
status,
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Sub-pixel peak from 1-D parabola fits of the correlation around `c`.
|
||
fn parabolic(plane: &Plane, p: &Patch, c: [f64; 2], a: [f64; 2]) -> [f64; 2] {
|
||
let f = |dx: f64, dy: f64| ncc(plane, p, [c[0] + dx, c[1] + dy], a);
|
||
let m = f(0.0, 0.0);
|
||
let off = |l: f64, r: f64| {
|
||
let d = l - 2.0 * m + r;
|
||
if d.abs() < 1e-12 { 0.0 } else { (0.5 * (l - r) / d).clamp(-0.5, 0.5) }
|
||
};
|
||
[c[0] + off(f(-1.0, 0.0), f(1.0, 0.0)), c[1] + off(f(0.0, -1.0), f(0.0, 1.0))]
|
||
}
|
||
|
||
/// Forward-additive Lucas–Kanade (Gauss–Newton) alignment of the level-0 template with gain and
|
||
/// bias: minimise Σ (I(c + A u) − g T(u) − b)² over the centre `c` and, with `shape`, the
|
||
/// similarity `A`. Returns `None` when it diverges.
|
||
fn lk_refine(crop: &Crop, p: &Patch, c0: [f64; 2], a0: [f64; 2], shape: bool) -> Option<([f64; 2], [f64; 2])> {
|
||
let plane = &crop.levels[0];
|
||
let (gxp, gyp) = &crop.grad0;
|
||
let ch = p.ch;
|
||
let (mut c, mut a) = (c0, a0);
|
||
let mut g = 1.0f64;
|
||
let mut b;
|
||
// Initial photometric estimate from means/variances.
|
||
{
|
||
let mut v = [0.0f32; 3];
|
||
let (mut si, mut si2, mut st, mut st2, mut n) = (0.0, 0.0, 0.0, 0.0, 0.0);
|
||
let mut k = 0;
|
||
for gy in -p.r[1]..=p.r[1] {
|
||
for gx in -p.r[0]..=p.r[0] {
|
||
let (ux, uy) = (gx as f64, gy as f64);
|
||
plane.sample(c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy, &mut v);
|
||
for cc in 0..ch {
|
||
let (i, t) = (v[cc] as f64, p.raw[k] as f64);
|
||
si += i;
|
||
si2 += i * i;
|
||
st += t;
|
||
st2 += t * t;
|
||
n += 1.0;
|
||
k += 1;
|
||
}
|
||
}
|
||
}
|
||
let (mi, mt) = (si / n, st / n);
|
||
let (vi, vt) = ((si2 / n - mi * mi).max(0.0), (st2 / n - mt * mt).max(0.0));
|
||
if vt > 1e-12 && vi > 1e-12 {
|
||
g = (vi / vt).sqrt();
|
||
}
|
||
b = mi - g * mt;
|
||
}
|
||
let np = if shape { 6 } else { 4 };
|
||
let (mut v, mut vx, mut vy) = ([0.0f32; 3], [0.0f32; 3], [0.0f32; 3]);
|
||
for _ in 0..30 {
|
||
let mut h = [[0.0f64; 6]; 6];
|
||
let mut rhs = [0.0f64; 6];
|
||
let mut k = 0;
|
||
for gy in -p.r[1]..=p.r[1] {
|
||
for gx in -p.r[0]..=p.r[0] {
|
||
let (ux, uy) = (gx as f64, gy as f64);
|
||
let q = [c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy];
|
||
plane.sample(q[0], q[1], &mut v);
|
||
gxp.sample(q[0], q[1], &mut vx);
|
||
gyp.sample(q[0], q[1], &mut vy);
|
||
for cc in 0..ch {
|
||
let t = p.raw[k] as f64;
|
||
k += 1;
|
||
let (ix, iy) = (vx[cc] as f64, vy[cc] as f64);
|
||
let r = v[cc] as f64 - g * t - b;
|
||
let j: [f64; 6] = if shape { [ix, iy, ix * ux + iy * uy, -ix * uy + iy * ux, -t, -1.0] } else { [ix, iy, -t, -1.0, 0.0, 0.0] };
|
||
for m in 0..np {
|
||
rhs[m] -= j[m] * r;
|
||
for n in m..np {
|
||
h[m][n] += j[m] * j[n];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
for m in 0..np {
|
||
for n in 0..m {
|
||
h[m][n] = h[n][m];
|
||
}
|
||
h[m][m] *= 1.0 + 1e-6;
|
||
h[m][m] += 1e-9;
|
||
}
|
||
let d = if shape {
|
||
solve::solve_linear(h, rhs)?
|
||
} else {
|
||
let hh = [
|
||
[h[0][0], h[0][1], h[0][2], h[0][3]],
|
||
[h[1][0], h[1][1], h[1][2], h[1][3]],
|
||
[h[2][0], h[2][1], h[2][2], h[2][3]],
|
||
[h[3][0], h[3][1], h[3][2], h[3][3]],
|
||
];
|
||
let x = solve::solve_linear(hh, [rhs[0], rhs[1], rhs[2], rhs[3]])?;
|
||
[x[0], x[1], 0.0, 0.0, x[2], x[3]]
|
||
};
|
||
// Limit a single step (Gauss-Newton overshoot on poorly textured features).
|
||
let step = (d[0] * d[0] + d[1] * d[1]).sqrt();
|
||
let k = if step > 1.0 { 1.0 / step } else { 1.0 };
|
||
c[0] += d[0] * k;
|
||
c[1] += d[1] * k;
|
||
if shape {
|
||
a[0] += d[2] * k;
|
||
a[1] += d[3] * k;
|
||
}
|
||
g += d[4] * k;
|
||
b += d[5] * k;
|
||
if !(c[0].is_finite() && c[1].is_finite() && a[0].is_finite() && a[1].is_finite()) {
|
||
return None;
|
||
}
|
||
if step < 1e-4 && (!shape || (d[2].abs() + d[3].abs()) < 1e-6) {
|
||
break;
|
||
}
|
||
}
|
||
let moved = ((c[0] - c0[0]).powi(2) + (c[1] - c0[1]).powi(2)).sqrt();
|
||
let s0 = (a0[0] * a0[0] + a0[1] * a0[1]).sqrt();
|
||
let s1 = (a[0] * a[0] + a[1] * a[1]).sqrt();
|
||
if moved > 2.5 || !(0.75..=1.33).contains(&(s1 / s0.max(1e-9))) || g <= 0.0 {
|
||
return None;
|
||
}
|
||
Some((c, a))
|
||
}
|
||
|
||
/// Tracks a set of points frame by frame (the points of one tracker).
|
||
#[derive(Clone, Debug)]
|
||
pub struct Tracker {
|
||
pub opts: TrackOptions,
|
||
points: Vec<PointState>,
|
||
}
|
||
|
||
impl Tracker {
|
||
/// Start tracking `points` as they appear on `frame` (the start frame).
|
||
pub fn new(opts: TrackOptions, points: &[PointSpec], frame: &Frame) -> Tracker {
|
||
let pts = points.par_iter().map(|s| PointState::new(*s, frame, &opts)).collect();
|
||
Tracker { opts, points: pts }
|
||
}
|
||
|
||
/// Track every point into the next frame (in either time direction).
|
||
pub fn step(&mut self, frame: &Frame) -> Vec<PointResult> {
|
||
let opts = &self.opts;
|
||
self.points.par_iter_mut().map(|p| p.step(frame, opts)).collect()
|
||
}
|
||
|
||
pub fn len(&self) -> usize {
|
||
self.points.len()
|
||
}
|
||
pub fn is_empty(&self) -> bool {
|
||
self.points.is_empty()
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests;
|