From bab4f88c22e525ac69474a8293b155fe70fccab4 Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Fri, 2 Oct 2026 21:44:51 -0400 Subject: [PATCH] M12.6: camera tracker solve Structure from motion for the 3D Camera Tracker in effectcraft-track (camtrack): long KLT feature tracks with re-detection, parallax keyframes, two-view initialisation (normalised 8-point essential matrix in RANSAC or planar homography decomposition), incremental resection + triangulation, sparse Levenberg-Marquardt bundle adjustment with the Schur complement and Huber loss, focal length search (Fixed / Variable Zoom / Specify Angle of View), tripod pan solve, Auto Detect, ground plane / target / world mapping. Synthetic-scene tests (path, focal, error, tripod, flat, rendered frames). Co-Authored-By: Claude Opus 5.5 --- crates/track/Cargo.toml | 2 +- crates/track/src/camtrack/bundle.rs | 416 ++++++++++++ crates/track/src/camtrack/features.rs | 115 ++++ crates/track/src/camtrack/geometry.rs | 432 +++++++++++++ crates/track/src/camtrack/linalg.rs | 476 ++++++++++++++ crates/track/src/camtrack/mod.rs | 376 +++++++++++ crates/track/src/camtrack/solver.rs | 894 ++++++++++++++++++++++++++ crates/track/src/camtrack/tests.rs | 360 +++++++++++ crates/track/src/lib.rs | 1 + 9 files changed, 3071 insertions(+), 1 deletion(-) create mode 100644 crates/track/src/camtrack/bundle.rs create mode 100644 crates/track/src/camtrack/features.rs create mode 100644 crates/track/src/camtrack/geometry.rs create mode 100644 crates/track/src/camtrack/linalg.rs create mode 100644 crates/track/src/camtrack/mod.rs create mode 100644 crates/track/src/camtrack/solver.rs create mode 100644 crates/track/src/camtrack/tests.rs diff --git a/crates/track/Cargo.toml b/crates/track/Cargo.toml index ba449c92..9bd46e26 100644 --- a/crates/track/Cargo.toml +++ b/crates/track/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "effectcraft-track" -description = "Motion tracking: feature/search region point tracking (pyramid normalized cross-correlation + Lucas-Kanade refinement), confidence, and corner-pin / transform solves" +description = "Motion tracking: feature/search region point tracking (pyramid normalized cross-correlation + Lucas-Kanade refinement), confidence, corner-pin / transform solves, stabilization and 3D camera solving (structure from motion)" version.workspace = true edition.workspace = true license.workspace = true diff --git a/crates/track/src/camtrack/bundle.rs b/crates/track/src/camtrack/bundle.rs new file mode 100644 index 00000000..84e8a5ad --- /dev/null +++ b/crates/track/src/camtrack/bundle.rs @@ -0,0 +1,416 @@ +//! Sparse bundle adjustment: Levenberg–Marquardt over cameras and points with the **Schur +//! complement** eliminating the points (Triggs, McLauchlan, Hartley & Fitzgibbon, "Bundle +//! Adjustment — A Modern Synthesis", 2000, §6; H&Z Appendix 6). Residuals are reprojection +//! errors in pixels with a **Huber** robust loss (applied by iteratively reweighted least +//! squares, Triggs et al. §3.3). +//! +//! Cameras are `(R, C, f)`; rotations are updated multiplicatively (`R ← exp([δ]×) R`), focal +//! lengths in log space. The focal length is held fixed, shared by all cameras, or per camera +//! (with a smoothness prior between consecutive cameras). Tripod solves hold the centres fixed. + +use rayon::prelude::*; + +use super::geometry::Pose; +use super::linalg::*; + +/// A camera in the adjustment. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BaCam { + pub pose: Pose, + /// Focal length (pixels). + pub f: f64, +} + +/// One observation: camera `cam` sees point `pt` at `uv` (pixels relative to the principal +/// point). +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BaObs { + pub cam: u32, + pub pt: u32, + pub uv: [f64; 2], +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum FocalMode { + Fixed, + Shared, + /// Per camera, with prior weight `k` on the log-focal difference of consecutive cameras. + PerCamera(f64), +} + +#[derive(Clone, Debug)] +pub struct BaOpts { + pub focal: FocalMode, + /// Motion only: the points stay fixed. + pub fix_points: bool, + /// The camera centres stay fixed (tripod). + pub fix_centers: bool, + /// Cameras that stay fixed (gauge), by index; missing entries are free. + pub fixed: Vec, + /// Huber threshold (pixels). + pub huber: f64, + pub max_iter: usize, +} + +impl Default for BaOpts { + fn default() -> Self { + BaOpts { focal: FocalMode::Fixed, fix_points: false, fix_centers: false, fixed: vec![], huber: 2.0, max_iter: 30 } + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct BaReport { + pub initial_cost: f64, + pub cost: f64, + pub iterations: usize, +} + +const BEHIND: f64 = 1e6; + +fn huber(e2: f64, k: f64) -> f64 { + if e2 <= k * k { e2 } else { 2.0 * k * e2.sqrt() - k * k } +} + +/// Residual (pixels) of one observation, `None` behind the camera. +pub fn residual(c: &BaCam, x: V3, uv: [f64; 2]) -> Option<[f64; 2]> { + let p = c.pose.to_cam(x); + if p[2] <= 1e-9 { + return None; + } + Some([c.f * p[0] / p[2] - uv[0], c.f * p[1] / p[2] - uv[1]]) +} + +/// Total robust cost. +pub fn cost(cams: &[BaCam], pts: &[V3], obs: &[BaObs], o: &BaOpts) -> f64 { + let c: f64 = obs + .par_iter() + .map(|ob| match residual(&cams[ob.cam as usize], pts[ob.pt as usize], ob.uv) { + Some(r) => huber(r[0] * r[0] + r[1] * r[1], o.huber), + None => BEHIND, + }) + .sum(); + c + prior_cost(cams, o) +} + +fn prior_cost(cams: &[BaCam], o: &BaOpts) -> f64 { + match o.focal { + FocalMode::PerCamera(k) => cams.windows(2).map(|w| (k * (w[0].f.ln() - w[1].f.ln())).powi(2)).sum(), + _ => 0.0, + } +} + +/// Parameter layout. +struct Layout { + /// First pose parameter of each camera (`usize::MAX` when its pose is fixed). + start: Vec, + /// Per-camera focal parameter (`usize::MAX` when none). Gauge-fixed cameras keep a free + /// focal length. + fidx: Vec, + /// Index of the shared focal parameter. + shared: Option, + n: usize, + rot_only: bool, +} + +fn layout(ncam: usize, o: &BaOpts) -> Layout { + let per_cam_f = matches!(o.focal, FocalMode::PerCamera(_)); + let d = if o.fix_centers { 3 } else { 6 }; + let mut start = vec![usize::MAX; ncam]; + let mut fidx = vec![usize::MAX; ncam]; + let mut n = 0; + for i in 0..ncam { + if !o.fixed.get(i).copied().unwrap_or(false) { + start[i] = n; + n += d; + } + if per_cam_f { + fidx[i] = n; + n += 1; + } + } + let shared = matches!(o.focal, FocalMode::Shared).then(|| { + n += 1; + n - 1 + }); + Layout { start, fidx, shared, n, rot_only: o.fix_centers } +} + +/// Camera-side Jacobian entries of one observation: (parameter index, d r / d param). +fn cam_jac(l: &Layout, ci: usize, c: &BaCam, xc: V3, a: &[[f64; 3]; 2], p: [f64; 2], out: &mut Vec<(usize, [f64; 2])>) { + out.clear(); + let s = l.start[ci]; + if s != usize::MAX { + // dXc/dδ = −[Xc]× + let sk = skew(xc); + for k in 0..3 { + let g = [-sk[0][k], -sk[1][k], -sk[2][k]]; + out.push((s + k, [dot(a[0], g), dot(a[1], g)])); + } + if !l.rot_only { + // dXc/dC = −R + for k in 0..3 { + let g = [-c.pose.r[0][k], -c.pose.r[1][k], -c.pose.r[2][k]]; + out.push((s + 3 + k, [dot(a[0], g), dot(a[1], g)])); + } + } + } + if l.fidx[ci] != usize::MAX { + out.push((l.fidx[ci], p)); + } + if let Some(g) = l.shared { + out.push((g, p)); + } +} + +/// Run Levenberg–Marquardt. `cams` and `pts` are updated in place. +pub fn adjust(cams: &mut [BaCam], pts: &mut [V3], obs: &[BaObs], o: &BaOpts) -> BaReport { + let l = layout(cams.len(), o); + let npts = pts.len(); + // Observations by point. + let mut by_pt: Vec> = vec![vec![]; npts]; + for (i, ob) in obs.iter().enumerate() { + by_pt[ob.pt as usize].push(i as u32); + } + let mut cur = cost(cams, pts, obs, o); + let initial = cur; + let mut lambda = 1e-4; + let mut iters = 0; + if l.n == 0 && o.fix_points { + return BaReport { initial_cost: initial, cost: cur, iterations: 0 }; + } + let n = l.n; + let parallel = n <= 700; + for _ in 0..o.max_iter { + iters += 1; + // ---- Normal equations (Gauss–Newton with IRLS weights). + // Per point: V (3×3), bp (3), W entries (cam-side index → 3). + struct PtSys { + v: [[f64; 3]; 3], + bp: V3, + w: Vec<(usize, V3)>, + } + let build_point = |j: usize, s_acc: &mut Vec, bc: &mut Vec| -> PtSys { + let mut ps = PtSys { v: [[0.0; 3]; 3], bp: [0.0; 3], w: vec![] }; + let mut jc: Vec<(usize, [f64; 2])> = Vec::with_capacity(8); + for &oi in &by_pt[j] { + let ob = &obs[oi as usize]; + let ci = ob.cam as usize; + let c = &cams[ci]; + let xc = c.pose.to_cam(pts[j]); + if xc[2] <= 1e-9 { + continue; + } + let iz = 1.0 / xc[2]; + let p = [c.f * xc[0] * iz, c.f * xc[1] * iz]; + let r = [p[0] - ob.uv[0], p[1] - ob.uv[1]]; + let e = (r[0] * r[0] + r[1] * r[1]).sqrt(); + let w = if e <= o.huber { 1.0 } else { o.huber / e }; + let a = [[c.f * iz, 0.0, -c.f * xc[0] * iz * iz], [0.0, c.f * iz, -c.f * xc[1] * iz * iz]]; + cam_jac(&l, ci, c, xc, &a, p, &mut jc); + // Point Jacobian: A R. + let jx = if o.fix_points { + [[0.0; 3]; 2] + } else { + let r0 = mtv(&c.pose.r, a[0]); + let r1 = mtv(&c.pose.r, a[1]); + [r0, r1] + }; + for (ia, ja) in &jc { + bc[*ia] -= w * (ja[0] * r[0] + ja[1] * r[1]); + for (ib, jb) in &jc { + s_acc[ia * n + ib] += w * (ja[0] * jb[0] + ja[1] * jb[1]); + } + } + if !o.fix_points { + for a1 in 0..3 { + ps.bp[a1] -= w * (jx[0][a1] * r[0] + jx[1][a1] * r[1]); + for b1 in 0..3 { + ps.v[a1][b1] += w * (jx[0][a1] * jx[0][b1] + jx[1][a1] * jx[1][b1]); + } + } + for (ia, ja) in &jc { + let wv = + [w * (ja[0] * jx[0][0] + ja[1] * jx[1][0]), w * (ja[0] * jx[0][1] + ja[1] * jx[1][1]), w * (ja[0] * jx[0][2] + ja[1] * jx[1][2])]; + match ps.w.iter_mut().find(|e| e.0 == *ia) { + Some(e) => e.1 = add(e.1, wv), + None => ps.w.push((*ia, wv)), + } + } + } + } + ps + }; + let (mut s_mat, mut bc, systems): (Vec, Vec, Vec) = if parallel && npts > 64 { + let chunks = rayon::current_num_threads().clamp(1, 16); + let per = npts.div_ceil(chunks); + let parts: Vec<(Vec, Vec, Vec)> = (0..chunks) + .into_par_iter() + .map(|t| { + let (mut s, mut b) = (vec![0.0; n * n], vec![0.0; n]); + let v = (t * per..((t + 1) * per).min(npts)).map(|j| build_point(j, &mut s, &mut b)).collect(); + (s, b, v) + }) + .collect(); + let mut s = vec![0.0; n * n]; + let mut b = vec![0.0; n]; + let mut sys = Vec::with_capacity(npts); + for (ps, pb, pv) in parts { + for (x, y) in s.iter_mut().zip(ps) { + *x += y; + } + for (x, y) in b.iter_mut().zip(pb) { + *x += y; + } + sys.extend(pv); + } + (s, b, sys) + } else { + let mut s = vec![0.0; n * n]; + let mut b = vec![0.0; n]; + let sys = (0..npts).map(|j| build_point(j, &mut s, &mut b)).collect(); + (s, b, sys) + }; + // Focal prior. + if let FocalMode::PerCamera(k) = o.focal { + for i in 0..cams.len().saturating_sub(1) { + let r = k * (cams[i].f.ln() - cams[i + 1].f.ln()); + let (a, b) = (l.fidx[i], l.fidx[i + 1]); + let ia = (a != usize::MAX).then_some(a); + let ib = (b != usize::MAX).then_some(b); + for (idx, g) in [(ia, k), (ib, -k)] { + let Some(x) = idx else { continue }; + bc[x] -= g * r; + for (idy, h) in [(ia, k), (ib, -k)] { + if let Some(y) = idy { + s_mat[x * n + y] += g * h; + } + } + } + } + } + let base_s = s_mat.clone(); + let base_bc = bc.clone(); + let mean_diag = ((0..n).map(|i| base_s[i * n + i]).sum::() / n.max(1) as f64).max(1e-12); + // ---- Try steps with increasing damping. + let mut accepted = false; + for _try in 0..12 { + let mut s = base_s.clone(); + let mut rhs = base_bc.clone(); + for i in 0..n { + let d = s[i * n + i]; + s[i * n + i] = d + lambda * d.max(1e-6 * mean_diag); + } + // Schur complement over the points. + let mut vinv: Vec> = vec![None; npts]; + if !o.fix_points { + for (j, ps) in systems.iter().enumerate() { + let mut v = ps.v; + let vd = (v[0][0] + v[1][1] + v[2][2]) / 3.0; + for k in 0..3 { + v[k][k] += lambda * v[k][k].max(1e-6 * vd.max(1e-12)) + 1e-12; + } + let Some(vi) = inv3(&v) else { continue }; + vinv[j] = Some(vi); + let vb = mv(&vi, ps.bp); + let wv: Vec = ps.w.iter().map(|(_, w)| mv(&vi, *w)).collect(); + for (a, (ia, wa)) in ps.w.iter().enumerate() { + rhs[*ia] -= dot(*wa, vb); + for (ib, wb) in &ps.w { + s[ia * n + ib] -= dot(wv[a], *wb); + } + } + } + } + let mut dc = rhs; + if n > 0 && !cholesky_solve(&mut s, n, &mut dc) { + lambda *= 10.0; + continue; + } + // Apply. + let mut nc: Vec = cams.to_vec(); + for (i, c) in nc.iter_mut().enumerate() { + let st = l.start[i]; + if st != usize::MAX { + let dr = [dc[st], dc[st + 1], dc[st + 2]]; + c.pose.r = mm(&rodrigues(dr), &c.pose.r); + if !l.rot_only { + c.pose.c = add(c.pose.c, [dc[st + 3], dc[st + 4], dc[st + 5]]); + } + } + if l.fidx[i] != usize::MAX { + c.f *= dc[l.fidx[i]].clamp(-0.5, 0.5).exp(); + } + if let Some(g) = l.shared { + c.f *= dc[g].clamp(-0.5, 0.5).exp(); + } + } + let mut np: Vec = pts.to_vec(); + if !o.fix_points { + np.par_iter_mut().enumerate().for_each(|(j, x)| { + let Some(vi) = &vinv[j] else { return }; + let ps = &systems[j]; + let mut b = ps.bp; + for (ia, w) in &ps.w { + b = sub(b, scale(*w, dc[*ia])); + } + *x = add(*x, mv(vi, b)); + }); + } + let c2 = cost(&nc, &np, obs, o); + if c2.is_finite() && c2 < cur { + cams.copy_from_slice(&nc); + pts.copy_from_slice(&np); + let gain = cur - c2; + cur = c2; + lambda = (lambda / 3.0).max(1e-9); + accepted = gain > 1e-10 * cur.max(1e-12); + if !accepted { + // Converged (tiny improvement). + return BaReport { initial_cost: initial, cost: cur, iterations: iters }; + } + break; + } + lambda *= 4.0; + } + if !accepted { + break; + } + } + BaReport { initial_cost: initial, cost: cur, iterations: iters } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::camtrack::geometry::Rng; + + #[test] + fn bundle_adjustment_converges_from_noise() { + let mut rng = Rng(99); + let mut u = || (rng.next_u64() % 100_000) as f64 / 100_000.0 - 0.5; + let truth: Vec = (0..80).map(|_| [u() * 6.0, u() * 4.0, 6.0 + u() * 4.0]).collect(); + let f = 900.0; + let cams_true: Vec = (0..6) + .map(|i| BaCam { pose: Pose { r: rodrigues([0.0, -0.05 * i as f64, 0.01 * i as f64]), c: [0.3 * i as f64, 0.05 * i as f64, 0.0] }, f }) + .collect(); + let mut obs = vec![]; + for (ci, c) in cams_true.iter().enumerate() { + for (pi, x) in truth.iter().enumerate() { + let r = residual(c, *x, [0.0, 0.0]).unwrap(); + obs.push(BaObs { cam: ci as u32, pt: pi as u32, uv: r }); + } + } + let mut cams = cams_true.clone(); + for (i, c) in cams.iter_mut().enumerate().skip(2) { + c.pose.r = mm(&rodrigues([0.01, -0.01 * i as f64 * 0.3, 0.005]), &c.pose.r); + c.pose.c = add(c.pose.c, [0.02, -0.03, 0.01]); + } + let mut pts: Vec = truth.iter().map(|x| add(*x, [u() * 0.1, u() * 0.1, u() * 0.2])).collect(); + let mut fixed = vec![false; 6]; + fixed[0] = true; + fixed[1] = true; + let rep = adjust(&mut cams, &mut pts, &obs, &BaOpts { fixed, max_iter: 50, ..Default::default() }); + let rms = (rep.cost / obs.len() as f64).sqrt(); + assert!(rms < 1e-4, "{rep:?}"); + } +} diff --git a/crates/track/src/camtrack/features.rs b/crates/track/src/camtrack/features.rs new file mode 100644 index 00000000..311ec711 --- /dev/null +++ b/crates/track/src/camtrack/features.rs @@ -0,0 +1,115 @@ +//! Step 1 of the camera analysis: long feature tracks through the clip. +//! +//! Every frame, the active features are followed from the previous frame with pyramidal +//! Lucas–Kanade and a forward–backward check ([`crate::klt::track`]); a feature that fails ends +//! its track. When fewer than ~85 % of the wanted features remain, new Shi–Tomasi corners are +//! detected away from the active ones and start new tracks. Tracks shorter than three frames are +//! dropped at the end. + +use crate::Frame; +use crate::klt::{CornerOpts, GrayPyramid, LkOpts, analysis_factor, good_features, track}; + +use super::{CameraTracks, Track2D}; + +/// Analysis options. +#[derive(Clone, Copy, Debug, Default)] +pub struct AnalyzeOpts { + /// Detailed Analysis: twice the resolution and more features. + pub detailed: bool, +} + +/// Incremental feature tracking: push the clip's frames in order. +pub struct TrackAnalyzer { + opts: AnalyzeOpts, + prev: Option, + /// (track index, level-0 plane position) of the live features. + active: Vec<(usize, [f64; 2])>, + tracks: Vec, + frame: u32, + size: [f64; 2], + factor: f64, +} + +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 } + } + + pub fn frames(&self) -> u32 { + self.frame + } + + /// Features currently followed. + pub fn active(&self) -> usize { + self.active.len() + } + + fn params(&self) -> (u32, usize) { + if self.opts.detailed { (1280, 900) } else { (720, 500) } + } + + /// Analyse the next frame. + pub fn push(&mut self, frame: &Frame) { + let (max_side, want) = self.params(); + let factor = analysis_factor(frame.img.width, frame.img.height, max_side); + self.factor = factor as f64; + let pyr = GrayPyramid::from_image(frame.img, frame.offset, factor, 4); + let k = self.frame; + match &self.prev { + Some(prev) if prev.width() == pyr.width() && prev.height() == pyr.height() && prev.offset == pyr.offset => { + let pts: Vec<[f64; 2]> = self.active.iter().map(|a| a.1).collect(); + let q = track(prev, &pyr, &pts, None, &LkOpts { radius: 6, iterations: 30, epsilon: 0.005, fb_max: 0.5, ..Default::default() }); + let mut next = Vec::with_capacity(self.active.len()); + for ((ti, _), q) in self.active.iter().zip(q) { + if let Some(q) = q { + let l = pyr.to_layer(q); + self.tracks[*ti].pts.push([l[0] as f32, l[1] as f32]); + next.push((*ti, q)); + } + } + self.active = next; + } + _ => self.active.clear(), + } + // Re-detect where there are no features. + if (self.active.len() as f64) < want as f64 * 0.85 { + let md = (pyr.width().max(pyr.height()) as f64 / (want as f64).sqrt() * 0.6).clamp(5.0, 40.0); + let cell = md; + let (gw, gh) = ((pyr.width() as f64 / cell).ceil() as usize + 1, (pyr.height() as f64 / cell).ceil() as usize + 1); + let mut occ = vec![false; gw * gh]; + for (_, p) in &self.active { + let (cx, cy) = ((p[0] / cell) as usize, (p[1] / cell) as usize); + for yy in cy.saturating_sub(1)..=(cy + 1).min(gh - 1) { + for xx in cx.saturating_sub(1)..=(cx + 1).min(gw - 1) { + occ[yy * gw + xx] = true; + } + } + } + let free = |p: [f64; 2]| { + let (cx, cy) = ((p[0] / cell) as usize, (p[1] / cell) as usize); + !occ.get(cy * gw + cx).copied().unwrap_or(true) + }; + let opts = CornerOpts { max_features: want - self.active.len().min(want), min_distance: md, quality: 0.005, window: 2, border: 8 }; + for p in good_features(&pyr, &opts, Some(&free)) { + let l = pyr.to_layer(p); + let id = self.tracks.len() as u32; + self.tracks.push(Track2D { id, start: k, pts: vec![[l[0] as f32, l[1] as f32]] }); + self.active.push((id as usize, p)); + } + } + self.prev = Some(pyr); + self.frame += 1; + } + + pub fn finish(self, start: f64, frame_duration: f64) -> CameraTracks { + let mut tracks: Vec = self.tracks.into_iter().filter(|t| t.pts.len() >= 3).collect(); + // Round to 1/100 pixel (smaller JSON) and renumber densely. + for (i, t) in tracks.iter_mut().enumerate() { + t.id = i as u32; + for p in t.pts.iter_mut() { + *p = [(p[0] * 100.0).round() / 100.0, (p[1] * 100.0).round() / 100.0]; + } + } + CameraTracks { version: 1, size: self.size, start, frame_duration, frames: self.frame, factor: self.factor, detailed: self.opts.detailed, tracks } + } +} diff --git a/crates/track/src/camtrack/geometry.rs b/crates/track/src/camtrack/geometry.rs new file mode 100644 index 00000000..6956ac2b --- /dev/null +++ b/crates/track/src/camtrack/geometry.rs @@ -0,0 +1,432 @@ +//! Two-view and multi-view geometry for the camera solver, from Hartley & Zisserman (H&Z), +//! *Multiple View Geometry in Computer Vision* (2nd ed.) and Ma et al., *An Invitation to 3-D +//! Vision* (2004). +//! +//! All image points here are **normalised** (calibrated) coordinates `x = (u − c) / f`. +//! A camera is `(R, C)` with `X_cam = R (X − C)`. + +use super::linalg::*; + +/// A deterministic xorshift generator for RANSAC samples. +pub struct Rng(pub u64); +impl Rng { + pub fn next_u64(&mut self) -> u64 { + let mut x = self.0; + x ^= x << 13; + x ^= x >> 7; + x ^= x << 17; + self.0 = x; + x + } + pub fn below(&mut self, n: usize) -> usize { + (self.next_u64() % n.max(1) as u64) as usize + } + /// `k` distinct indices below `n`. + pub fn sample(&mut self, n: usize, k: usize, out: &mut Vec) { + out.clear(); + let mut guard = 0; + while out.len() < k && guard < 1000 { + guard += 1; + let c = self.below(n); + if !out.contains(&c) { + out.push(c); + } + } + } +} + +/// Hartley normalisation (H&Z §4.4.4): centroid to the origin, mean distance √2. +fn normalizer(p: &[[f64; 2]]) -> M3 { + let n = p.len().max(1) as f64; + let c = [p.iter().map(|q| q[0]).sum::() / n, p.iter().map(|q| q[1]).sum::() / n]; + let d = p.iter().map(|q| (q[0] - c[0]).hypot(q[1] - c[1])).sum::() / n; + let s = if d > 1e-12 { std::f64::consts::SQRT_2 / d } else { 1.0 }; + [[s, 0.0, -s * c[0]], [0.0, s, -s * c[1]], [0.0, 0.0, 1.0]] +} + +fn h2(m: &M3, p: [f64; 2]) -> V3 { + mv(m, [p[0], p[1], 1.0]) +} + +/// The essential matrix from ≥ 8 correspondences (`x2ᵀ E x1 = 0`) by the normalised 8-point +/// algorithm (H&Z Alg. 11.1), with the essential constraint (two equal singular values, one +/// zero) enforced (H&Z §9.6.1 / Result 9.18). +pub fn essential_8pt(x1: &[[f64; 2]], x2: &[[f64; 2]]) -> Option { + let n = x1.len().min(x2.len()); + if n < 8 { + return None; + } + let (t1, t2) = (normalizer(&x1[..n]), normalizer(&x2[..n])); + let mut a = Vec::with_capacity(n * 9); + for i in 0..n { + let p = h2(&t1, x1[i]); + let q = h2(&t2, x2[i]); + a.extend_from_slice(&[q[0] * p[0], q[0] * p[1], q[0], q[1] * p[0], q[1] * p[1], q[1], p[0], p[1], 1.0]); + } + let e = null_vector(&a, n, 9); + let en = [[e[0], e[1], e[2]], [e[3], e[4], e[5]], [e[6], e[7], e[8]]]; + // Denormalise: E = T2ᵀ Ê T1. + let e = mm(&mt(&t2), &mm(&en, &t1)); + let (u, s, v) = svd3(&e); + let m = (s[0] + s[1]) * 0.5; + if m <= 0.0 || !m.is_finite() { + return None; + } + let d = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]]; + Some(mm(&u, &mm(&d, &mt(&v)))) +} + +/// Squared Sampson distance of a correspondence to `x2ᵀ E x1 = 0` (H&Z §11.4.3), in the units of +/// the coordinates. +pub fn sampson2(e: &M3, p: [f64; 2], q: [f64; 2]) -> f64 { + let x1 = [p[0], p[1], 1.0]; + let x2 = [q[0], q[1], 1.0]; + let ex1 = mv(e, x1); + let etx2 = mtv(e, x2); + let num = dot(x2, ex1); + let den = ex1[0] * ex1[0] + ex1[1] * ex1[1] + etx2[0] * etx2[0] + etx2[1] * etx2[1]; + if den < 1e-300 { f64::INFINITY } else { num * num / den } +} + +/// RANSAC essential matrix (8-point samples, Sampson distance below `thr`), refitted on the +/// consensus set. Returns the matrix and the inlier mask. +pub fn ransac_essential(x1: &[[f64; 2]], x2: &[[f64; 2]], thr: f64, iterations: usize, seed: u64) -> Option<(M3, Vec)> { + let n = x1.len().min(x2.len()); + if n < 8 { + return None; + } + let t2 = thr * thr; + let mut rng = Rng(seed.wrapping_mul(0x9E37_79B9_7F4A_7C15) | 1); + let mut best: Option<(usize, f64, M3)> = None; + let mut idx = vec![]; + let mut max_iter = iterations; + let mut it = 0; + let (mut s1, mut s2) = (vec![[0.0; 2]; 8], vec![[0.0; 2]; 8]); + while it < max_iter { + it += 1; + rng.sample(n, 8, &mut idx); + if idx.len() < 8 { + break; + } + for (k, &i) in idx.iter().enumerate() { + s1[k] = x1[i]; + s2[k] = x2[i]; + } + let Some(e) = essential_8pt(&s1, &s2) else { continue }; + let (mut c, mut err) = (0, 0.0); + for i in 0..n { + let d = sampson2(&e, x1[i], x2[i]); + if d < t2 { + c += 1; + err += d; + } + } + if best.as_ref().is_none_or(|b| c > b.0 || (c == b.0 && err < b.1)) { + best = Some((c, err, e)); + // Adaptive iteration count for 99 % confidence. + let w = c as f64 / n as f64; + let p_good = w.powi(8); + if p_good > 1e-9 { + let need = ((1.0 - 0.99f64).ln() / (1.0 - p_good).max(1e-12).ln()).ceil(); + if need.is_finite() { + max_iter = max_iter.min((need as usize).max(30)); + } + } + } + } + let (_, _, mut e) = best?; + // Refit on the inliers (twice: the consensus may grow). + let mut inl = vec![false; n]; + for _ in 0..2 { + for i in 0..n { + inl[i] = sampson2(&e, x1[i], x2[i]) < t2; + } + let (a, b): (Vec<[f64; 2]>, Vec<[f64; 2]>) = (0..n).filter(|i| inl[*i]).map(|i| (x1[i], x2[i])).unzip(); + match essential_8pt(&a, &b) { + Some(e2) => e = e2, + None => break, + } + } + for i in 0..n { + inl[i] = sampson2(&e, x1[i], x2[i]) < t2; + } + Some((e, inl)) +} + +/// The four `(R, t)` with `x2 ~ R x1 + t` (|t| = 1) of an essential matrix (H&Z Result 9.19). +pub fn decompose_essential(e: &M3) -> [(M3, V3); 4] { + let (mut u, _, mut v) = svd3(e); + if det3(&u) < 0.0 { + for row in u.iter_mut() { + row[2] = -row[2]; + } + } + if det3(&v) < 0.0 { + for row in v.iter_mut() { + row[2] = -row[2]; + } + } + let w = [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]; + let r1 = mm(&u, &mm(&w, &mt(&v))); + let r2 = mm(&u, &mm(&mt(&w), &mt(&v))); + let t = col(&u, 2); + [(r1, t), (r1, scale(t, -1.0)), (r2, t), (r2, scale(t, -1.0))] +} + +/// A camera `(R, C)`. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Pose { + pub r: M3, + pub c: V3, +} + +impl Pose { + pub const IDENTITY: Pose = Pose { r: I3, c: [0.0; 3] }; + /// From `x_cam = R X + t`. + pub fn from_rt(r: M3, t: V3) -> Pose { + Pose { r, c: scale(mtv(&r, t), -1.0) } + } + pub fn t(&self) -> V3 { + scale(mv(&self.r, self.c), -1.0) + } + pub fn to_cam(&self, x: V3) -> V3 { + mv(&self.r, sub(x, self.c)) + } +} + +/// Linear (DLT) triangulation from normalised observations (H&Z §12.2), minimising the +/// algebraic error over all views. +pub fn triangulate(views: &[(Pose, [f64; 2])]) -> Option { + if views.len() < 2 { + return None; + } + let mut ata = [0.0f64; 16]; + for (p, x) in views { + let t = p.t(); + let rows = [[p.r[0][0], p.r[0][1], p.r[0][2], t[0]], [p.r[1][0], p.r[1][1], p.r[1][2], t[1]], [p.r[2][0], p.r[2][1], p.r[2][2], t[2]]]; + let a0: Vec = (0..4).map(|j| x[0] * rows[2][j] - rows[0][j]).collect(); + let a1: Vec = (0..4).map(|j| x[1] * rows[2][j] - rows[1][j]).collect(); + for a in [a0, a1] { + let nrm = a.iter().map(|v| v * v).sum::().sqrt().max(1e-300); + for i in 0..4 { + for j in 0..4 { + ata[i * 4 + j] += a[i] * a[j] / (nrm * nrm); + } + } + } + } + let (_, vecs) = sym_eigen(&ata, 4); + let h = [vecs[0], vecs[4], vecs[8], vecs[12]]; + if h[3].abs() < 1e-12 { + return None; + } + let x = [h[0] / h[3], h[1] / h[3], h[2] / h[3]]; + x.iter().all(|v| v.is_finite()).then_some(x) +} + +/// Angle (radians) between the rays from two camera centres to `x`. +pub fn ray_angle(a: V3, b: V3, x: V3) -> f64 { + let (u, v) = (normalize(sub(x, a)), normalize(sub(x, b))); + dot(u, v).clamp(-1.0, 1.0).acos() +} + +/// Squared reprojection error (normalised units) of `x` in `pose` against observation `o` +/// (`None` behind the camera). +pub fn reproj2(pose: &Pose, x: V3, o: [f64; 2]) -> Option { + let c = pose.to_cam(x); + if c[2] <= 1e-9 { + return None; + } + let (u, v) = (c[0] / c[2] - o[0], c[1] / c[2] - o[1]); + Some(u * u + v * v) +} + +/// Pick the essential decomposition with the most points in front of both cameras; returns +/// the second camera (the first is the identity) and that count. +pub fn select_pose(cands: &[(M3, V3)], x1: &[[f64; 2]], x2: &[[f64; 2]]) -> Option<(Pose, usize)> { + let mut best: Option<(Pose, usize)> = None; + for (r, t) in cands { + let p2 = Pose::from_rt(*r, *t); + let mut c = 0; + for i in 0..x1.len() { + if let Some(x) = triangulate(&[(Pose::IDENTITY, x1[i]), (p2, x2[i])]) + && x[2] > 0.0 + && p2.to_cam(x)[2] > 0.0 + { + c += 1; + } + } + if best.as_ref().is_none_or(|b| c > b.1) { + best = Some((p2, c)); + } + } + best +} + +/// Decompose a calibrated planar homography `H ~ R + t nᵀ / d` (normalised coordinates, +/// `x2 ~ H x1`) into its motions `(R, t/d, n)` (Ma et al. §5.3.3). Up to four solutions; those +/// that put the plane behind the first camera are dropped. +pub fn decompose_homography(h: &M3, x1: &[[f64; 2]], x2: &[[f64; 2]]) -> Vec<(M3, V3, V3)> { + let (_, s, _) = svd3(h); + if s[1] <= 1e-12 { + return vec![]; + } + let mut hn = mscale(h, 1.0 / s[1]); + // Sign: x2ᵀ H x1 > 0 for the correspondences (positive depth). + let pos = x1.iter().zip(x2).filter(|(p, q)| dot([q[0], q[1], 1.0], mv(&hn, [p[0], p[1], 1.0])) > 0.0).count(); + if pos * 2 < x1.len() { + hn = mscale(&hn, -1.0); + } + let hth = mm(&mt(&hn), &hn); + let a: Vec = hth.iter().flatten().copied().collect(); + let (vals, vecs) = sym_eigen(&a, 3); + // Descending: σ1² ≥ σ2² = 1 ≥ σ3². + let v1 = [vecs[2], vecs[5], vecs[8]]; + let v2 = [vecs[1], vecs[4], vecs[7]]; + let v3 = [vecs[0], vecs[3], vecs[6]]; + let (s1, s3) = (vals[2].max(1.0), vals[0].clamp(0.0, 1.0)); + let mut out = vec![]; + if s1 - s3 < 1e-10 { + // H is a rotation: no translation (pure rotation or plane at infinity). + return vec![(nearest_rotation(&hn), [0.0; 3], [0.0, 0.0, 1.0])]; + } + let k = (s1 - s3).sqrt(); + let (a1, a3) = ((1.0 - s3).sqrt(), (s1 - 1.0).sqrt()); + let u1 = scale(add(scale(v1, a1), scale(v3, a3)), 1.0 / k); + let u2 = scale(sub(scale(v1, a1), scale(v3, a3)), 1.0 / k); + for u in [u1, u2] { + let uu = from_cols(v2, u, cross(v2, u)); + let hv2 = mv(&hn, v2); + let hu = mv(&hn, u); + let ww = from_cols(hv2, hu, cross(hv2, hu)); + let r = mm(&ww, &mt(&uu)); + let n = cross(v2, u); + let t = mv(&madd(&hn, &r, -1.0), n); + for sgn in [1.0, -1.0] { + let (n2, t2) = (scale(n, sgn), scale(t, sgn)); + // The plane must be in front of the first camera: nᵀ x1 > 0 (with n pointing away). + let front = x1.iter().filter(|p| dot(n2, [p[0], p[1], 1.0]) > 0.0).count(); + if front * 2 >= x1.len() { + out.push((r, t2, n2)); + } + } + } + out +} + +/// Least-squares plane through points: (centroid, unit normal, RMS in-plane spread). +pub fn fit_plane(p: &[V3]) -> Option<(V3, V3, f64)> { + if p.len() < 3 { + return None; + } + let k = 1.0 / p.len() as f64; + let c = scale(p.iter().fold([0.0; 3], |a, b| add(a, *b)), k); + let mut cov = [0.0; 9]; + for q in p { + let d = sub(*q, c); + for i in 0..3 { + for j in 0..3 { + cov[i * 3 + j] += d[i] * d[j] * k; + } + } + } + let (vals, vecs) = sym_eigen(&cov, 3); + if vals[1] <= 1e-18 { + return None; + } + let n = normalize([vecs[0], vecs[3], vecs[6]]); + Some((c, n, (vals[1] + vals[2]).max(0.0).sqrt())) +} + +/// The rotation `R` minimising `Σ |b − R a|²` over unit rays (orthogonal Procrustes / Kabsch). +pub fn procrustes(a: &[V3], b: &[V3]) -> Option { + if a.len() < 2 { + return None; + } + let mut m = [[0.0; 3]; 3]; + for (x, y) in a.iter().zip(b) { + m = madd(&m, &outer(*y, *x), 1.0); + } + let (u, _, v) = svd3(&m); + let mut r = mm(&u, &mt(&v)); + if det3(&r) < 0.0 { + let mut u2 = u; + for row in u2.iter_mut() { + row[2] = -row[2]; + } + r = mm(&u2, &mt(&v)); + } + Some(r) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn scene() -> (Vec, Pose) { + let mut rng = Rng(12345); + let pts: Vec = (0..60) + .map(|_| { + let f = |r: &mut Rng| (r.next_u64() % 10_000) as f64 / 10_000.0 - 0.5; + [f(&mut rng) * 4.0, f(&mut rng) * 3.0, 5.0 + f(&mut rng) * 3.0] + }) + .collect(); + let p2 = Pose { r: rodrigues([0.02, -0.15, 0.03]), c: [0.6, 0.1, -0.2] }; + (pts, p2) + } + + fn proj(p: &Pose, x: V3) -> [f64; 2] { + let c = p.to_cam(x); + [c[0] / c[2], c[1] / c[2]] + } + + #[test] + fn essential_recovers_relative_pose() { + let (pts, p2) = scene(); + let x1: Vec<[f64; 2]> = pts.iter().map(|x| proj(&Pose::IDENTITY, *x)).collect(); + let mut x2: Vec<[f64; 2]> = pts.iter().map(|x| proj(&p2, *x)).collect(); + // Outliers. + x2[3] = [0.3, -0.2]; + x2[17] = [-0.1, 0.25]; + let (e, inl) = ransac_essential(&x1, &x2, 1e-3, 500, 7).unwrap(); + assert!(!inl[3] && !inl[17]); + assert!(inl.iter().filter(|b| **b).count() >= 57); + let (pose, n) = select_pose(&decompose_essential(&e), &x1, &x2).unwrap(); + assert!(n >= 57); + let r_err = norm(log_rot(&mm(&pose.r, &mt(&p2.r)))); + assert!(r_err < 1e-6, "{r_err}"); + let dir = normalize(p2.c); + assert!(norm(sub(normalize(pose.c), dir)) < 1e-5, "{:?} {:?}", pose.c, dir); + } + + #[test] + fn homography_decomposition_contains_the_motion() { + // Plane z = 4 (normal (0,0,1) in camera 1), seen after a motion. + let p2 = Pose { r: rodrigues([0.05, 0.2, -0.04]), c: [0.8, -0.3, 0.4] }; + let pts: Vec = (0..30).map(|i| [((i * 37) % 17) as f64 / 8.0 - 1.0, ((i * 11) % 13) as f64 / 6.0 - 1.0, 4.0]).collect(); + let x1: Vec<[f64; 2]> = pts.iter().map(|x| proj(&Pose::IDENTITY, *x)).collect(); + let x2: Vec<[f64; 2]> = pts.iter().map(|x| proj(&p2, *x)).collect(); + // H = R + t nᵀ / d with t = −R C, n = (0,0,1), d = 4. + let t = p2.t(); + let h = madd(&p2.r, &outer(t, [0.0, 0.0, 0.25]), 1.0); + let sols = decompose_homography(&mscale(&h, 3.7), &x1, &x2); + assert!(!sols.is_empty()); + let ok = sols + .iter() + .any(|(r, td, n)| norm(log_rot(&mm(r, &mt(&p2.r)))) < 1e-6 && norm(sub(*td, scale(t, 0.25))) < 1e-6 && norm(sub(*n, [0.0, 0.0, 1.0])) < 1e-6); + assert!(ok, "{sols:?}"); + } + + #[test] + fn triangulation_and_plane() { + let (pts, p2) = scene(); + for x in &pts[..5] { + let y = triangulate(&[(Pose::IDENTITY, proj(&Pose::IDENTITY, *x)), (p2, proj(&p2, *x))]).unwrap(); + assert!(norm(sub(*x, y)) < 1e-8); + } + let plane: Vec = (0..10).map(|i| [i as f64, (i * i % 7) as f64, 2.0 * i as f64 + 1.0]).collect(); + let (_, n, _) = fit_plane(&plane).unwrap(); + // All points satisfy z = 2x + 1: normal ∝ (2, 0, −1). + assert!(norm(cross(n, normalize([2.0, 0.0, -1.0]))) < 1e-9); + } +} diff --git a/crates/track/src/camtrack/linalg.rs b/crates/track/src/camtrack/linalg.rs new file mode 100644 index 00000000..a7067f26 --- /dev/null +++ b/crates/track/src/camtrack/linalg.rs @@ -0,0 +1,476 @@ +//! Small dense linear algebra for the camera solver, written from the textbook methods: +//! +//! - 3-vectors / 3 × 3 matrices and rotations (Rodrigues' formula and its inverse); +//! - the **one-sided Jacobi SVD** (Hestenes 1958; Golub & Van Loan, *Matrix Computations*, +//! §8.6.4), accurate for the small, badly scaled systems of the 8-point algorithm; +//! - the **cyclic Jacobi eigenvalue method** for symmetric matrices (Golub & Van Loan §8.5); +//! - **Cholesky** factorisation for the bundle adjustment's normal equations; +//! - the closed-form **similarity alignment** of two point sets (Umeyama, "Least-squares +//! estimation of transformation parameters between two point patterns", PAMI 1991). + +pub type V3 = [f64; 3]; +pub type M3 = [[f64; 3]; 3]; + +pub const I3: M3 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + +pub fn add(a: V3, b: V3) -> V3 { + [a[0] + b[0], a[1] + b[1], a[2] + b[2]] +} +pub fn sub(a: V3, b: V3) -> V3 { + [a[0] - b[0], a[1] - b[1], a[2] - b[2]] +} +pub fn scale(a: V3, k: f64) -> V3 { + [a[0] * k, a[1] * k, a[2] * k] +} +pub fn dot(a: V3, b: V3) -> f64 { + a[0] * b[0] + a[1] * b[1] + a[2] * b[2] +} +pub fn cross(a: V3, b: V3) -> V3 { + [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]] +} +pub fn norm(a: V3) -> f64 { + dot(a, a).sqrt() +} +pub fn normalize(a: V3) -> V3 { + let n = norm(a); + if n > 0.0 { scale(a, 1.0 / n) } else { a } +} +pub fn mv(m: &M3, v: V3) -> V3 { + [dot(m[0], v), dot(m[1], v), dot(m[2], v)] +} +/// `mᵀ v`. +pub fn mtv(m: &M3, v: V3) -> V3 { + [m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2], m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2], m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2]] +} +pub fn mm(a: &M3, b: &M3) -> M3 { + let mut o = [[0.0; 3]; 3]; + for (i, row) in o.iter_mut().enumerate() { + for (j, v) in row.iter_mut().enumerate() { + *v = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j]; + } + } + o +} +pub fn mt(a: &M3) -> M3 { + [[a[0][0], a[1][0], a[2][0]], [a[0][1], a[1][1], a[2][1]], [a[0][2], a[1][2], a[2][2]]] +} +pub fn madd(a: &M3, b: &M3, k: f64) -> M3 { + let mut o = *a; + for i in 0..3 { + for j in 0..3 { + o[i][j] += k * b[i][j]; + } + } + o +} +pub fn mscale(a: &M3, k: f64) -> M3 { + madd(&[[0.0; 3]; 3], a, k) +} +pub fn det3(m: &M3) -> f64 { + m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]) +} +pub fn skew(v: V3) -> M3 { + [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]] +} +pub fn outer(a: V3, b: V3) -> M3 { + [[a[0] * b[0], a[0] * b[1], a[0] * b[2]], [a[1] * b[0], a[1] * b[1], a[1] * b[2]], [a[2] * b[0], a[2] * b[1], a[2] * b[2]]] +} +pub fn col(m: &M3, j: usize) -> V3 { + [m[0][j], m[1][j], m[2][j]] +} +pub fn from_cols(a: V3, b: V3, c: V3) -> M3 { + [[a[0], b[0], c[0]], [a[1], b[1], c[1]], [a[2], b[2], c[2]]] +} +pub fn inv3(m: &M3) -> Option { + let d = det3(m); + if d.abs() < 1e-300 { + return None; + } + let c = |r0: usize, c0: usize, r1: usize, c1: usize| m[r0][c0] * m[r1][c1] - m[r0][c1] * m[r1][c0]; + let adj = [[c(1, 1, 2, 2), -c(0, 1, 2, 2), c(0, 1, 1, 2)], [-c(1, 0, 2, 2), c(0, 0, 2, 2), -c(0, 0, 1, 2)], [c(1, 0, 2, 1), -c(0, 0, 2, 1), c(0, 0, 1, 1)]]; + Some(mscale(&adj, 1.0 / d)) +} + +/// Rotation matrix of an axis-angle vector (Rodrigues' formula). +pub fn rodrigues(w: V3) -> M3 { + let th = norm(w); + let k = skew(w); + if th < 1e-12 { + return madd(&I3, &k, 1.0); + } + let a = th.sin() / th; + let b = (1.0 - th.cos()) / (th * th); + madd(&madd(&I3, &k, a), &mm(&k, &k), b) +} + +/// Axis-angle vector of a rotation matrix (inverse of [`rodrigues`]). +pub fn log_rot(r: &M3) -> V3 { + let c = ((r[0][0] + r[1][1] + r[2][2] - 1.0) * 0.5).clamp(-1.0, 1.0); + let th = c.acos(); + let v = [r[2][1] - r[1][2], r[0][2] - r[2][0], r[1][0] - r[0][1]]; + if th < 1e-9 { + return scale(v, 0.5); + } + if std::f64::consts::PI - th < 1e-6 { + // Near π: the axis is the dominant column of R + I. + let b = madd(r, &I3, 1.0); + let j = (0..3).max_by(|&a, &bb| b[a][a].total_cmp(&b[bb][bb])).unwrap_or(0); + let axis = normalize(col(&b, j)); + return scale(axis, th); + } + scale(v, th / (2.0 * th.sin())) +} + +/// The rotation closest to `m` (Frobenius norm), via the SVD. +pub fn nearest_rotation(m: &M3) -> M3 { + let (u, _, v) = svd3(m); + let mut r = mm(&u, &mt(&v)); + if det3(&r) < 0.0 { + let mut u2 = u; + for row in u2.iter_mut() { + row[2] = -row[2]; + } + r = mm(&u2, &mt(&v)); + } + r +} + +/// Spherical interpolation between two rotations. +pub fn slerp(a: &M3, b: &M3, t: f64) -> M3 { + let d = log_rot(&mm(b, &mt(a))); + mm(&rodrigues(scale(d, t)), a) +} + +/// Thin SVD `A = U diag(s) Vᵀ` of an `m × n` row-major matrix (one-sided Jacobi). `U` is +/// `max(m, n) × n` (rows beyond `m` belong to zero padding), `s` is sorted descending and `V` +/// is `n × n` (columns are the right singular vectors). +pub fn svd(a: &[f64], m: usize, n: usize) -> (Vec, Vec, Vec) { + let rows = m.max(n); + let mut u = vec![0.0; rows * n]; + u[..m * n].copy_from_slice(&a[..m * n]); + let mut v = vec![0.0; n * n]; + for i in 0..n { + v[i * n + i] = 1.0; + } + for _sweep in 0..60 { + let mut off = 0.0f64; + for p in 0..n { + for q in p + 1..n { + let (mut alpha, mut beta, mut gamma) = (0.0, 0.0, 0.0); + for r in 0..rows { + let (x, y) = (u[r * n + p], u[r * n + q]); + alpha += x * x; + beta += y * y; + gamma += x * y; + } + if gamma == 0.0 { + continue; + } + let c0 = gamma.abs() / (alpha * beta).sqrt().max(1e-300); + off = off.max(c0); + if c0 < 1e-15 { + continue; + } + let zeta = (beta - alpha) / (2.0 * gamma); + let t = zeta.signum() / (zeta.abs() + (1.0 + zeta * zeta).sqrt()); + let t = if zeta == 0.0 { 1.0 } else { t }; + let c = 1.0 / (1.0 + t * t).sqrt(); + let s = c * t; + for r in 0..rows { + let (x, y) = (u[r * n + p], u[r * n + q]); + u[r * n + p] = c * x - s * y; + u[r * n + q] = s * x + c * y; + } + for r in 0..n { + let (x, y) = (v[r * n + p], v[r * n + q]); + v[r * n + p] = c * x - s * y; + v[r * n + q] = s * x + c * y; + } + } + } + if off < 1e-15 { + break; + } + } + let mut s: Vec = (0..n).map(|j| (0..rows).map(|r| u[r * n + j] * u[r * n + j]).sum::().sqrt()).collect(); + for (j, sj) in s.iter().enumerate() { + if *sj > 1e-300 { + for r in 0..rows { + u[r * n + j] /= sj; + } + } + } + // Sort descending. + let mut order: Vec = (0..n).collect(); + order.sort_by(|&a, &b| s[b].total_cmp(&s[a])); + let (mut u2, mut v2) = (vec![0.0; rows * n], vec![0.0; n * n]); + for (k, &j) in order.iter().enumerate() { + for r in 0..rows { + u2[r * n + k] = u[r * n + j]; + } + for r in 0..n { + v2[r * n + k] = v[r * n + j]; + } + } + s = order.iter().map(|&j| s[j]).collect(); + (u2, s, v2) +} + +/// SVD of a 3 × 3 matrix: (U, singular values descending, V) with `m = U diag(s) Vᵀ`. +pub fn svd3(m: &M3) -> (M3, V3, M3) { + let a: Vec = m.iter().flatten().copied().collect(); + let (u, s, v) = svd(&a, 3, 3); + let g = |x: &[f64]| [[x[0], x[1], x[2]], [x[3], x[4], x[5]], [x[6], x[7], x[8]]]; + let mut um = g(&u); + // Columns of U for zero singular values are undefined: complete them to an orthonormal basis. + if s[2] < 1e-300 { + let c = cross(col(&um, 0), col(&um, 1)); + for (r, row) in um.iter_mut().enumerate() { + row[2] = c[r]; + } + } + (um, [s[0], s[1], s[2]], g(&v)) +} + +/// The unit vector `x` minimising `|A x|` (`m × n` row-major): the right singular vector of +/// the smallest singular value. +pub fn null_vector(a: &[f64], m: usize, n: usize) -> Vec { + let (_, _, v) = svd(a, m, n); + (0..n).map(|r| v[r * n + n - 1]).collect() +} + +/// Eigen-decomposition of a symmetric `n × n` matrix (cyclic Jacobi): eigenvalues ascending +/// and the matching eigenvectors as the columns of the returned row-major matrix. +pub fn sym_eigen(a: &[f64], n: usize) -> (Vec, Vec) { + let mut a = a.to_vec(); + let mut v = vec![0.0; n * n]; + for i in 0..n { + v[i * n + i] = 1.0; + } + for _ in 0..100 { + let mut off = 0.0; + for p in 0..n { + for q in p + 1..n { + off += a[p * n + q] * a[p * n + q]; + } + } + if off < 1e-30 { + break; + } + for p in 0..n { + for q in p + 1..n { + let apq = a[p * n + q]; + if apq.abs() < 1e-300 { + continue; + } + let theta = (a[q * n + q] - a[p * n + p]) / (2.0 * apq); + let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt()); + let t = if theta == 0.0 { 1.0 } else { t }; + let c = 1.0 / (t * t + 1.0).sqrt(); + let s = t * c; + for k in 0..n { + let (akp, akq) = (a[k * n + p], a[k * n + q]); + a[k * n + p] = c * akp - s * akq; + a[k * n + q] = s * akp + c * akq; + } + for k in 0..n { + let (apk, aqk) = (a[p * n + k], a[q * n + k]); + a[p * n + k] = c * apk - s * aqk; + a[q * n + k] = s * apk + c * aqk; + } + for k in 0..n { + let (vkp, vkq) = (v[k * n + p], v[k * n + q]); + v[k * n + p] = c * vkp - s * vkq; + v[k * n + q] = s * vkp + c * vkq; + } + } + } + } + let mut order: Vec = (0..n).collect(); + order.sort_by(|&x, &y| a[x * n + x].total_cmp(&a[y * n + y])); + let vals = order.iter().map(|&j| a[j * n + 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] = v[r * n + j]; + } + } + (vals, vecs) +} + +/// Solve `A x = b` for a symmetric positive definite `A` (`n × n`, row-major) in place by +/// Cholesky factorisation. Returns false when `A` is not positive definite. +pub fn cholesky_solve(a: &mut [f64], n: usize, b: &mut [f64]) -> bool { + for j in 0..n { + let mut d = a[j * n + j]; + for k in 0..j { + d -= a[j * n + k] * a[j * n + k]; + } + if d <= 0.0 || !d.is_finite() { + return false; + } + let d = d.sqrt(); + a[j * n + j] = d; + let (head, tail) = a.split_at_mut((j + 1) * n); + let rowj = &head[j * n..j * n + j]; + // Rows below j are independent. + use rayon::prelude::*; + let work = |row: &mut [f64]| { + let s: f64 = row[j] - row[..j].iter().zip(rowj).map(|(a, b)| a * b).sum::(); + row[j] = s / d; + }; + if n - j > 192 { + tail.par_chunks_mut(n).for_each(work); + } else { + tail.chunks_mut(n).for_each(work); + } + } + // Forward: L y = b. + for i in 0..n { + let mut s = b[i]; + for k in 0..i { + s -= a[i * n + k] * b[k]; + } + b[i] = s / a[i * n + i]; + } + // Back: Lᵀ x = y. + for i in (0..n).rev() { + let mut s = b[i]; + for k in i + 1..n { + s -= a[k * n + i] * b[k]; + } + b[i] = s / a[i * n + i]; + } + b.iter().all(|v| v.is_finite()) +} + +/// A similarity transform `x ↦ s R x + t`. +#[derive(Clone, Copy, Debug, PartialEq, serde::Serialize, serde::Deserialize)] +pub struct Similarity { + pub s: f64, + pub r: M3, + pub t: V3, +} + +impl Default for Similarity { + fn default() -> Self { + Similarity { s: 1.0, r: I3, t: [0.0; 3] } + } +} + +impl Similarity { + pub fn apply(&self, x: V3) -> V3 { + add(scale(mv(&self.r, x), self.s), self.t) + } +} + +/// The similarity minimising `Σ |dst − (s R src + t)|²` (Umeyama 1991). +pub fn umeyama(src: &[V3], dst: &[V3]) -> Option { + let n = src.len().min(dst.len()); + if n < 3 { + return None; + } + let k = 1.0 / n as f64; + let ms = scale(src[..n].iter().fold([0.0; 3], |a, b| add(a, *b)), k); + let md = scale(dst[..n].iter().fold([0.0; 3], |a, b| add(a, *b)), k); + let mut cov = [[0.0; 3]; 3]; + let mut var = 0.0; + for i in 0..n { + let (a, b) = (sub(src[i], ms), sub(dst[i], md)); + cov = madd(&cov, &outer(b, a), k); + var += dot(a, a) * k; + } + if var < 1e-300 { + return None; + } + let (u, d, v) = svd3(&cov); + let mut sgn = [1.0, 1.0, 1.0]; + if det3(&u) * det3(&v) < 0.0 { + sgn[2] = -1.0; + } + let ud = from_cols(scale(col(&u, 0), sgn[0]), scale(col(&u, 1), sgn[1]), scale(col(&u, 2), sgn[2])); + let r = mm(&ud, &mt(&v)); + let s = (d[0] * sgn[0] + d[1] * sgn[1] + d[2] * sgn[2]) / var; + let t = sub(md, scale(mv(&r, ms), s)); + Some(Similarity { s, r, t }) +} + +/// Euler angles (degrees) `[x, y, z]` with `m = Rz(z) Ry(y) Rx(x)` (After Effects' Orientation +/// order: X, then Y, then Z applied to the vector). +pub fn euler_xyz(m: &M3) -> V3 { + let sy = (-m[2][0]).clamp(-1.0, 1.0); + let y = sy.asin(); + let (x, z) = if sy.abs() < 0.999_999 { (m[2][1].atan2(m[2][2]), m[1][0].atan2(m[0][0])) } else { ((-m[1][2]).atan2(m[1][1]), 0.0) }; + [x.to_degrees(), y.to_degrees(), z.to_degrees()] +} + +/// `Rz(z) Ry(y) Rx(x)` from degrees. +pub fn from_euler_xyz(e: V3) -> M3 { + let (sx, cx) = e[0].to_radians().sin_cos(); + let (sy, cy) = e[1].to_radians().sin_cos(); + let (sz, cz) = e[2].to_radians().sin_cos(); + let rx = [[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]]; + let ry = [[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]]; + let rz = [[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]]; + mm(&rz, &mm(&ry, &rx)) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn svd_reconstructs() { + let a = [3.0, 1.0, 2.0, -1.0, 4.0, 0.5, 2.0, 2.0, 7.0, 0.1, -3.0, 1.0]; + let (u, s, v) = svd(&a, 4, 3); + for r in 0..4 { + for c in 0..3 { + let x: f64 = (0..3).map(|k| u[r * 3 + k] * s[k] * v[c * 3 + k]).sum(); + assert!((x - a[r * 3 + c]).abs() < 1e-10); + } + } + assert!(s[0] >= s[1] && s[1] >= s[2]); + } + + #[test] + fn rotations_round_trip() { + let w = [0.3, -1.2, 0.7]; + let r = rodrigues(w); + let w2 = log_rot(&r); + assert!(norm(sub(w, w2)) < 1e-12); + let e = euler_xyz(&r); + let r2 = from_euler_xyz(e); + for i in 0..3 { + for j in 0..3 { + assert!((r[i][j] - r2[i][j]).abs() < 1e-12); + } + } + assert!((det3(&r) - 1.0).abs() < 1e-12); + } + + #[test] + fn eigen_and_cholesky() { + let a = [4.0, 1.0, 0.5, 1.0, 3.0, 0.2, 0.5, 0.2, 2.0]; + let (vals, vecs) = sym_eigen(&a, 3); + for k in 0..3 { + let v = [vecs[k], vecs[3 + k], vecs[6 + k]]; + let av = [dot([a[0], a[1], a[2]], v), dot([a[3], a[4], a[5]], v), dot([a[6], a[7], a[8]], v)]; + assert!(norm(sub(av, scale(v, vals[k]))) < 1e-10); + } + let mut m = a; + let mut b = [1.0, 2.0, 3.0]; + assert!(cholesky_solve(&mut m, 3, &mut b)); + let r = [dot([a[0], a[1], a[2]], b), dot([a[3], a[4], a[5]], b), dot([a[6], a[7], a[8]], b)]; + assert!(norm(sub(r, [1.0, 2.0, 3.0])) < 1e-12); + } + + #[test] + fn umeyama_recovers_a_similarity() { + let t = Similarity { s: 2.5, r: rodrigues([0.1, 0.4, -0.3]), t: [1.0, -2.0, 5.0] }; + let src = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 2.0, 3.0]]; + let dst: Vec = src.iter().map(|p| t.apply(*p)).collect(); + let e = umeyama(&src, &dst).unwrap(); + assert!((e.s - 2.5).abs() < 1e-10); + assert!(norm(sub(e.t, t.t)) < 1e-9); + } +} diff --git a/crates/track/src/camtrack/mod.rs b/crates/track/src/camtrack/mod.rs new file mode 100644 index 00000000..056d7ad0 --- /dev/null +++ b/crates/track/src/camtrack/mod.rs @@ -0,0 +1,376 @@ +//! The 3D Camera Tracker's analysis: structure from motion on a single clip, clean-room from the +//! textbook pipeline (Hartley & Zisserman, *Multiple View Geometry in Computer Vision*, 2nd ed., +//! 2004; Triggs et al., "Bundle Adjustment — A Modern Synthesis", 2000): +//! +//! 1. **Feature tracks** ([`features::TrackAnalyzer`]): Shi–Tomasi corners followed through the +//! clip with pyramidal Lucas–Kanade and a forward–backward check ([`crate::klt`]); lost +//! features end their track and new ones are detected where the frame has none, so tracks +//! are as long as possible. +//! 2. **Solve** ([`solve`]): keyframes are selected by parallax; a two-view reconstruction +//! initialises the scene from either the **essential matrix** (normalised 8-point algorithm +//! inside RANSAC, H&Z §11.2 and §9.6) or, for mostly flat scenes, a **homography +//! decomposition** (Ma, Soatto, Košecká & Sastry, *An Invitation to 3-D Vision*, 2004, §5.3); +//! the other keyframes are added incrementally (**resection** by robust pose refinement, +//! **triangulation** of new tracks by the linear method of H&Z §12.2); a sparse +//! **Levenberg–Marquardt bundle adjustment** with the Schur complement over the points +//! ([`bundle`]) refines everything; the frames between keyframes are resected last. +//! 3. **Focal length**: *Specify Angle of View* fixes it; *Fixed Angle of View* searches it +//! (the reprojection error of the keyframe reconstruction over a log-spaced range of focal +//! lengths, refined by golden-section search) and then adjusts it in the bundle adjustment +//! as one shared parameter; *Variable Zoom* starts from the same estimate and adjusts a focal +//! length per frame, with a weak smoothness prior between neighbouring frames. +//! 4. **Tripod pan** shots (no camera translation) are solved as a pure rotation: frame-to-frame +//! rotations from the tracked rays (orthogonal Procrustes) refined by the same bundle +//! adjustment with the camera centres fixed. *Auto Detect* solves both ways and keeps the +//! tripod solution when it explains the tracks as well as the general one. +//! +//! The camera model has square pixels, the principal point at the layer centre and no lens +//! distortion. Camera space is x right, y down, z forward (After Effects' convention); a camera +//! is `(R, C, f)` with `X_cam = R (X − C)` and pixel `u = f x / z + w / 2`. +//! +//! The solve is stored in a canonical frame: the first solved frame's camera is at the origin +//! looking along +z, scaled so the median depth of the points it sees is 1. [`CameraSolve::world`] +//! maps it into composition space for a camera zoom, optionally re-based on a ground plane. + +pub mod bundle; +pub mod features; +pub mod geometry; +pub mod linalg; +mod solver; +#[cfg(test)] +mod tests; + +use serde::{Deserialize, Serialize}; + +pub use features::{AnalyzeOpts, TrackAnalyzer}; +use linalg::{M3, Similarity, V3}; +pub use solver::{SolveError, solve}; + +/// One feature track: positions (layer pixels) on consecutive frames from `start`. +#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase")] +pub struct Track2D { + pub id: u32, + pub start: u32, + pub pts: Vec<[f32; 2]>, +} + +impl Track2D { + pub fn end(&self) -> u32 { + self.start + self.pts.len() as u32 + } + /// Position on frame `k` when the track covers it. + pub fn at(&self, k: u32) -> Option<[f64; 2]> { + let i = k.checked_sub(self.start)? as usize; + self.pts.get(i).map(|p| [p[0] as f64, p[1] as f64]) + } +} + +/// The tracked features of a clip (step 1 of the analysis). +#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct CameraTracks { + pub version: u32, + /// Layer size (pixels). + pub size: [f64; 2], + /// Layer time (seconds) of frame 0, and the frame duration. + pub start: f64, + pub frame_duration: f64, + pub frames: u32, + /// Analysis downsampling factor (image pixels per analysis pixel): the tracking noise scale. + pub factor: f64, + pub detailed: bool, + pub tracks: Vec, +} + +impl CameraTracks { + pub fn to_json(&self) -> String { + serde_json::to_string(self).unwrap_or_default() + } + pub fn from_json(s: &str) -> Option { + if s.trim().is_empty() { + return None; + } + serde_json::from_str(s).ok() + } + pub fn is_empty(&self) -> bool { + self.frames == 0 + } + pub fn track(&self, id: u32) -> Option<&Track2D> { + self.tracks.binary_search_by_key(&id, |t| t.id).ok().map(|i| &self.tracks[i]) + } +} + +/// 3D Camera Tracker ▸ Shot Type. +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(rename_all = "camelCase")] +pub enum ShotType { + #[default] + FixedAngle, + VariableZoom, + SpecifyAngle, +} + +impl ShotType { + pub const ALL: [ShotType; 3] = [ShotType::FixedAngle, ShotType::VariableZoom, ShotType::SpecifyAngle]; + pub fn label(self) -> &'static str { + match self { + ShotType::FixedAngle => "Fixed Angle of View", + ShotType::VariableZoom => "Variable Zoom", + ShotType::SpecifyAngle => "Specify Angle of View", + } + } + pub fn from_name(s: &str) -> Option { + let k = s.to_ascii_lowercase().replace([' ', '-', '_'], ""); + match k.as_str() { + "fixed" | "fixedangle" | "fixedangleofview" => Some(ShotType::FixedAngle), + "variable" | "variablezoom" | "zoom" => Some(ShotType::VariableZoom), + "specify" | "specifyangle" | "specifyangleofview" => Some(ShotType::SpecifyAngle), + _ => None, + } + } +} + +/// 3D Camera Tracker ▸ Advanced ▸ Solve Method (and Method Used). +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(rename_all = "camelCase")] +pub enum SolveMethod { + #[default] + Auto, + Typical, + MostlyFlat, + TripodPan, +} + +impl SolveMethod { + pub const ALL: [SolveMethod; 4] = [SolveMethod::Auto, SolveMethod::Typical, SolveMethod::MostlyFlat, SolveMethod::TripodPan]; + pub fn label(self) -> &'static str { + match self { + SolveMethod::Auto => "Auto Detect", + SolveMethod::Typical => "Typical", + SolveMethod::MostlyFlat => "Mostly Flat Scene", + SolveMethod::TripodPan => "Tripod Pan", + } + } + pub fn from_name(s: &str) -> Option { + let k = s.to_ascii_lowercase().replace([' ', '-', '_'], ""); + match k.as_str() { + "auto" | "autodetect" => Some(SolveMethod::Auto), + "typical" => Some(SolveMethod::Typical), + "flat" | "mostlyflat" | "mostlyflatscene" => Some(SolveMethod::MostlyFlat), + "tripod" | "tripodpan" => Some(SolveMethod::TripodPan), + _ => None, + } + } +} + +/// What a solve is made with. +#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct SolveSettings { + pub shot: ShotType, + /// Horizontal angle of view (degrees) for [`ShotType::SpecifyAngle`]. + pub hfov: f64, + pub method: SolveMethod, + /// Track ids the user deleted. + pub deleted: Vec, +} + +/// The solved camera of one frame. +#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct SolvedFrame { + /// World → camera rotation (axis-angle). + pub rot: V3, + /// Camera centre. + pub center: V3, + /// Focal length (layer pixels). + pub focal: f64, + pub solved: bool, +} + +impl SolvedFrame { + pub fn r(&self) -> M3 { + linalg::rodrigues(self.rot) + } + /// Camera-space position of a world point. + pub fn to_cam(&self, x: V3) -> V3 { + linalg::mv(&self.r(), linalg::sub(x, self.center)) + } + /// Project a world point to layer pixels (`None` behind the camera). + pub fn project(&self, size: [f64; 2], x: V3) -> Option<[f64; 2]> { + let c = self.to_cam(x); + (c[2] > 1e-9).then(|| [self.focal * c[0] / c[2] + size[0] * 0.5, self.focal * c[1] / c[2] + size[1] * 0.5]) + } +} + +/// A solved 3D track point. +#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct SolvedPoint { + /// The feature track it was made from. + pub id: u32, + pub pos: V3, + /// Average reprojection error (pixels). + pub error: f32, + /// Frames `[first, last]` the point was tracked on. + pub first: u32, + pub last: u32, +} + +/// A ground plane and origin (canonical solve coordinates). +#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct Ground { + pub origin: V3, + /// Unit normal on the cameras' side ("up"). + pub normal: V3, + /// Unit in-plane direction that becomes world +x. + pub x_axis: V3, +} + +/// A plane through solved points: After Effects' target. +#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct Target { + pub center: V3, + /// Unit normal facing the camera. + pub normal: V3, + /// Diameter in canonical units. + pub size: f64, +} + +/// The camera solve (step 2 of the analysis). +#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "camelCase", default)] +pub struct CameraSolve { + pub version: u32, + pub size: [f64; 2], + pub start: f64, + pub frame_duration: f64, + pub shot: ShotType, + /// The model that was used (Auto Detect resolves to one of the others). + pub method_used: SolveMethod, + /// Average reprojection error (pixels) over all solved observations. + pub average_error: f64, + pub frames: Vec, + pub points: Vec, + #[serde(skip_serializing_if = "Option::is_none")] + pub ground: Option, +} + +impl CameraSolve { + pub fn to_json(&self) -> String { + serde_json::to_string(self).unwrap_or_default() + } + pub fn from_json(s: &str) -> Option { + if s.trim().is_empty() { + return None; + } + serde_json::from_str(s).ok() + } + pub fn is_empty(&self) -> bool { + self.frames.is_empty() + } + /// Frame index at a layer time (held at the ends). + pub fn frame_at(&self, layer_time: f64) -> Option { + if self.frames.is_empty() || self.frame_duration <= 0.0 { + return None; + } + let i = ((layer_time - self.start) / self.frame_duration).round(); + Some(i.clamp(0.0, (self.frames.len() - 1) as f64) as usize) + } + pub fn point(&self, id: u32) -> Option<&SolvedPoint> { + self.points.iter().find(|p| p.id == id) + } + /// Points tracked on frame `k`. + pub fn visible(&self, k: usize) -> impl Iterator { + let k = k as u32; + self.points.iter().filter(move |p| p.first <= k && k <= p.last) + } + /// Median focal length of the solved frames (layer pixels). + pub fn focal(&self) -> f64 { + let mut f: Vec = self.frames.iter().filter(|c| c.solved).map(|c| c.focal).collect(); + if f.is_empty() { + return 0.0; + } + f.sort_by(f64::total_cmp); + f[f.len() / 2] + } + /// Horizontal angle of view (degrees) of focal length `f`. + pub fn hfov(&self, f: f64) -> f64 { + 2.0 * (self.size[0] * 0.5 / f.max(1e-9)).atan().to_degrees() + } + + /// The plane through points `ids` seen from frame `k` (needs three points; with more the + /// plane is a least-squares fit). + pub fn target(&self, ids: &[u32], k: usize) -> Option { + let pts: Vec = ids.iter().filter_map(|i| self.point(*i)).map(|p| p.pos).collect(); + let cam = self.frames.get(k).filter(|c| c.solved).or_else(|| self.frames.iter().find(|c| c.solved))?; + geometry::fit_plane(&pts).map(|(c, mut n, spread)| { + if linalg::dot(n, linalg::sub(cam.center, c)) < 0.0 { + n = linalg::scale(n, -1.0); + } + // After Effects sizes the target from the camera distance when the points are close. + let depth = linalg::norm(linalg::sub(cam.center, c)); + Target { center: c, normal: n, size: (spread * 2.0).max(depth * 0.12) } + }) + } + + /// A ground plane through `ids` (origin at their centroid, normal towards frame `k`'s + /// camera, +x along that camera's right vector projected on the plane). + pub fn ground_from(&self, ids: &[u32], k: usize) -> Option { + let t = self.target(ids, k)?; + let cam = self.frames.get(k).filter(|c| c.solved).or_else(|| self.frames.iter().find(|c| c.solved))?; + let right = linalg::mtv(&cam.r(), [1.0, 0.0, 0.0]); + let mut x = linalg::sub(right, linalg::scale(t.normal, linalg::dot(right, t.normal))); + if linalg::norm(x) < 1e-6 { + let fwd = linalg::mtv(&cam.r(), [0.0, 0.0, 1.0]); + x = linalg::cross(fwd, t.normal); + } + Some(Ground { origin: t.center, normal: t.normal, x_axis: linalg::normalize(x) }) + } + + /// Canonical solve coordinates → composition world, for a camera whose zoom on the first + /// solved frame is `zoom0` (comp pixels) in a `comp` sized composition. Without a ground + /// plane, the first solved frame's camera becomes After Effects' default camera (at + /// `(w/2, h/2, −zoom)` looking down +z, so points at the median depth land near z = 0). With + /// one, the ground plane becomes the X-Z plane through the origin, normal along −y (up). + pub fn world(&self, comp: [f64; 2], zoom0: f64) -> Similarity { + match &self.ground { + Some(g) => { + // Rows: canonical directions that map to world x, y, z. + let y = linalg::scale(g.normal, -1.0); + let x = g.x_axis; + let z = linalg::cross(x, y); + let r = [x, y, z]; + let t = linalg::scale(linalg::mv(&r, g.origin), -zoom0); + Similarity { s: zoom0, r, t } + } + None => Similarity { s: zoom0, r: linalg::I3, t: [comp[0] * 0.5, comp[1] * 0.5, -zoom0] }, + } + } + + /// The first solved frame. + pub fn first_solved(&self) -> Option { + self.frames.iter().position(|c| c.solved) + } +} + +/// Deterministic colour of a track point (After Effects draws each target in its own colour). +pub fn point_color(id: u32) -> [f32; 3] { + let mut h = id.wrapping_mul(0x9E37_79B9) ^ 0x5bd1_e995; + h ^= h >> 15; + let hue = (h % 360) as f32 / 60.0; + let x = 1.0 - (hue % 2.0 - 1.0).abs(); + let (r, g, b) = match hue as u32 { + 0 => (1.0, x, 0.0), + 1 => (x, 1.0, 0.0), + 2 => (0.0, 1.0, x), + 3 => (0.0, x, 1.0), + 4 => (x, 0.0, 1.0), + _ => (1.0, 0.0, x), + }; + // Bright, slightly desaturated. + [0.25 + 0.75 * r, 0.25 + 0.75 * g, 0.25 + 0.75 * b] +} diff --git a/crates/track/src/camtrack/solver.rs b/crates/track/src/camtrack/solver.rs new file mode 100644 index 00000000..da3a9290 --- /dev/null +++ b/crates/track/src/camtrack/solver.rs @@ -0,0 +1,894 @@ +//! The camera solve pipeline (see the module docs of [`super`]). + +use std::collections::{BTreeMap, HashMap, HashSet}; +use std::sync::atomic::{AtomicBool, Ordering}; + +use rayon::prelude::*; + +use super::bundle::{self, BaCam, BaObs, BaOpts, FocalMode}; +use super::geometry::{self, Pose}; +use super::linalg::*; +use super::{CameraSolve, CameraTracks, ShotType, SolveMethod, SolveSettings, SolvedFrame, SolvedPoint}; +use crate::fit::{Model, ransac}; + +fn debug() -> bool { + std::env::var_os("EC_CAMTRACK_DEBUG").is_some() +} + +/// Why a solve failed. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct SolveError(pub String); + +impl std::fmt::Display for SolveError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.write_str(&self.0) + } +} + +fn err(m: &str) -> Result { + Err(SolveError(m.to_string())) +} + +/// Tracks in solver form: pixels relative to the principal point. +struct Data { + n: usize, + w: f64, + /// (track id, first frame, positions). + tracks: Vec<(u32, usize, Vec<[f64; 2]>)>, + /// Per frame: (track index, position). + by_frame: Vec>, + /// Tracking noise scale (pixels). + noise: f64, +} + +impl Data { + fn new(t: &CameraTracks, deleted: &[u32]) -> Data { + let n = t.frames as usize; + let c = [t.size[0] * 0.5, t.size[1] * 0.5]; + let del: HashSet = deleted.iter().copied().collect(); + let mut tracks = vec![]; + let mut by_frame = vec![vec![]; n]; + for tr in t.tracks.iter().filter(|tr| !del.contains(&tr.id) && tr.pts.len() >= 3) { + let ti = tracks.len() as u32; + let pts: Vec<[f64; 2]> = tr.pts.iter().map(|p| [p[0] as f64 - c[0], p[1] as f64 - c[1]]).collect(); + for (k, p) in pts.iter().enumerate() { + if let Some(v) = by_frame.get_mut(tr.start as usize + k) { + v.push((ti, *p)); + } + } + tracks.push((tr.id, tr.start as usize, pts)); + } + Data { n, w: t.size[0], tracks, by_frame, noise: t.factor.max(1.0) } + } + fn at(&self, ti: u32, k: usize) -> Option<[f64; 2]> { + let (_, s, p) = &self.tracks[ti as usize]; + k.checked_sub(*s).and_then(|i| p.get(i).copied()) + } + fn inlier_thr(&self) -> f64 { + 4.0 * self.noise + } + fn huber(&self) -> f64 { + 1.0 * self.noise + } +} + +/// Parallax-based keyframes: a new keyframe when the median feature motion since the last +/// exceeds 3 % of the width, when fewer than 60 % of its features survive, or before the shared +/// features drop below 24. The first and last frames with features are keyframes. +fn keyframes(d: &Data) -> Vec { + let Some(first) = (0..d.n).find(|k| d.by_frame[*k].len() >= 8) else { return vec![] }; + let mut kf = vec![first]; + let mut last = first; + let mut j = first + 1; + while j < d.n { + let base = &d.by_frame[last]; + let mut disp = vec![]; + for (ti, p) in base { + if let Some(q) = d.at(*ti, j) { + disp.push((q[0] - p[0]).hypot(q[1] - p[1])); + } + } + if disp.len() < 24 && j > last + 1 { + // Too few shared features: the previous frame becomes a keyframe. + last = j - 1; + kf.push(last); + continue; + } + disp.sort_by(f64::total_cmp); + let med = disp.get(disp.len() / 2).copied().unwrap_or(0.0); + let ratio = disp.len() as f64 / base.len().max(1) as f64; + if med > 0.03 * d.w || ratio < 0.6 || disp.len() < 24 { + last = j; + kf.push(j); + } + j += 1; + } + if let Some(lastf) = (0..d.n).rev().find(|k| d.by_frame[*k].len() >= 8) + && *kf.last().expect("one") < lastf + { + kf.push(lastf); + } + kf +} + +/// A (partial) reconstruction. +#[derive(Clone, Default)] +struct Recon { + cams: BTreeMap, + pts: HashMap, + /// Rejected observations (track index, frame). + outliers: HashSet<(u32, usize)>, +} + +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +enum Kind { + General, + Tripod, +} + +#[derive(Clone, Copy, PartialEq, Debug)] +struct Cfg { + kind: Kind, + /// Focal mode of the global adjustments. + focal: FocalMode, + /// Registration estimates a focal length per frame. + per_frame_f: bool, +} + +const F_PRIOR: f64 = 30.0; + +impl Recon { + /// Observations among `frames` (inliers, with a 3D point). + fn observations(&self, d: &Data, frames: &[usize]) -> (Vec, Vec, Vec, Vec) { + let cams: Vec = frames.iter().map(|k| self.cams[k]).collect(); + let mut pidx: HashMap = HashMap::new(); + let mut pts = vec![]; + let mut ids = vec![]; + let mut obs = vec![]; + for (ci, k) in frames.iter().enumerate() { + for (ti, uv) in &d.by_frame[*k] { + let Some(x) = self.pts.get(ti) else { continue }; + if self.outliers.contains(&(*ti, *k)) { + continue; + } + let pi = *pidx.entry(*ti).or_insert_with(|| { + pts.push(*x); + ids.push(*ti); + (pts.len() - 1) as u32 + }); + obs.push(BaObs { cam: ci as u32, pt: pi, uv: *uv }); + } + } + (cams, pts, obs, ids) + } + + /// Bundle-adjust the cameras of `frames` (the first stays fixed) and their points. + fn adjust(&mut self, d: &Data, frames: &[usize], cfg: &Cfg, focal: FocalMode, iters: usize) -> f64 { + let (mut cams, mut pts, obs, ids) = self.observations(d, frames); + if obs.is_empty() { + return f64::INFINITY; + } + let mut fixed = vec![false; cams.len()]; + fixed[0] = true; + let rep = bundle::adjust( + &mut cams, + &mut pts, + &obs, + &BaOpts { focal, fix_points: false, fix_centers: cfg.kind == Kind::Tripod, fixed, huber: d.huber(), max_iter: iters }, + ); + for (k, c) in frames.iter().zip(&cams) { + self.cams.insert(*k, *c); + } + for (ti, x) in ids.iter().zip(&pts) { + self.pts.insert(*ti, *x); + } + rep.cost + } + + /// Mark observations above the inlier threshold as outliers; drop points left with fewer + /// than two observations among the registered frames. + fn reject(&mut self, d: &Data) { + let thr2 = d.inlier_thr().powi(2); + let mut count: HashMap = HashMap::new(); + for (k, c) in &self.cams { + for (ti, uv) in &d.by_frame[*k] { + let Some(x) = self.pts.get(ti) else { continue }; + if self.outliers.contains(&(*ti, *k)) { + continue; + } + match bundle::residual(c, *x, *uv) { + Some(r) if r[0] * r[0] + r[1] * r[1] <= thr2 => *count.entry(*ti).or_default() += 1, + _ => { + self.outliers.insert((*ti, *k)); + } + } + } + } + self.pts.retain(|ti, _| count.get(ti).copied().unwrap_or(0) >= 2); + } + + /// Triangulate tracks seen on ≥ 2 registered frames that have no point yet. + fn triangulate(&mut self, d: &Data, kind: Kind, min_angle_deg: f64, check: f64) { + let mut seen: HashMap> = HashMap::new(); + for k in self.cams.keys() { + for (ti, _) in &d.by_frame[*k] { + if !self.pts.contains_key(ti) { + seen.entry(*ti).or_default().push(*k); + } + } + } + let thr2 = (d.inlier_thr() * check).powi(2); + let new: Vec<(u32, V3)> = seen + .par_iter() + .filter(|(_, ks)| ks.len() >= 2) + .filter_map(|(ti, ks)| { + let views: Vec<(Pose, [f64; 2], &BaCam)> = ks + .iter() + .map(|k| { + let c = &self.cams[k]; + let uv = d.at(*ti, *k).expect("seen"); + (c.pose, [uv[0] / c.f, uv[1] / c.f], c) + }) + .collect(); + let x = match kind { + Kind::Tripod => normalize(views.iter().fold([0.0; 3], |a, (p, o, _)| add(a, normalize(mtv(&p.r, [o[0], o[1], 1.0]))))), + Kind::General => { + let v: Vec<(Pose, [f64; 2])> = views.iter().map(|(p, o, _)| (*p, *o)).collect(); + let x = geometry::triangulate(&v)?; + // Enough parallax between the most separated views. + let (a, b) = (views[0].0.c, views[views.len() - 1].0.c); + if geometry::ray_angle(a, b, x).to_degrees() < min_angle_deg { + return None; + } + x + } + }; + for (k, (_, _, c)) in ks.iter().zip(&views) { + let uv = d.at(*ti, *k).expect("seen"); + let r = bundle::residual(c, x, uv)?; + if r[0] * r[0] + r[1] * r[1] > thr2 { + return None; + } + } + Some((*ti, x)) + }) + .collect(); + self.pts.extend(new); + } + + /// Resect frame `k` from the existing points starting at `init` (pose, and the focal + /// length when `per_frame_f`). Returns false when too few points agree. + fn register(&mut self, d: &Data, k: usize, init: BaCam, cfg: &Cfg) -> bool { + let mut pts = vec![]; + let mut obs = vec![]; + let mut ids = vec![]; + for (ti, uv) in &d.by_frame[k] { + if let Some(x) = self.pts.get(ti) { + obs.push(BaObs { cam: 0, pt: pts.len() as u32, uv: *uv }); + pts.push(*x); + ids.push(*ti); + } + } + if obs.len() < 6 { + return false; + } + let thr2 = d.inlier_thr().powi(2); + let opts = |huber: f64| BaOpts { + focal: if cfg.per_frame_f { FocalMode::PerCamera(0.0) } else { FocalMode::Fixed }, + fix_points: true, + fix_centers: cfg.kind == Kind::Tripod, + fixed: vec![false], + huber, + max_iter: 40, + }; + let mut best: Option<(usize, BaCam)> = None; + // Robust start (large Huber threshold), then tighten; from the prediction and, failing + // that, from the nearest registered camera. + let nearest = self.cams.iter().min_by_key(|(c, _)| c.abs_diff(k)).map(|(_, c)| *c); + for start in std::iter::once(init).chain(nearest) { + if best.as_ref().is_some_and(|b| b.0 as f64 >= 0.8 * obs.len() as f64) { + break; + } + let mut cams = vec![start]; + bundle::adjust(&mut cams, &mut pts.clone(), &obs, &opts(d.inlier_thr() * 4.0)); + bundle::adjust(&mut cams, &mut pts.clone(), &obs, &opts(d.huber())); + let inl = obs.iter().filter(|o| bundle::residual(&cams[0], pts[o.pt as usize], o.uv).is_some_and(|r| r[0] * r[0] + r[1] * r[1] <= thr2)).count(); + if best.as_ref().is_none_or(|b| inl > b.0) { + best = Some((inl, cams[0])); + } + } + let Some((inl, cam)) = best else { return false }; + if inl < 6 || (inl as f64) < 0.5 * obs.len() as f64 { + return false; + } + // Refine on the inliers only. + let keep: Vec = + obs.iter().filter(|o| bundle::residual(&cam, pts[o.pt as usize], o.uv).is_some_and(|r| r[0] * r[0] + r[1] * r[1] <= thr2)).copied().collect(); + let mut cams = vec![cam]; + bundle::adjust(&mut cams, &mut pts.clone(), &keep, &opts(d.huber())); + for o in &obs { + let ok = bundle::residual(&cams[0], pts[o.pt as usize], o.uv).is_some_and(|r| r[0] * r[0] + r[1] * r[1] <= thr2); + if !ok { + self.outliers.insert((ids[o.pt as usize], k)); + } + } + self.cams.insert(k, cams[0]); + true + } + + /// How well the reconstruction explains every observation on its frames: the mean of the + /// reprojection error truncated at the inlier threshold, an observation without a point + /// counting as the threshold. + fn score(&self, d: &Data) -> f64 { + let thr = d.inlier_thr(); + let mut s = 0.0; + let mut n = 0usize; + for (k, c) in &self.cams { + for (ti, uv) in &d.by_frame[*k] { + n += 1; + s += match self.pts.get(ti).and_then(|x| bundle::residual(c, *x, *uv)) { + Some(r) => r[0].hypot(r[1]).min(thr), + None => thr, + }; + } + } + if n > 0 { s / n as f64 } else { f64::INFINITY } + } + + /// Average reprojection error (pixels) and observation count over the inliers. + fn error(&self, d: &Data) -> (f64, usize) { + let mut s = 0.0; + let mut n = 0; + for (k, c) in &self.cams { + for (ti, uv) in &d.by_frame[*k] { + let Some(x) = self.pts.get(ti) else { continue }; + if self.outliers.contains(&(*ti, *k)) { + continue; + } + if let Some(r) = bundle::residual(c, *x, *uv) { + s += r[0].hypot(r[1]); + n += 1; + } + } + } + (if n > 0 { s / n as f64 } else { f64::INFINITY }, n) + } +} + +/// Interpolated / extrapolated starting camera for frame `k` from the registered ones. +fn predict(r: &Recon, k: usize) -> Option { + let before = r.cams.range(..k).next_back().map(|(a, b)| (*a, *b)); + let after = r.cams.range(k + 1..).next().map(|(a, b)| (*a, *b)); + match (before, after) { + (Some((ka, a)), Some((kb, b))) => { + let t = (k - ka) as f64 / (kb - ka) as f64; + Some(BaCam { pose: Pose { r: slerp(&a.pose.r, &b.pose.r, t), c: add(scale(a.pose.c, 1.0 - t), scale(b.pose.c, t)) }, f: a.f * (b.f / a.f).powf(t) }) + } + (Some((ka, a)), None) => { + // Constant velocity from the two previous cameras. + match r.cams.range(..ka).next_back() { + Some((kp, p)) => { + let t = (k - ka) as f64 / (ka - kp) as f64; + let dr = log_rot(&mm(&a.pose.r, &mt(&p.pose.r))); + let rr = mm(&rodrigues(scale(dr, t.min(2.0))), &a.pose.r); + let c = add(a.pose.c, scale(sub(a.pose.c, p.pose.c), t.min(2.0))); + Some(BaCam { pose: Pose { r: rr, c }, f: a.f }) + } + None => Some(a), + } + } + (None, Some((kb, b))) => match r.cams.range(kb + 1..).next() { + Some((kn, nx)) => { + let t = (kb - k) as f64 / (kn - kb) as f64; + let dr = log_rot(&mm(&b.pose.r, &mt(&nx.pose.r))); + let rr = mm(&rodrigues(scale(dr, t.min(2.0))), &b.pose.r); + let c = add(b.pose.c, scale(sub(b.pose.c, nx.pose.c), t.min(2.0))); + Some(BaCam { pose: Pose { r: rr, c }, f: b.f }) + } + None => Some(b), + }, + (None, None) => None, + } +} + +/// Shared tracks of frames `a` and `b`: (track index, pixels in a, pixels in b). +fn shared(d: &Data, a: usize, b: usize) -> Vec<(u32, [f64; 2], [f64; 2])> { + d.by_frame[a].iter().filter_map(|(ti, p)| d.at(*ti, b).map(|q| (*ti, *p, q))).collect() +} + +/// Two-view initialisations of frames `a`, `b` at focal length `f`: the second camera and the +/// triangulated points (the first camera is the identity). +fn init_pair(d: &Data, a: usize, b: usize, f: f64, use_e: bool, use_h: bool) -> Vec<(Pose, HashMap)> { + let sh = shared(d, a, b); + if sh.len() < 12 { + return vec![]; + } + let x1: Vec<[f64; 2]> = sh.iter().map(|s| [s.1[0] / f, s.1[1] / f]).collect(); + let x2: Vec<[f64; 2]> = sh.iter().map(|s| [s.2[0] / f, s.2[1] / f]).collect(); + let thr = 2.0 * d.noise / f; + let mut cands: Vec = vec![]; + if use_e && let Some((e, inl)) = geometry::ransac_essential(&x1, &x2, thr, 400, (a * 7919 + b) as u64) { + let (p1, p2): (Vec<[f64; 2]>, Vec<[f64; 2]>) = (0..x1.len()).filter(|i| inl[*i]).map(|i| (x1[i], x2[i])).unzip(); + if let Some((pose, _)) = geometry::select_pose(&geometry::decompose_essential(&e), &p1, &p2) { + cands.push(pose); + } + } + if use_h { + let (s1, s2): (Vec<[f64; 2]>, Vec<[f64; 2]>) = sh.iter().map(|s| (s.1, s.2)).unzip(); + if let Some(fit) = ransac(Model::Homography, &s1, &s2, 2.0 * d.noise, 500, (a * 31 + b) as u64) { + let h = fit.h.0; + // Normalised: K⁻¹ H K with K = diag(f, f, 1). + let hn = [[h[0][0], h[0][1], h[0][2] / f], [h[1][0], h[1][1], h[1][2] / f], [h[2][0] * f, h[2][1] * f, h[2][2]]]; + let (p1, p2): (Vec<[f64; 2]>, Vec<[f64; 2]>) = (0..x1.len()).filter(|i| fit.inliers[*i]).map(|i| (x1[i], x2[i])).unzip(); + for (r, td, _) in geometry::decompose_homography(&hn, &p1, &p2) { + if norm(td) > 1e-6 { + cands.push(Pose::from_rt(r, normalize(td))); + } + } + } + } + let thr2 = (d.inlier_thr() / f).powi(2); + cands + .into_iter() + .filter_map(|pose| { + let mut pts = HashMap::new(); + for (i, s) in sh.iter().enumerate() { + let Some(x) = geometry::triangulate(&[(Pose::IDENTITY, x1[i]), (pose, x2[i])]) else { continue }; + let ok = geometry::reproj2(&Pose::IDENTITY, x, x1[i]).is_some_and(|e| e < thr2) + && geometry::reproj2(&pose, x, x2[i]).is_some_and(|e| e < thr2) + && geometry::ray_angle([0.0; 3], pose.c, x).to_degrees() > 0.3; + if ok { + pts.insert(s.0, x); + } + } + (pts.len() >= 10 && pts.len() * 3 >= sh.len()).then_some((pose, pts)) + }) + .collect() +} + +/// Grow a reconstruction over keyframes `kfs` from an initial pair (`kfs[0]`, `kfs[j]`). +fn grow(d: &Data, kfs: &[usize], j: usize, init: &(Pose, HashMap), f: f64, cfg: &Cfg, cancel: Option<&AtomicBool>) -> Option { + let (a, b) = (kfs[0], kfs[j]); + let mut r = Recon::default(); + r.cams.insert(a, BaCam { pose: Pose::IDENTITY, f }); + r.cams.insert(b, BaCam { pose: init.0, f }); + r.pts = init.1.clone(); + let inner = |r: &mut Recon, frames: &[usize]| { + let focal = if cfg.per_frame_f && frames.len() > 2 { FocalMode::PerCamera(F_PRIOR) } else { FocalMode::Fixed }; + r.adjust(d, frames, cfg, focal, 10); + r.reject(d); + }; + inner(&mut r, &[a, b]); + let order: Vec = kfs[1..j].iter().chain(&kfs[j + 1..]).copied().collect(); + let mut done = 0; + for k in order { + if cancel.is_some_and(|c| c.load(Ordering::Relaxed)) { + return None; + } + let start = predict(&r, k)?; + if !r.register(d, k, start, cfg) { + // A failed keyframe ends the reconstruction (a later frame is too far from the known + // points); the caller resects the rest from what is known. + break; + } + r.triangulate(d, cfg.kind, 1.0, 1.0); + done += 1; + let frames: Vec = r.cams.keys().copied().collect(); + if done % 3 == 0 || frames.len() <= 6 { + inner(&mut r, &frames); + } + } + let frames: Vec = r.cams.keys().copied().collect(); + inner(&mut r, &frames); + (r.cams.len() >= 2 && r.pts.len() >= 8).then_some(r) +} + +/// Tripod reconstruction over keyframes `kfs` at focal length `f`. +fn grow_tripod(d: &Data, kfs: &[usize], f: f64, cfg: &Cfg, cancel: Option<&AtomicBool>) -> Option { + let mut r = Recon::default(); + r.cams.insert(kfs[0], BaCam { pose: Pose::IDENTITY, f }); + let ray = |p: [f64; 2], f: f64| normalize([p[0] / f, p[1] / f, 1.0]); + for w in kfs.windows(2) { + if cancel.is_some_and(|c| c.load(Ordering::Relaxed)) { + return None; + } + let (a, b) = (w[0], w[1]); + let sh = shared(d, a, b); + if sh.len() < 6 { + break; + } + let ra: Vec = sh.iter().map(|s| ray(s.1, f)).collect(); + let rb: Vec = sh.iter().map(|s| ray(s.2, f)).collect(); + let mut rel = geometry::procrustes(&ra, &rb)?; + // One round of outlier trimming. + let errs: Vec = ra.iter().zip(&rb).map(|(x, y)| norm(sub(*y, mv(&rel, *x)))).collect(); + let mut sorted = errs.clone(); + sorted.sort_by(f64::total_cmp); + let lim = (sorted[sorted.len() / 2] * 3.0).max(d.inlier_thr() / f); + let (ia, ib): (Vec, Vec) = ra.iter().zip(&rb).zip(&errs).filter(|(_, e)| **e <= lim).map(|((x, y), _)| (*x, *y)).unzip(); + if ia.len() >= 4 { + rel = geometry::procrustes(&ia, &ib)?; + } + let pa = r.cams[&a].pose.r; + r.cams.insert(b, BaCam { pose: Pose { r: mm(&rel, &pa), c: [0.0; 3] }, f }); + } + r.triangulate(d, Kind::Tripod, 0.0, 20.0); + let frames: Vec = r.cams.keys().copied().collect(); + let focal = if cfg.per_frame_f && frames.len() > 2 { FocalMode::PerCamera(F_PRIOR) } else { FocalMode::Fixed }; + r.adjust(d, &frames, cfg, focal, 15); + r.reject(d); + r.triangulate(d, Kind::Tripod, 0.0, 1.0); + r.adjust(d, &frames, cfg, focal, 15); + (r.cams.len() >= 2 && r.pts.len() >= 8).then_some(r) +} + +/// Focal length (pixels) of a horizontal angle of view. +fn focal_of(w: f64, hfov: f64) -> f64 { + w * 0.5 / (hfov.clamp(1.0, 170.0).to_radians() * 0.5).tan() +} + +struct Attempt { + recon: Recon, + err: f64, + method: SolveMethod, +} + +/// Choose the initial pair: the first keyframe and the later keyframe (among the next few) +/// that keeps the most points while having enough parallax. +fn best_init(d: &Data, kfs: &[usize], f: f64, method: SolveMethod) -> Option<(usize, (Pose, HashMap), bool)> { + let (use_e, use_h) = match method { + SolveMethod::Typical => (true, false), + SolveMethod::MostlyFlat => (false, true), + _ => (true, true), + }; + let mut best: Option<(f64, usize, (Pose, HashMap), bool)> = None; + for j in 1..kfs.len().min(6) { + let cands = init_pair(d, kfs[0], kfs[j], f, use_e, false); + let hc = if use_h { init_pair(d, kfs[0], kfs[j], f, false, true) } else { vec![] }; + for (c, is_h) in cands.into_iter().map(|c| (c, false)).chain(hc.into_iter().map(|c| (c, true))) { + // Score: points × median triangulation angle (capped). + let mut ang: Vec = c.1.values().map(|x| geometry::ray_angle([0.0; 3], c.0.c, *x).to_degrees()).collect(); + ang.sort_by(f64::total_cmp); + let med = ang.get(ang.len() / 2).copied().unwrap_or(0.0); + let score = c.1.len() as f64 * med.min(4.0); + if best.as_ref().is_none_or(|b| score > b.0) { + best = Some((score, j, c, is_h)); + } + } + } + best.map(|b| (b.1, b.2, b.3)) +} + +/// All initial candidates for the pair chosen by [`best_init`] (E and H decompositions). +fn init_candidates(d: &Data, kfs: &[usize], f: f64, method: SolveMethod) -> Vec<(usize, (Pose, HashMap), bool)> { + let Some((j, first, is_h)) = best_init(d, kfs, f, method) else { return vec![] }; + let mut out = vec![(j, first, is_h)]; + if matches!(method, SolveMethod::Auto | SolveMethod::MostlyFlat) { + for c in init_pair(d, kfs[0], kfs[j], f, false, true) { + if out.iter().all(|o| norm(log_rot(&mm(&o.1.0.r, &mt(&c.0.r)))) > 1e-3 || norm(sub(o.1.0.c, c.0.c)) > 1e-3) { + out.push((j, c, true)); + } + } + } + out +} + +/// A general (translating camera) reconstruction of keyframes `kfs` at focal length `f`. +fn general(d: &Data, kfs: &[usize], f: f64, method: SolveMethod, cfg: &Cfg, cancel: Option<&AtomicBool>) -> Option { + let cands = init_candidates(d, kfs, f, method); + let res: Vec = cands + .par_iter() + .filter_map(|(j, init, is_h)| { + let r = grow(d, kfs, *j, init, f, cfg, cancel)?; + let (_, n) = r.error(d); + // Prefer reconstructions covering more keyframes. + let cover = r.cams.len() as f64 / kfs.len() as f64; + (n > 0).then(|| Attempt { err: r.score(d) / cover.max(0.05), recon: r, method: if *is_h { SolveMethod::MostlyFlat } else { SolveMethod::Typical } }) + }) + .collect(); + res.into_iter().min_by(|a, b| a.err.total_cmp(&b.err)) +} + +fn tripod(d: &Data, kfs: &[usize], f: f64, cfg: &Cfg, cancel: Option<&AtomicBool>) -> Option { + let r = grow_tripod(d, kfs, f, cfg, cancel)?; + let cover = r.cams.len() as f64 / kfs.len() as f64; + Some(Attempt { err: r.score(d) / cover.max(0.05), recon: r, method: SolveMethod::TripodPan }) +} + +/// Search the focal length minimising `eval(f)` (a reprojection error): a log-spaced scan of +/// horizontal angles of view, then golden-section refinement around the best. +fn search_focal(w: f64, eval: &(dyn Fn(f64) -> f64 + Sync)) -> f64 { + let fovs = [8.0, 12.0, 16.0, 20.0, 25.0, 30.0, 36.0, 43.0, 51.0, 60.0, 70.0, 82.0, 96.0, 112.0]; + let fs: Vec = fovs.iter().map(|a| focal_of(w, *a)).collect(); + let errs: Vec = fs.par_iter().map(|f| eval(*f)).collect(); + if debug() { + eprintln!("focal scan: {:?}", fs.iter().zip(&errs).map(|(f, e)| format!("{f:.0}:{e:.3}")).collect::>()); + } + let i = (0..fs.len()).min_by(|a, b| errs[*a].total_cmp(&errs[*b])).unwrap_or(5); + let (mut lo, mut hi) = (fs[(i + 1).min(fs.len() - 1)].ln(), fs[i.saturating_sub(1)].ln()); + if lo > hi { + std::mem::swap(&mut lo, &mut hi); + } + let g = 0.618_033_988_75; + let (mut x1, mut x2) = (hi - g * (hi - lo), lo + g * (hi - lo)); + let (mut e1, mut e2) = rayon::join(|| eval(x1.exp()), || eval(x2.exp())); + for _ in 0..7 { + if e1 < e2 { + hi = x2; + x2 = x1; + e2 = e1; + x1 = hi - g * (hi - lo); + e1 = eval(x1.exp()); + } else { + lo = x1; + x1 = x2; + e1 = e2; + x2 = lo + g * (hi - lo); + e2 = eval(x2.exp()); + } + } + let best = if e1 < e2 { (x1, e1) } else { (x2, e2) }; + if best.1 <= errs[i] { best.0.exp() } else { fs[i] } +} + +/// Solve the camera of a tracked clip. +pub fn solve(tracks: &CameraTracks, s: &SolveSettings, cancel: Option<&AtomicBool>) -> Result { + let d = Data::new(tracks, &s.deleted); + if d.n < 2 { + return err("the clip must be at least two frames long"); + } + let kfs = keyframes(&d); + if kfs.len() < 2 { + return err("not enough features were tracked to solve the camera"); + } + let cancelled = || cancel.is_some_and(|c| c.load(Ordering::Relaxed)); + let fixed_f = (s.shot == ShotType::SpecifyAngle).then(|| focal_of(d.w, if s.hfov > 0.0 { s.hfov } else { 40.0 })); + let variable = s.shot == ShotType::VariableZoom; + let gcfg = Cfg { kind: Kind::General, focal: FocalMode::Fixed, per_frame_f: false }; + let tcfg = Cfg { kind: Kind::Tripod, ..gcfg }; + // The focal-length search runs on the first keyframes only. + let mini: Vec = kfs.iter().copied().take(10).collect(); + let try_general = matches!(s.method, SolveMethod::Auto | SolveMethod::Typical | SolveMethod::MostlyFlat); + let try_tripod = matches!(s.method, SolveMethod::Auto | SolveMethod::TripodPan); + + let mut results: Vec<(Attempt, f64)> = vec![]; + if try_general { + let f = match fixed_f { + Some(f) => f, + None => { + search_focal(d.w, &|f| general(&d, &mini, f, s.method, &Cfg { per_frame_f: variable, ..gcfg }, cancel).map(|a| a.err).unwrap_or(f64::INFINITY)) + } + }; + if cancelled() { + return err("cancelled"); + } + if let Some(a) = general(&d, &kfs, f, s.method, &Cfg { per_frame_f: variable, ..gcfg }, cancel) { + results.push((a, f)); + } + } + if try_tripod && !cancelled() { + let f = match fixed_f { + Some(f) => f, + None => search_focal(d.w, &|f| tripod(&d, &mini, f, &Cfg { per_frame_f: variable, ..tcfg }, cancel).map(|a| a.err).unwrap_or(f64::INFINITY)), + }; + if let Some(a) = tripod(&d, &kfs, f, &Cfg { per_frame_f: variable, ..tcfg }, cancel) { + results.push((a, f)); + } + } + if cancelled() { + return err("cancelled"); + } + // Auto Detect keeps the tripod model when it explains the tracks about as well. + let pick = { + let g = results.iter().position(|r| r.0.method != SolveMethod::TripodPan); + let t = results.iter().position(|r| r.0.method == SolveMethod::TripodPan); + match (g, t) { + (Some(g), Some(t)) => { + let (eg, et) = (results[g].0.err, results[t].0.err); + // A general solve whose camera hardly moves relative to the scene depth is a pan. + let rg = &results[g].0.recon; + let cs: Vec = rg.cams.values().map(|c| c.pose.c).collect(); + let base = cs.iter().flat_map(|a| cs.iter().map(move |b| norm(sub(*a, *b)))).fold(0.0, f64::max); + let c0 = rg.cams.values().next().expect("camera"); + let mut depth: Vec = rg.pts.values().map(|x| norm(sub(*x, c0.pose.c))).collect(); + depth.sort_by(f64::total_cmp); + let med = depth.get(depth.len() / 2).copied().unwrap_or(1.0); + if debug() { + eprintln!("auto: general {eg:.4} tripod {et:.4} baseline/depth {:.4}", base / med); + } + if et <= (eg * 1.25).max(eg + 0.05 * d.noise) || base / med < 0.01 { t } else { g } + } + (Some(g), None) => g, + (None, Some(t)) => t, + (None, None) => return err("the camera could not be solved (not enough parallax or features)"), + } + }; + if debug() { + for (a, f) in &results { + eprintln!("candidate {:?}: err {:.4} f {f:.1} cams {} pts {}", a.method, a.err, a.recon.cams.len(), a.recon.pts.len()); + } + } + let (att, _) = results.swap_remove(pick); + let kind = if att.method == SolveMethod::TripodPan { Kind::Tripod } else { Kind::General }; + let cfg = Cfg { kind, focal: FocalMode::Fixed, per_frame_f: variable }; + let mut r = att.recon; + let focal = match (fixed_f, variable) { + (Some(_), _) => FocalMode::Fixed, + (None, true) => FocalMode::PerCamera(F_PRIOR), + (None, false) => FocalMode::Shared, + }; + // Keyframes: adjust with the focal length free, reject, re-triangulate, adjust again. + let kf_frames: Vec = r.cams.keys().copied().collect(); + r.adjust(&d, &kf_frames, &cfg, focal, 40); + r.reject(&d); + r.triangulate(&d, kind, 1.0, 1.0); + r.adjust(&d, &kf_frames, &cfg, focal, 40); + if cancelled() { + return err("cancelled"); + } + // Every other frame: resect from the points (nearest registered camera first). + let mut todo: Vec = (0..d.n).filter(|k| !r.cams.contains_key(k)).collect(); + todo.sort_by_key(|k| r.cams.keys().map(|c| c.abs_diff(*k)).min().unwrap_or(usize::MAX)); + let base = r.clone(); + let resected: Vec<(usize, Option)> = todo + .par_iter() + .map(|k| { + let mut local = Recon { cams: base.cams.clone(), pts: base.pts.clone(), outliers: HashSet::new() }; + let ok = predict(&local, *k).is_some_and(|start| local.register(&d, *k, start, &cfg)); + (*k, if ok { local.cams.get(k).copied() } else { None }) + }) + .collect(); + for (k, c) in resected { + if let Some(c) = c { + r.cams.insert(k, c); + } + } + if cancelled() { + return err("cancelled"); + } + // Points for the tracks that only lived between keyframes, then the final adjustment over + // every frame (when that stays affordable). + r.triangulate(&d, kind, 1.0, 1.0); + let all: Vec = r.cams.keys().copied().collect(); + r.reject(&d); + if all.len() <= 160 { + r.adjust(&d, &all, &cfg, focal, 25); + r.reject(&d); + } else { + // Points only (cameras fixed) would be cheap but the keyframe solution is already + // adjusted; refine each non-keyframe once more against the final points. + let base = r.clone(); + let refined: Vec<(usize, Option)> = all + .par_iter() + .filter(|k| !kf_frames.contains(k)) + .map(|k| { + let mut local = Recon { cams: base.cams.clone(), pts: base.pts.clone(), outliers: HashSet::new() }; + let start = local.cams[k]; + let ok = local.register(&d, *k, start, &cfg); + (*k, if ok { local.cams.get(k).copied() } else { None }) + }) + .collect(); + for (k, c) in refined.into_iter() { + if let Some(c) = c { + r.cams.insert(k, c); + } + } + r.reject(&d); + } + // Method Used: the chosen model; Auto Detect calls a general solve "Mostly Flat Scene" when + // its points are close to one plane. + let method = match (s.method, att.method) { + (_, SolveMethod::TripodPan) => SolveMethod::TripodPan, + (SolveMethod::Auto, _) => { + let p: Vec = r.pts.values().copied().collect(); + let flat = cov_ratio(&p) < 0.08; + if flat { SolveMethod::MostlyFlat } else { SolveMethod::Typical } + } + (m, _) => m, + }; + Ok(finish(&d, tracks, s, r, method, kind)) +} + +/// Thickness of a point cloud: √(smallest / middle eigenvalue of its covariance). +fn cov_ratio(p: &[V3]) -> f64 { + if p.len() < 3 { + return 1.0; + } + let k = 1.0 / p.len() as f64; + let c = scale(p.iter().fold([0.0; 3], |a, b| add(a, *b)), k); + let mut cov = [0.0; 9]; + for q in p { + let d = sub(*q, c); + for i in 0..3 { + for j in 0..3 { + cov[i * 3 + j] += d[i] * d[j] * k; + } + } + } + let (vals, _) = sym_eigen(&cov, 3); + (vals[0].max(0.0) / vals[1].max(1e-300)).sqrt() +} + +/// Canonical frame, per-frame output (unsolved frames interpolated), points and errors. +fn finish(d: &Data, tracks: &CameraTracks, s: &SolveSettings, r: Recon, method: SolveMethod, kind: Kind) -> CameraSolve { + let (k0, c0) = r.cams.iter().next().map(|(k, c)| (*k, *c)).expect("solved"); + // Scale: median depth of the points seen by the first solved camera is 1. + let mut depths: Vec = d.by_frame[k0].iter().filter_map(|(ti, _)| r.pts.get(ti)).map(|x| c0.pose.to_cam(*x)[2]).filter(|z| *z > 0.0).collect(); + depths.sort_by(f64::total_cmp); + let sc = match kind { + Kind::Tripod => 1.0, + Kind::General => depths.get(depths.len() / 2).copied().filter(|v| *v > 1e-12).unwrap_or(1.0), + }; + let r0 = c0.pose.r; + let cc = c0.pose.c; + let to_world = |x: V3| scale(mv(&r0, sub(x, cc)), 1.0 / sc); + let mut frames = vec![SolvedFrame::default(); d.n]; + for (k, c) in &r.cams { + let rr = mm(&c.pose.r, &mt(&r0)); + frames[*k] = SolvedFrame { rot: log_rot(&rr), center: to_world(c.pose.c), focal: c.f, solved: true }; + } + // Hold / interpolate unsolved frames so the camera path stays continuous. + let solved: Vec = (0..d.n).filter(|k| frames[*k].solved).collect(); + for k in 0..d.n { + if frames[k].solved { + continue; + } + let a = solved.iter().rev().find(|x| **x < k).copied(); + let b = solved.iter().find(|x| **x > k).copied(); + frames[k] = match (a, b) { + (Some(a), Some(b)) => { + let t = (k - a) as f64 / (b - a) as f64; + let (fa, fb) = (frames[a], frames[b]); + let rr = slerp(&fa.r(), &fb.r(), t); + SolvedFrame { + rot: log_rot(&rr), + center: add(scale(fa.center, 1.0 - t), scale(fb.center, t)), + focal: fa.focal + (fb.focal - fa.focal) * t, + solved: false, + } + } + (Some(a), None) => SolvedFrame { solved: false, ..frames[a] }, + (None, Some(b)) => SolvedFrame { solved: false, ..frames[b] }, + (None, None) => frames[k], + }; + } + let mut points = vec![]; + let (mut esum, mut en) = (0.0, 0usize); + let mut ids: Vec<&u32> = r.pts.keys().collect(); + ids.sort(); + for ti in ids { + let x = r.pts[ti]; + let (id, start, pts) = &d.tracks[*ti as usize]; + let (mut s1, mut n1) = (0.0, 0); + for (i, uv) in pts.iter().enumerate() { + let k = start + i; + if r.outliers.contains(&(*ti, k)) { + continue; + } + if let Some(c) = r.cams.get(&k) + && let Some(res) = bundle::residual(c, x, *uv) + { + s1 += res[0].hypot(res[1]); + n1 += 1; + } + } + if n1 < 2 { + continue; + } + esum += s1; + en += n1; + points.push(SolvedPoint { id: *id, pos: to_world(x), error: (s1 / n1 as f64) as f32, first: *start as u32, last: (start + pts.len() - 1) as u32 }); + } + CameraSolve { + version: 1, + size: tracks.size, + start: tracks.start, + frame_duration: tracks.frame_duration, + shot: s.shot, + method_used: method, + average_error: if en > 0 { esum / en as f64 } else { 0.0 }, + frames, + points, + ground: None, + } +} diff --git a/crates/track/src/camtrack/tests.rs b/crates/track/src/camtrack/tests.rs new file mode 100644 index 00000000..258161f9 --- /dev/null +++ b/crates/track/src/camtrack/tests.rs @@ -0,0 +1,360 @@ +//! Synthetic scenes: random 3D points (or a plane) seen by a known moving camera with a known +//! focal length, projected directly (with sub-pixel noise) into feature tracks. + +use super::geometry::Rng; +use super::linalg::*; +use super::*; + +const W: f64 = 1280.0; +const H: f64 = 720.0; + +struct Truth { + /// Per frame: (R world→cam, centre, focal). + cams: Vec<(M3, V3, f64)>, +} + +fn noise(rng: &mut Rng) -> f64 { + // Sum of uniforms ≈ Gaussian, σ ≈ 0.1 px. + let u: f64 = (0..4).map(|_| (rng.next_u64() % 10_000) as f64 / 10_000.0 - 0.5).sum(); + u * 0.17 +} + +/// Project `pts` through the cameras into tracks (a track ends when its point leaves the frame; +/// re-entering starts a new track). +fn tracks_of(pts: &[V3], t: &Truth, seed: u64, noise_on: bool) -> CameraTracks { + let mut rng = Rng(seed); + let mut tracks: Vec = vec![]; + for x in pts { + let mut cur: Option = None; + for (k, (r, c, f)) in t.cams.iter().enumerate() { + let p = mv(r, sub(*x, *c)); + let uv = (p[2] > 0.1).then(|| [f * p[0] / p[2] + W / 2.0, f * p[1] / p[2] + H / 2.0]); + match uv.filter(|u| u[0] >= 2.0 && u[1] >= 2.0 && u[0] < W - 2.0 && u[1] < H - 2.0) { + Some(u) => { + let (nx, ny) = if noise_on { (noise(&mut rng), noise(&mut rng)) } else { (0.0, 0.0) }; + let q = [(u[0] + nx) as f32, (u[1] + ny) as f32]; + match &mut cur { + Some(tr) => tr.pts.push(q), + None => cur = Some(Track2D { id: 0, start: k as u32, pts: vec![q] }), + } + } + None => { + if let Some(tr) = cur.take() { + tracks.push(tr); + } + } + } + } + if let Some(tr) = cur.take() { + tracks.push(tr); + } + } + tracks.retain(|t| t.pts.len() >= 3); + for (i, tr) in tracks.iter_mut().enumerate() { + tr.id = i as u32; + } + CameraTracks { version: 1, size: [W, H], start: 0.0, frame_duration: 1.0 / 30.0, frames: t.cams.len() as u32, factor: 1.0, detailed: false, tracks } +} + +fn random_points(n: usize, seed: u64, center: V3, ext: V3) -> Vec { + let mut rng = Rng(seed); + (0..n) + .map(|_| { + let mut u = || (rng.next_u64() % 100_000) as f64 / 100_000.0 - 0.5; + [center[0] + u() * ext[0], center[1] + u() * ext[1], center[2] + u() * ext[2]] + }) + .collect() +} + +/// A camera dollying sideways and forward while panning to keep the scene in view. +fn moving_camera(frames: usize, f: impl Fn(usize) -> f64) -> Truth { + let cams = (0..frames) + .map(|k| { + let t = k as f64 / (frames - 1) as f64; + let c = [-3.0 + 6.0 * t, -0.6 + 0.8 * t + 0.5 * (std::f64::consts::PI * t).sin(), -1.0 + 2.0 * t - 1.5 * (t - 0.5) * (t - 0.5)]; + // Look at the scene centre (0, 0, 10). + let fwd = normalize(sub([0.3 * t, 0.2, 10.0], c)); + let right = normalize(cross([0.0, 1.0, 0.0], fwd)); + let down = cross(fwd, right); + let r = [right, down, fwd]; + // A little roll. + let roll = rodrigues([0.0, 0.0, 0.05 * (t * 6.0).sin()]); + (mm(&roll, &r), c, f(k)) + }) + .collect(); + Truth { cams } +} + +/// Align the solved centres to the true ones (similarity) and return the RMS path error +/// relative to the path length, plus the worst rotation error (degrees). +fn path_error(s: &CameraSolve, t: &Truth) -> (f64, f64) { + let solved: Vec = (0..s.frames.len()).filter(|k| s.frames[*k].solved).collect(); + let src: Vec = solved.iter().map(|k| s.frames[*k].center).collect(); + let dst: Vec = solved.iter().map(|k| t.cams[*k].1).collect(); + let sim = umeyama(&src, &dst).expect("alignment"); + let len = norm(sub(dst[0], dst[dst.len() - 1])).max(1e-9); + let rms = (src.iter().zip(&dst).map(|(a, b)| dot(sub(sim.apply(*a), *b), sub(sim.apply(*a), *b))).sum::() / src.len() as f64).sqrt(); + // Rotation: R_true ≈ R_solved · simᵀ. + let mut worst: f64 = 0.0; + for k in &solved { + let rs = mm(&s.frames[*k].r(), &mt(&sim.r)); + let e = norm(log_rot(&mm(&rs, &mt(&t.cams[*k].0)))).to_degrees(); + worst = worst.max(e); + } + (rms / len, worst) +} + +fn settings(shot: ShotType, method: SolveMethod) -> SolveSettings { + SolveSettings { shot, hfov: 0.0, method, deleted: vec![] } +} + +#[test] +fn fixed_angle_solve_recovers_path_and_focal() { + let f = 1100.0; + let t = moving_camera(40, |_| f); + let pts = random_points(400, 1, [0.0, 0.0, 10.0], [14.0, 8.0, 8.0]); + let tr = tracks_of(&pts, &t, 2, true); + let t0 = std::time::Instant::now(); + let s = solve(&tr, &settings(ShotType::FixedAngle, SolveMethod::Typical), None).expect("solve"); + eprintln!("fixed: {:?} err {:.3} px focal {:.1} in {:?}", s.method_used, s.average_error, s.focal(), t0.elapsed()); + assert!(s.frames.iter().all(|c| c.solved)); + assert!(s.average_error < 0.5, "{}", s.average_error); + assert!((s.focal() - f).abs() / f < 0.02, "focal {}", s.focal()); + let (pe, re) = path_error(&s, &t); + assert!(pe < 0.01, "path {pe}"); + assert!(re < 0.5, "rotation {re}"); + // Canonical frame: first camera at the origin. + assert!(norm(s.frames[0].center) < 1e-9 && norm(s.frames[0].rot) < 1e-9); + // Serde round trip. + let j = s.to_json(); + assert_eq!(CameraSolve::from_json(&j).unwrap().points.len(), s.points.len()); +} + +#[test] +fn auto_detect_picks_a_general_model_for_a_translating_camera() { + let f = 900.0; + let t = moving_camera(30, |_| f); + let pts = random_points(300, 3, [0.0, 0.0, 10.0], [14.0, 8.0, 8.0]); + let tr = tracks_of(&pts, &t, 4, true); + let s = solve(&tr, &settings(ShotType::FixedAngle, SolveMethod::Auto), None).expect("solve"); + assert_eq!(s.method_used, SolveMethod::Typical); + assert!(s.average_error < 0.5); + assert!((s.focal() - f).abs() / f < 0.02, "focal {}", s.focal()); +} + +#[test] +fn specified_angle_of_view_is_kept() { + let f = 1000.0; + let t = moving_camera(25, |_| f); + let pts = random_points(300, 5, [0.0, 0.0, 10.0], [14.0, 8.0, 8.0]); + let tr = tracks_of(&pts, &t, 6, true); + let hfov = 2.0 * (W * 0.5 / f).atan().to_degrees(); + let s = solve(&tr, &SolveSettings { shot: ShotType::SpecifyAngle, hfov, method: SolveMethod::Typical, deleted: vec![] }, None).unwrap(); + assert!((s.focal() - f).abs() < 1e-6); + assert!(s.average_error < 0.5); + let (pe, _) = path_error(&s, &t); + assert!(pe < 0.01, "{pe}"); +} + +#[test] +fn variable_zoom_recovers_per_frame_focal() { + let t = moving_camera(40, |k| 950.0 + 6.0 * k as f64); + let pts = random_points(450, 7, [0.0, 0.0, 10.0], [16.0, 9.0, 8.0]); + let tr = tracks_of(&pts, &t, 8, true); + let s = solve(&tr, &settings(ShotType::VariableZoom, SolveMethod::Typical), None).expect("solve"); + eprintln!("variable: err {:.3}", s.average_error); + assert!(s.average_error < 0.5); + for (k, c) in s.frames.iter().enumerate() { + let f = t.cams[k].2; + assert!((c.focal - f).abs() / f < 0.02, "frame {k}: {} vs {f}", c.focal); + } +} + +#[test] +fn tripod_pan_solve() { + let f = 1200.0; + let cams = (0..40) + .map(|k| { + let a = -0.4 + 0.8 * k as f64 / 39.0; + (mm(&rodrigues([0.0, 0.0, 0.02]), &mm(&rodrigues([0.05 * a, 0.0, 0.0]), &rodrigues([0.0, a, 0.0]))), [0.0; 3], f) + }) + .collect(); + let t = Truth { cams }; + // Points all around at varying distances (rotation only: depth is unobservable). + let pts = random_points(600, 9, [0.0, 0.0, 0.0], [60.0, 20.0, 60.0]).into_iter().filter(|p| p[2] > 3.0).collect::>(); + let tr = tracks_of(&pts, &t, 10, true); + let s = solve(&tr, &settings(ShotType::FixedAngle, SolveMethod::Auto), None).expect("solve"); + eprintln!("tripod: {:?} err {:.3} focal {:.1}", s.method_used, s.average_error, s.focal()); + assert_eq!(s.method_used, SolveMethod::TripodPan); + assert!(s.average_error < 0.5); + assert!((s.focal() - f).abs() / f < 0.02, "{}", s.focal()); + for (k, c) in s.frames.iter().enumerate() { + assert!(norm(c.center) < 1e-9); + // Relative rotation to frame 0 matches. + let rel_s = mm(&c.r(), &mt(&s.frames[0].r())); + let rel_t = mm(&t.cams[k].0, &mt(&t.cams[0].0)); + assert!(norm(log_rot(&mm(&rel_s, &mt(&rel_t)))).to_degrees() < 0.2); + } +} + +#[test] +fn mostly_flat_scene_and_ground_plane() { + let f = 1000.0; + // A textured floor y = 2 (below the camera), seen by a camera moving forward and sideways. + let cams = (0..40) + .map(|k| { + let t = k as f64 / 39.0; + let c = [-2.0 + 4.0 * t, -0.3 * (std::f64::consts::PI * t).sin(), 2.0 * t]; + let fwd = normalize([0.1 - 0.2 * t, 0.45, 1.0]); + let right = normalize(cross([0.0, 1.0, 0.0], fwd)); + let down = cross(fwd, right); + ([right, down, fwd], c, f) + }) + .collect(); + let t = Truth { cams }; + let mut rng = Rng(11); + let pts: Vec = (0..500).map(|_| [((rng.next_u64() % 2000) as f64 / 100.0) - 10.0, 2.0, 2.0 + (rng.next_u64() % 2000) as f64 / 100.0]).collect(); + let tr = tracks_of(&pts, &t, 12, true); + for method in [SolveMethod::MostlyFlat, SolveMethod::Auto] { + let s = solve(&tr, &settings(ShotType::FixedAngle, method), None).expect("solve"); + eprintln!("flat {method:?}: {:?} err {:.3} focal {:.1}", s.method_used, s.average_error, s.focal()); + assert_eq!(s.method_used, SolveMethod::MostlyFlat); + assert!(s.average_error < 0.5, "{}", s.average_error); + assert!((s.focal() - f).abs() / f < 0.02, "{}", s.focal()); + let (pe, _) = path_error(&s, &t); + assert!(pe < 0.01, "{pe}"); + } + // Ground plane from the solved (coplanar) points: every point lies on it. + let hfov = 2.0 * (W * 0.5 / f).atan().to_degrees(); + let mut s = solve(&tr, &SolveSettings { shot: ShotType::SpecifyAngle, hfov, method: SolveMethod::MostlyFlat, deleted: vec![] }, None).expect("solve"); + let ids: Vec = s.visible(0).map(|p| p.id).take(12).collect(); + let g = s.ground_from(&ids, 0).expect("ground"); + let spread = s.points.iter().map(|p| dot(sub(p.pos, g.origin), g.normal).abs()).fold(0.0, f64::max); + let depth = norm(sub(s.frames[0].center, g.origin)); + assert!(spread < 0.01 * depth, "{spread}"); + // Up points towards the camera. + assert!(dot(sub(s.frames[0].center, g.origin), g.normal) > 0.0); + s.ground = Some(g); + let w = s.world([W, H], 1000.0); + // In the world, ground points have y ≈ 0 and the cameras are above (y < 0). + for p in s.points.iter().take(20) { + assert!(w.apply(p.pos)[1].abs() < 0.01 * 1000.0 * depth, "{:?}", w.apply(p.pos)); + } + assert!(w.apply(s.frames[0].center)[1] < 0.0); + assert!(norm(w.apply(g.origin)) < 1e-6); +} + +#[test] +fn deleted_tracks_are_ignored_and_cancel_stops() { + let t = moving_camera(20, |_| 1000.0); + let pts = random_points(250, 13, [0.0, 0.0, 10.0], [14.0, 8.0, 8.0]); + let tr = tracks_of(&pts, &t, 14, true); + let deleted: Vec = (0..40).collect(); + let hfov = 2.0 * (W * 0.5 / 1000.0f64).atan().to_degrees(); + let s = solve(&tr, &SolveSettings { shot: ShotType::SpecifyAngle, hfov, method: SolveMethod::Typical, deleted: deleted.clone() }, None).unwrap(); + assert!(s.points.iter().all(|p| !deleted.contains(&p.id))); + let stop = std::sync::atomic::AtomicBool::new(true); + let r = solve(&tr, &settings(ShotType::FixedAngle, SolveMethod::Typical), Some(&stop)); + assert!(r.is_err()); +} + +/// End to end: rendered frames of a textured scene through the KLT feature tracker. +#[test] +fn rendered_scene_end_to_end() { + use effectcraft_raster::Image; + let (w, h) = (480u32, 270u32); + let f = 420.0; + // Three textured planes at different depths (a floor, a back wall and a box face). + fn tex(u: f64, v: f64) -> f32 { + fn hsh(i: i64, j: i64) -> f64 { + let mut x = (i.wrapping_mul(73856093) ^ j.wrapping_mul(19349663)) as u64; + x ^= x >> 13; + x = x.wrapping_mul(0x5bd1e995); + x ^= x >> 15; + (x % 1000) as f64 / 1000.0 + } + fn noise(x: f64, y: f64) -> f64 { + let (i, j) = (x.floor(), y.floor()); + let (tx, ty) = (x - i, y - j); + let (sx, sy) = (tx * tx * (3.0 - 2.0 * tx), ty * ty * (3.0 - 2.0 * ty)); + let (i, j) = (i as i64, j as i64); + let a = hsh(i, j) + (hsh(i + 1, j) - hsh(i, j)) * sx; + let b = hsh(i, j + 1) + (hsh(i + 1, j + 1) - hsh(i, j + 1)) * sx; + a + (b - a) * sy + } + (0.15 + 0.5 * noise(u * 4.0, v * 4.0) + 0.3 * noise(u * 11.0 + 7.0, v * 11.0 + 3.0)) as f32 + } + let frames = 24; + let t = Truth { + cams: (0..frames) + .map(|k| { + let s = k as f64 / (frames - 1) as f64; + let c = [-1.0 + 2.0 * s, -0.2, 0.5 * s]; + let fwd = normalize(sub([0.0, 0.3, 8.0], c)); + let right = normalize(cross([0.0, 1.0, 0.0], fwd)); + ([right, cross(fwd, right), fwd], c, f) + }) + .collect(), + }; + // Planes: (point, normal, u axis, v axis). + let planes: [(V3, V3, V3, V3); 3] = [ + ([0.0, 1.5, 0.0], [0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]), + ([0.0, 0.0, 10.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]), + ([-1.0, 0.0, 6.0], [0.3, 0.0, -1.0], [1.0, 0.0, 0.3], [0.0, 1.0, 0.0]), + ]; + let render = |k: usize| { + let (r, c, f) = t.cams[k]; + let mut img = Image::new(w, h); + for y in 0..h { + for x in 0..w { + let d = mtv(&r, [(x as f64 + 0.5 - w as f64 / 2.0) / f, (y as f64 + 0.5 - h as f64 / 2.0) / f, 1.0]); + let mut best = (f64::INFINITY, 0.0f32); + for (i, (p0, n, ua, va)) in planes.iter().enumerate() { + let den = dot(*n, d); + if den.abs() < 1e-9 { + continue; + } + let tt = dot(*n, sub(*p0, c)) / den; + let hit = add(c, scale(d, tt)); + // The box face is limited in extent. + if i == 2 && (dot(sub(hit, *p0), *ua).abs() > 1.2 || dot(sub(hit, *p0), *va).abs() > 1.0) { + continue; + } + if tt > 0.1 && tt < best.0 { + best = (tt, tex(dot(sub(hit, *p0), *ua) + 10.0 * i as f64, dot(sub(hit, *p0), *va))); + } + } + let v = best.1; + img.set(x, y, [v, v, v, 1.0]); + } + } + img + }; + let mut an = TrackAnalyzer::new([w as f64, h as f64], AnalyzeOpts::default()); + for k in 0..frames { + let img = render(k); + an.push(&crate::Frame::new(&img)); + } + let tr = an.finish(0.0, 1.0 / 24.0); + assert!(tr.tracks.len() > 100, "{}", tr.tracks.len()); + let s = solve(&tr, &settings(ShotType::FixedAngle, SolveMethod::Auto), None).expect("solve"); + eprintln!("rendered: {:?} err {:.3} focal {:.1} points {}", s.method_used, s.average_error, s.focal(), s.points.len()); + assert!(s.average_error < 0.5, "{}", s.average_error); + assert!((s.focal() - f).abs() / f < 0.05, "{}", s.focal()); + let (pe, _) = path_error(&s, &t); + assert!(pe < 0.03, "{pe}"); +} + +/// A long clip (300 frames, ~900 points): `cargo test -p effectcraft-track --release -- --ignored`. +#[test] +#[ignore] +fn perf_long_clip() { + let f = 1100.0; + let t = moving_camera(300, |_| f); + let pts = random_points(900, 21, [0.0, 0.0, 10.0], [16.0, 9.0, 8.0]); + let tr = tracks_of(&pts, &t, 22, true); + let t0 = std::time::Instant::now(); + let s = solve(&tr, &settings(ShotType::FixedAngle, SolveMethod::Auto), None).expect("solve"); + eprintln!("300 frames, {} tracks: {:?} err {:.3} focal {:.1} in {:?}", tr.tracks.len(), s.method_used, s.average_error, s.focal(), t0.elapsed()); + assert!(s.average_error < 0.5); +} diff --git a/crates/track/src/lib.rs b/crates/track/src/lib.rs index d7b239f5..bd4e21dc 100644 --- a/crates/track/src/lib.rs +++ b/crates/track/src/lib.rs @@ -18,6 +18,7 @@ //! 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;