Files
effectcraft/crates/track/src/solve.rs
T
Brandon ThomasandClaude Opus 5.5 243098a144 M12.3: tracker core
New L2 crate effectcraft-track: per-point feature/search region tracking with
pyramid normalized cross-correlation and Lucas-Kanade (similarity + gain/bias)
sub-pixel refinement, confidence, adapt/stop/extrapolate/continue actions,
RGB/luminance/saturation channels, blur/enhance pre-processing, and
homography/affine/similarity solves.

Project: Motion Trackers > Tracker n > Track Point n property tree
(GroupKind::Tracker with type, target and options). Engine: track.* commands
(new/motion/stabilize/property/select/setType/setTarget/options/setPoint/
analyze/stop/apply/reset/delete/status), background or blocking analysis
job with progress and cancel (one undo step), apply to Transform, Stabilize
and Corner Pin. Renderer::layer_source for the tracker's frames.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-10-02 08:28:44 -04:00

177 lines
6.9 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//! Small geometric solves for applying tracks: a 2-point similarity (Transform track rotation and
//! scale), a 3-point affine (Affine Corner Pin) and a 4-point homography (Perspective Corner Pin,
//! the direct linear transform with `h33 = 1`, Hartley & Zisserman §4.1).
/// Solve `a x = b` (n × n, row-major) by Gaussian elimination with partial pivoting.
pub fn solve_linear<const N: usize>(mut a: [[f64; N]; N], mut b: [f64; N]) -> Option<[f64; N]> {
for col in 0..N {
let piv = (col..N).max_by(|&i, &j| a[i][col].abs().total_cmp(&a[j][col].abs()))?;
if a[piv][col].abs() < 1e-12 {
return None;
}
a.swap(col, piv);
b.swap(col, piv);
for r in col + 1..N {
let f = a[r][col] / a[col][col];
if f == 0.0 {
continue;
}
for c in col..N {
a[r][c] -= f * a[col][c];
}
b[r] -= f * b[col];
}
}
let mut x = [0.0; N];
for r in (0..N).rev() {
let mut s = b[r];
for c in r + 1..N {
s -= a[r][c] * x[c];
}
x[r] = s / a[r][r];
}
x.iter().all(|v| v.is_finite()).then_some(x)
}
/// A 3 × 3 projective transform (row-major), mapping `[x, y, 1]`.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Homography(pub [[f64; 3]; 3]);
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]]);
pub fn apply(&self, p: [f64; 2]) -> [f64; 2] {
let m = &self.0;
let w = m[2][0] * p[0] + m[2][1] * p[1] + m[2][2];
let w = if w.abs() < 1e-12 { 1e-12 } else { w };
[(m[0][0] * p[0] + m[0][1] * p[1] + m[0][2]) / w, (m[1][0] * p[0] + m[1][1] * p[1] + m[1][2]) / w]
}
/// The homography taking `src[i]` to `dst[i]` (four points, no three collinear).
pub fn from_points(src: [[f64; 2]; 4], dst: [[f64; 2]; 4]) -> Option<Homography> {
// Normalise both sets (centroid at 0, mean distance √2) for conditioning.
let (ns, ts) = normalise(&src);
let (nd, td) = normalise(&dst);
let mut a = [[0.0; 8]; 8];
let mut b = [0.0; 8];
for i in 0..4 {
let ([x, y], [u, v]) = (ns[i], nd[i]);
a[2 * i] = [x, y, 1.0, 0.0, 0.0, 0.0, -u * x, -u * y];
b[2 * i] = u;
a[2 * i + 1] = [0.0, 0.0, 0.0, x, y, 1.0, -v * x, -v * y];
b[2 * i + 1] = v;
}
let h = solve_linear(a, b)?;
let hn = [[h[0], h[1], h[2]], [h[3], h[4], h[5]], [h[6], h[7], 1.0]];
// H = Td⁻¹ · Hn · Ts
let m = mul(inv_similarity(td), mul(hn, ts));
let s = m[2][2];
if s.abs() < 1e-12 {
return None;
}
Some(Homography(m.map(|r| r.map(|v| v / s))))
}
/// The affine map taking three points to three points.
pub fn affine_from_points(src: [[f64; 2]; 3], dst: [[f64; 2]; 3]) -> Option<Homography> {
let mut a = [[0.0; 3]; 3];
for i in 0..3 {
a[i] = [src[i][0], src[i][1], 1.0];
}
let x = solve_linear(a, [dst[0][0], dst[1][0], dst[2][0]])?;
let y = solve_linear(a, [dst[0][1], dst[1][1], dst[2][1]])?;
Some(Homography([x, y, [0.0, 0.0, 1.0]]))
}
/// The similarity (rotation, uniform scale, translation) taking two points to two points.
pub fn similarity_from_points(src: [[f64; 2]; 2], dst: [[f64; 2]; 2]) -> Option<Homography> {
let (ds, dd) = (sub(src[1], src[0]), sub(dst[1], dst[0]));
let n = ds[0] * ds[0] + ds[1] * ds[1];
if n < 1e-12 {
return None;
}
// dd = [[a, -b], [b, a]] ds
let a = (dd[0] * ds[0] + dd[1] * ds[1]) / n;
let b = (dd[1] * ds[0] - dd[0] * ds[1]) / n;
let tx = dst[0][0] - (a * src[0][0] - b * src[0][1]);
let ty = dst[0][1] - (b * src[0][0] + a * src[0][1]);
Some(Homography([[a, -b, tx], [b, a, ty], [0.0, 0.0, 1.0]]))
}
}
/// Angle (degrees, y down = clockwise positive like After Effects' Rotation) and length of `b - a`.
pub fn angle_and_length(a: [f64; 2], b: [f64; 2]) -> (f64, f64) {
let d = sub(b, a);
(d[1].atan2(d[0]).to_degrees(), (d[0] * d[0] + d[1] * d[1]).sqrt())
}
/// `angle` moved by multiples of 360° to be closest to `prev` (continuous rotation curves).
pub fn unwrap_degrees(prev: f64, angle: f64) -> f64 {
angle + ((prev - angle) / 360.0).round() * 360.0
}
fn sub(a: [f64; 2], b: [f64; 2]) -> [f64; 2] {
[a[0] - b[0], a[1] - b[1]]
}
fn mul(a: [[f64; 3]; 3], b: [[f64; 3]; 3]) -> [[f64; 3]; 3] {
let mut o = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
o[i][j] = (0..3).map(|k| a[i][k] * b[k][j]).sum();
}
}
o
}
fn normalise(p: &[[f64; 2]; 4]) -> ([[f64; 2]; 4], [[f64; 3]; 3]) {
let c = [p.iter().map(|q| q[0]).sum::<f64>() / 4.0, p.iter().map(|q| q[1]).sum::<f64>() / 4.0];
let d = p.iter().map(|q| ((q[0] - c[0]).powi(2) + (q[1] - c[1]).powi(2)).sqrt()).sum::<f64>() / 4.0;
let s = if d > 1e-12 { std::f64::consts::SQRT_2 / d } else { 1.0 };
let t = [[s, 0.0, -s * c[0]], [0.0, s, -s * c[1]], [0.0, 0.0, 1.0]];
(p.map(|q| [s * (q[0] - c[0]), s * (q[1] - c[1])]), t)
}
fn inv_similarity(t: [[f64; 3]; 3]) -> [[f64; 3]; 3] {
let s = t[0][0];
[[1.0 / s, 0.0, -t[0][2] / s], [0.0, 1.0 / s, -t[1][2] / s], [0.0, 0.0, 1.0]]
}
#[cfg(test)]
mod tests {
use super::*;
fn close(a: [f64; 2], b: [f64; 2], tol: f64) -> bool {
(a[0] - b[0]).abs() < tol && (a[1] - b[1]).abs() < tol
}
#[test]
fn homography_recovers_known_projection() {
let h = Homography([[0.9, 0.12, 30.0], [-0.08, 1.1, -12.0], [0.0004, -0.0002, 1.0]]);
let src = [[100.0, 100.0], [700.0, 120.0], [680.0, 500.0], [90.0, 520.0]];
let dst = src.map(|p| h.apply(p));
let got = Homography::from_points(src, dst).unwrap();
for i in 0..3 {
for j in 0..3 {
assert!((got.0[i][j] - h.0[i][j]).abs() < 1e-6 * (1.0 + h.0[i][j].abs()), "{:?}", got.0);
}
}
for p in [[0.0, 0.0], [400.0, 300.0], [1000.0, 50.0]] {
assert!(close(got.apply(p), h.apply(p), 1e-6));
}
// Degenerate (collinear) input has no solution.
assert!(Homography::from_points([[0.0, 0.0], [1.0, 1.0], [2.0, 2.0], [3.0, 3.0]], dst).is_none());
}
#[test]
fn affine_and_similarity() {
let a = Homography::affine_from_points([[0.0, 0.0], [10.0, 0.0], [0.0, 10.0]], [[5.0, 5.0], [15.0, 7.0], [3.0, 15.0]]).unwrap();
assert!(close(a.apply([10.0, 10.0]), [13.0, 17.0], 1e-9));
let s = Homography::similarity_from_points([[0.0, 0.0], [10.0, 0.0]], [[1.0, 1.0], [1.0, 21.0]]).unwrap();
assert!(close(s.apply([0.0, 10.0]), [-19.0, 1.0], 1e-9));
let (ang, len) = angle_and_length([1.0, 1.0], [1.0, 21.0]);
assert!((ang - 90.0).abs() < 1e-9 && (len - 20.0).abs() < 1e-9);
assert!((unwrap_degrees(350.0, -5.0) - 355.0).abs() < 1e-9);
}
}