mirror of
https://github.com/storytold/effectcraft.git
synced 2026-11-16 15:22:43 +00:00
823 lines
35 KiB
Rust
823 lines
35 KiB
Rust
//! 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. `AᵀA` is symmetric and banded (every
|
||
/// row couples unknowns at most `bw` apart), so only its lower band is stored: entry `(i, j)`
|
||
/// with `i − bw ≤ j ≤ i` sits at `i * (bw + 1) + j + bw − i`.
|
||
struct Normal {
|
||
bw: usize,
|
||
a: Vec<f64>,
|
||
b: Vec<f64>,
|
||
}
|
||
|
||
impl Normal {
|
||
fn row(&mut self, coefs: &[(usize, f64)], rhs: f64, w: f64) {
|
||
let width = self.bw + 1;
|
||
for (ia, ca) in coefs {
|
||
self.b[*ia] += w * ca * rhs;
|
||
for (ib, cb) in coefs {
|
||
if ib <= ia {
|
||
self.a[ia * width + ib + self.bw - ia] += w * ca * cb;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Cholesky solve of `A x = b` for the banded `Normal::a`, in place. Returns false when `A` is not
|
||
/// positive definite.
|
||
fn cholesky_band(a: &mut [f64], n: usize, bw: usize, b: &mut [f64]) -> bool {
|
||
let w = bw + 1;
|
||
for j in 0..n {
|
||
let kj = j.saturating_sub(bw);
|
||
let rowj = j * w + kj + bw - j;
|
||
let mut d = a[j * w + bw];
|
||
for v in &a[rowj..j * w + bw] {
|
||
d -= v * v;
|
||
}
|
||
if d <= 0.0 || !d.is_finite() {
|
||
return false;
|
||
}
|
||
let d = d.sqrt();
|
||
a[j * w + bw] = d;
|
||
for i in j + 1..(j + bw + 1).min(n) {
|
||
let ki = i - bw.min(i);
|
||
let mut s = a[i * w + j + bw - i];
|
||
let len = j - ki;
|
||
let (head, tail) = a.split_at_mut(i * w);
|
||
let li = &tail[ki + bw - i..ki + bw - i + len];
|
||
let lj = &head[j * w + ki + bw - j..j * w + ki + bw - j + len];
|
||
for (x, y) in li.iter().zip(lj) {
|
||
s -= x * y;
|
||
}
|
||
a[i * w + j + bw - i] = s / d;
|
||
}
|
||
}
|
||
for i in 0..n {
|
||
let mut s = b[i];
|
||
for k in i.saturating_sub(bw)..i {
|
||
s -= a[i * w + k + bw - i] * b[k];
|
||
}
|
||
b[i] = s / a[i * w + bw];
|
||
}
|
||
for i in (0..n).rev() {
|
||
let mut s = b[i];
|
||
for k in i + 1..(i + bw + 1).min(n) {
|
||
s -= a[k * w + i + bw - k] * b[k];
|
||
}
|
||
b[i] = s / a[i * w + bw];
|
||
}
|
||
b.iter().all(|v| v.is_finite())
|
||
}
|
||
|
||
/// 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;
|
||
// A cell couples its four corners and, through the triangles, vertices up to `cols + 2` apart.
|
||
let bw = (2 * (m.cols + 2) + 1).min(n - 1);
|
||
let mut ne = Normal { bw, a: vec![0.0; n * (bw + 1)], 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: [(usize, f64); 4] = std::array::from_fn(|c| (2 * ks[c] + d, w[c]));
|
||
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_band(&mut ne.a, n, bw, &mut x) {
|
||
for k in 0..nv {
|
||
m.verts[k] = [x[2 * k], x[2 * k + 1]];
|
||
}
|
||
}
|
||
m
|
||
}
|
||
|
||
/// Reduce the symmetric `a` (row-major `n × n`, consumed) to tridiagonal form `T = Qᵀ A Q` by
|
||
/// Householder reflections. Returns the diagonal of `T`, its off-diagonal (`off[i] = T[i][i+1]`)
|
||
/// and `Qᵀ` (row-major: row `j` is column `j` of `Q`).
|
||
///
|
||
/// Step `k` picks the reflector `H = I − β v vᵀ` that maps column `k` below the diagonal onto a
|
||
/// multiple of the first unit vector, and applies it to the trailing block `B` from both sides
|
||
/// as the symmetric rank-2 update `B −= v wᵀ + w vᵀ` with `p = β B v`, `w = p − (β pᵀv / 2) v`.
|
||
fn householder_tridiagonal(mut a: Vec<f64>, n: usize) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
|
||
let mut qt = vec![0.0; n * n];
|
||
for i in 0..n {
|
||
qt[i * n + i] = 1.0;
|
||
}
|
||
let mut v = vec![0.0; n];
|
||
let mut p = vec![0.0; n];
|
||
let mut u = vec![0.0; n];
|
||
for k in 0..n.saturating_sub(2) {
|
||
let s = k + 1;
|
||
let len = n - s;
|
||
for i in 0..len {
|
||
v[i] = a[(s + i) * n + k];
|
||
}
|
||
let norm = v[..len].iter().map(|x| x * x).sum::<f64>().sqrt();
|
||
if norm == 0.0 {
|
||
continue;
|
||
}
|
||
let alpha = if v[0] >= 0.0 { -norm } else { norm };
|
||
v[0] -= alpha;
|
||
let beta = 2.0 / v[..len].iter().map(|x| x * x).sum::<f64>();
|
||
for i in 0..len {
|
||
let row = &a[(s + i) * n + s..(s + i) * n + n];
|
||
p[i] = beta * row.iter().zip(&v[..len]).map(|(x, y)| x * y).sum::<f64>();
|
||
}
|
||
let half = 0.5 * beta * p[..len].iter().zip(&v[..len]).map(|(x, y)| x * y).sum::<f64>();
|
||
for i in 0..len {
|
||
p[i] -= half * v[i];
|
||
}
|
||
// `p` now holds `w`.
|
||
for i in 0..len {
|
||
let row = &mut a[(s + i) * n + s..(s + i) * n + n];
|
||
for (j, x) in row.iter_mut().enumerate() {
|
||
*x -= v[i] * p[j] + p[i] * v[j];
|
||
}
|
||
}
|
||
a[s * n + k] = alpha;
|
||
a[k * n + s] = alpha;
|
||
// Q ← Q H: with rows of Qᵀ, row j −= β v_j (Σ_l v_l row l).
|
||
u.fill(0.0);
|
||
for (l, vl) in v[..len].iter().enumerate() {
|
||
for (x, q) in u.iter_mut().zip(&qt[(s + l) * n..(s + l + 1) * n]) {
|
||
*x += vl * q;
|
||
}
|
||
}
|
||
for (j, vj) in v[..len].iter().enumerate() {
|
||
for (q, x) in qt[(s + j) * n..(s + j + 1) * n].iter_mut().zip(&u) {
|
||
*q -= beta * vj * x;
|
||
}
|
||
}
|
||
}
|
||
let diag = (0..n).map(|i| a[i * n + i]).collect();
|
||
let off = (0..n.saturating_sub(1)).map(|i| a[(i + 1) * n + i]).collect();
|
||
(diag, off, qt)
|
||
}
|
||
|
||
/// One implicit symmetric QR step with the Wilkinson shift on the unreduced block `lo..=hi` of
|
||
/// the tridiagonal `(diag, off)`: a Givens rotation of rows/columns `k, k+1` for `k = lo..hi`
|
||
/// first introduces, then chases down a bulge below the subdiagonal. The rotations are applied to
|
||
/// the rows of `qt` as well.
|
||
fn wilkinson_qr_step(diag: &mut [f64], off: &mut [f64], qt: &mut [f64], n: usize, lo: usize, hi: usize) {
|
||
// The eigenvalue of the trailing 2×2 block nearer its last diagonal entry.
|
||
let t = off[hi - 1];
|
||
let half = 0.5 * (diag[hi - 1] - diag[hi]);
|
||
let sign = if half >= 0.0 { 1.0 } else { -1.0 };
|
||
let shift = diag[hi] - t * t / (half + sign * half.hypot(t));
|
||
let (mut x, mut z) = (diag[lo] - shift, off[lo]);
|
||
for k in lo..hi {
|
||
let r = x.hypot(z);
|
||
let (c, s) = if r == 0.0 { (1.0, 0.0) } else { (x / r, z / r) };
|
||
if k > lo {
|
||
off[k - 1] = r;
|
||
}
|
||
let (a, b, d) = (diag[k], off[k], diag[k + 1]);
|
||
diag[k] = c * c * a + 2.0 * c * s * b + s * s * d;
|
||
diag[k + 1] = s * s * a - 2.0 * c * s * b + c * c * d;
|
||
off[k] = c * s * (d - a) + (c * c - s * s) * b;
|
||
if k + 1 < hi {
|
||
// The rotation spreads `off[k+1]` into the bulge at (k+2, k).
|
||
x = off[k];
|
||
z = s * off[k + 1];
|
||
off[k + 1] *= c;
|
||
}
|
||
let (top, bottom) = qt.split_at_mut((k + 1) * n);
|
||
for (p, q) in top[k * n..].iter_mut().zip(&mut bottom[..n]) {
|
||
let (pk, qk) = (*p, *q);
|
||
*p = c * pk + s * qk;
|
||
*q = c * qk - s * pk;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Symmetric eigen-decomposition: Householder reduction to tridiagonal form, then implicit QR
|
||
/// steps with the Wilkinson shift and deflation of negligible off-diagonal entries (the method of
|
||
/// Golub & Van Loan, *Matrix Computations*, ch. 8: Householder tridiagonalisation and the
|
||
/// symmetric QR algorithm). Same contract as `sym_eigen`: eigenvalues ascending, eigenvectors as
|
||
/// the columns of a row-major matrix. `None` for non-finite input, a size mismatch or no
|
||
/// convergence within `30 n` steps; the caller then falls back to Jacobi.
|
||
fn sym_eigen_qr(a: &[f64], n: usize) -> Option<(Vec<f64>, Vec<f64>)> {
|
||
if a.len() != n * n || a.iter().any(|x| !x.is_finite()) {
|
||
return None;
|
||
}
|
||
let (mut diag, mut off, mut qt) = householder_tridiagonal(a.to_vec(), n);
|
||
let mut budget = 30 * n.max(1);
|
||
let mut hi = n.saturating_sub(1);
|
||
while hi > 0 {
|
||
for i in 0..hi {
|
||
if off[i].abs() <= f64::EPSILON * (diag[i].abs() + diag[i + 1].abs()) {
|
||
off[i] = 0.0;
|
||
}
|
||
}
|
||
if off[hi - 1] == 0.0 {
|
||
hi -= 1;
|
||
continue;
|
||
}
|
||
let mut lo = hi - 1;
|
||
while lo > 0 && off[lo - 1] != 0.0 {
|
||
lo -= 1;
|
||
}
|
||
budget = budget.checked_sub(1)?;
|
||
wilkinson_qr_step(&mut diag, &mut off, &mut qt, n, lo, hi);
|
||
}
|
||
if diag.iter().chain(&qt).any(|x| !x.is_finite()) {
|
||
return None;
|
||
}
|
||
let mut order: Vec<usize> = (0..n).collect();
|
||
order.sort_by(|&x, &y| diag[x].total_cmp(&diag[y]));
|
||
let vals = order.iter().map(|&j| diag[j]).collect();
|
||
let mut vecs = vec![0.0; n * n];
|
||
for (k, &j) in order.iter().enumerate() {
|
||
for r in 0..n {
|
||
vecs[r * n + k] = qt[j * n + r];
|
||
}
|
||
}
|
||
Some((vals, vecs))
|
||
}
|
||
|
||
/// Gaussian weights of the frames `lo..=hi` around `t`.
|
||
fn gauss(lo: usize, hi: usize, t: usize, sigma: f64) -> Vec<f64> {
|
||
let g: Vec<f64> = (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<Vec<([f64; 2], [f64; 2])>> {
|
||
let max_r = ((3.0 * sigma).ceil() as usize).clamp(1, 30);
|
||
// The trajectories seen on each frame (in track order): a frame only ever uses those, so none
|
||
// scans the whole collection (every frame did, once per window shrink).
|
||
let mut seen: Vec<Vec<u32>> = vec![Vec::new(); frames];
|
||
for (i, tr) in tracks.iter().enumerate() {
|
||
let (a, b) = (tr.start as usize, (tr.end() as usize).min(frames));
|
||
for list in seen.iter_mut().take(b).skip(a) {
|
||
list.push(i as u32);
|
||
}
|
||
}
|
||
(0..frames)
|
||
.into_par_iter()
|
||
.map(|t| {
|
||
let t32 = t as u32;
|
||
let on_frame = &seen[t];
|
||
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> =
|
||
on_frame.iter().map(|&i| &tracks[i as usize]).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_qr(&gram, m).unwrap_or_else(|| sym_eigen(&gram, m));
|
||
// C[q][c] = vecs[c][m − 1 − q] (largest first).
|
||
let cmat: Vec<Vec<f64>> = (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<f64> = 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 on_frame.iter().map(|&i| &tracks[i as usize]).filter(|tr| !(tr.start as usize <= lo && tr.end() as usize > hi)) {
|
||
let cols: Vec<usize> = (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 Some(p) = tr.at((lo + c) as u32) else { continue };
|
||
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 && let Some(p) = tr.at(t32) {
|
||
out.push((p, dst));
|
||
}
|
||
}
|
||
out
|
||
})
|
||
.collect()
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
/// Smoothing visits only the trajectories seen on each frame: tracks that start after the clip
|
||
/// (or on other frames) change nothing and cost nothing, however many there are.
|
||
#[test]
|
||
fn smoothing_ignores_trajectories_off_the_frame() {
|
||
let frames = 60usize;
|
||
let wob = |k: usize, f: usize| [10.0 + k as f32 * 3.0 + (f as f32 * 0.2).sin() * 4.0, 20.0 + (k % 7) as f32 * 5.0 + f as f32 * 0.5];
|
||
let mut tracks: Vec<Track2D> = (0..30).map(|k| Track2D { id: k as u32, start: 0, pts: (0..frames).map(|f| wob(k, f)).collect() }).collect();
|
||
// Partial tracks: some seen on only part of the clip.
|
||
tracks.extend((0..10).map(|k| Track2D { id: 100 + k as u32, start: 5 * k as u32, pts: (5 * k..frames.min(5 * k + 30)).map(|f| wob(k, f)).collect() }));
|
||
let base = smooth_trajectories(&tracks, frames, 3.0, 9);
|
||
assert!(base.iter().any(|v| v.len() >= 30), "the fixture gives frames with pairs");
|
||
let mut more = tracks.clone();
|
||
more.extend((0..200_000u32).map(|i| Track2D { id: 1000 + i, start: frames as u32 + i % 50, pts: vec![[1.0, 2.0]] }));
|
||
assert_eq!(smooth_trajectories(&more, frames, 3.0, 9), base);
|
||
}
|
||
|
||
#[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}");
|
||
}
|
||
|
||
fn lcg(seed: &mut u64) -> f64 {
|
||
*seed = seed.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407);
|
||
(*seed >> 11) as f64 / (1u64 << 53) as f64 - 0.5
|
||
}
|
||
|
||
/// Checks an eigen-decomposition of `a` directly: `A v = λ v` per column, `VᵀV = I`, values
|
||
/// ascending; `tol` relative to the largest |entry| of `a`.
|
||
fn check_eigen(a: &[f64], n: usize, vals: &[f64], vecs: &[f64], tol: f64, what: &str) {
|
||
let scale = a.iter().fold(0.0f64, |m, x| m.max(x.abs())).max(f64::MIN_POSITIVE);
|
||
assert!(vals.windows(2).all(|w| w[0] <= w[1]), "{what}: values not ascending");
|
||
for k in 0..n {
|
||
for i in 0..n {
|
||
let av: f64 = (0..n).map(|j| a[i * n + j] * vecs[j * n + k]).sum();
|
||
let r = (av - vals[k] * vecs[i * n + k]).abs();
|
||
assert!(r <= tol * scale, "{what}: residual {r} at ({i}, {k})");
|
||
}
|
||
for l in 0..n {
|
||
let d: f64 = (0..n).map(|i| vecs[i * n + k] * vecs[i * n + l]).sum();
|
||
let want = if k == l { 1.0 } else { 0.0 };
|
||
assert!((d - want).abs() < 1e-10, "{what}: VᵀV ({k}, {l}) = {d}");
|
||
}
|
||
}
|
||
}
|
||
|
||
fn random_symmetric(n: usize, seed: &mut u64, scale: impl Fn(usize) -> f64) -> Vec<f64> {
|
||
let mut a = vec![0.0; n * n];
|
||
for i in 0..n {
|
||
for j in 0..=i {
|
||
let v = lcg(seed) * scale(i) * scale(j);
|
||
a[i * n + j] = v;
|
||
a[j * n + i] = v;
|
||
}
|
||
}
|
||
a
|
||
}
|
||
|
||
#[test]
|
||
fn qr_eigen_matches_jacobi_on_random_matrices() {
|
||
use crate::camtrack::linalg::sym_eigen;
|
||
let mut seed = 3;
|
||
for n in [1, 2, 3, 7, 20, 61] {
|
||
for _ in 0..3 {
|
||
let a = random_symmetric(n, &mut seed, |_| 1.0);
|
||
let (vals, vecs) = sym_eigen_qr(&a, n).unwrap();
|
||
check_eigen(&a, n, &vals, &vecs, 1e-12, &format!("random n {n}"));
|
||
let (v0, _) = sym_eigen(&a, n);
|
||
for (x, y) in vals.iter().zip(&v0) {
|
||
assert!((x - y).abs() < 1e-11, "n {n}: {x} vs {y}");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn qr_eigen_handles_special_matrices() {
|
||
let mut seed = 5;
|
||
let n = 6;
|
||
let mut identity = vec![0.0; n * n];
|
||
let mut blocks = vec![0.0; n * n];
|
||
for i in 0..n {
|
||
identity[i * n + i] = 1.0;
|
||
// Two equal 3×3 blocks: every eigenvalue twice.
|
||
for j in 0..n {
|
||
if i / 3 == j / 3 {
|
||
blocks[i * n + j] = [[2.0, 1.0, 0.0], [1.0, 3.0, 1.0], [0.0, 1.0, 4.0]][i % 3][j % 3];
|
||
}
|
||
}
|
||
}
|
||
let cases: Vec<(&str, usize, Vec<f64>)> = vec![
|
||
("identity", n, identity),
|
||
("repeated blocks", n, blocks),
|
||
("zero", n, vec![0.0; n * n]),
|
||
("1x1", 1, vec![-4.5]),
|
||
("0x0", 0, vec![]),
|
||
// Rows and columns scaled from 1e-8 to 1e8.
|
||
("badly scaled", 9, random_symmetric(9, &mut seed, |i| 10f64.powi(2 * i as i32 - 8))),
|
||
("tridiagonal 1-2-1", 30, (0..900usize).map(|k| [0.0, 1.0, 2.0][2 - (k / 30).abs_diff(k % 30).min(2)]).collect()),
|
||
];
|
||
for (what, n, a) in cases {
|
||
let (vals, vecs) = sym_eigen_qr(&a, n).unwrap();
|
||
check_eigen(&a, n, &vals, &vecs, 1e-12, what);
|
||
}
|
||
let (vals, _) = sym_eigen_qr(&[1.0, 0.0, 0.0, 1.0], 2).unwrap();
|
||
assert_eq!(vals, vec![1.0, 1.0]);
|
||
// Non-finite input or a size mismatch: None (the caller falls back to Jacobi) at once.
|
||
let mut nan = random_symmetric(8, &mut seed, |_| 1.0);
|
||
nan[9] = f64::NAN;
|
||
assert!(sym_eigen_qr(&nan, 8).is_none());
|
||
assert!(sym_eigen_qr(&[f64::INFINITY], 1).is_none());
|
||
assert!(sym_eigen_qr(&[1.0, 2.0, 3.0], 2).is_none());
|
||
}
|
||
|
||
#[test]
|
||
fn qr_eigen_keeps_the_leading_subspace_of_gram_like_matrices() {
|
||
use crate::camtrack::linalg::sym_eigen;
|
||
let mut seed = 7;
|
||
for n in [1, 2, 5, 31, 61] {
|
||
// Large magnitudes like a trajectory Gram matrix, with a low-rank part.
|
||
let rank = 3.min(n);
|
||
let f: Vec<f64> = (0..n * rank).map(|_| 300.0 * lcg(&mut seed)).collect();
|
||
let mut a = vec![0.0; n * n];
|
||
for i in 0..n {
|
||
for j in 0..n {
|
||
a[i * n + j] = (0..rank).map(|k| f[i * rank + k] * f[j * rank + k]).sum::<f64>() * 40.0;
|
||
}
|
||
}
|
||
let (v0, e0) = sym_eigen(&a, n);
|
||
let (v1, e1) = sym_eigen_qr(&a, n).unwrap();
|
||
check_eigen(&a, n, &v1, &e1, 1e-12, &format!("gram n {n}"));
|
||
let top = v0[n - 1].abs().max(1.0);
|
||
for k in 0..n {
|
||
assert!((v0[k] - v1[k]).abs() < 1e-9 * top, "n {n} value {k}: {} vs {}", v0[k], v1[k]);
|
||
}
|
||
// The leading subspace (distinct eigenvalues, compared as a projector: signs and the
|
||
// basis are free).
|
||
for i in 0..n {
|
||
for j in 0..n {
|
||
let p0: f64 = (0..rank).map(|q| e0[i * n + n - 1 - q] * e0[j * n + n - 1 - q]).sum();
|
||
let p1: f64 = (0..rank).map(|q| e1[i * n + n - 1 - q] * e1[j * n + n - 1 - q]).sum();
|
||
assert!((p0 - p1).abs() < 1e-9, "n {n} projector ({i},{j}): {p0} vs {p1}");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn band_cholesky_matches_dense() {
|
||
let mut seed = 11;
|
||
for (n, bw) in [(1, 0), (6, 2), (40, 7), (40, 39)] {
|
||
let mut dense = vec![0.0; n * n];
|
||
let mut band = vec![0.0; n * (bw + 1)];
|
||
for i in 0..n {
|
||
for j in i.saturating_sub(bw)..=i {
|
||
let v = if i == j { 4.0 * bw as f64 + 2.0 + lcg(&mut seed) } else { lcg(&mut seed) };
|
||
dense[i * n + j] = v;
|
||
dense[j * n + i] = v;
|
||
band[i * (bw + 1) + j + bw - i] = v;
|
||
}
|
||
}
|
||
let b: Vec<f64> = (0..n).map(|_| 10.0 * lcg(&mut seed)).collect();
|
||
let (mut x0, mut x1) = (b.clone(), b);
|
||
assert!(cholesky_solve(&mut dense, n, &mut x0));
|
||
assert!(cholesky_band(&mut band, n, bw, &mut x1));
|
||
for (a, b) in x0.iter().zip(&x1) {
|
||
assert!((a - b).abs() < 1e-10, "{a} vs {b}");
|
||
}
|
||
}
|
||
let mut band = vec![-1.0, 0.0, 0.0, 1.0];
|
||
assert!(!cholesky_band(&mut band, 2, 1, &mut [1.0, 1.0]));
|
||
}
|
||
}
|