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 <noreply@anthropic.com>
This commit is contained in:
Brandon Thomas
parent
e911cc5c93
commit
bab4f88c22
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<bool>,
|
||||
/// 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<usize>,
|
||||
/// Per-camera focal parameter (`usize::MAX` when none). Gauge-fixed cameras keep a free
|
||||
/// focal length.
|
||||
fidx: Vec<usize>,
|
||||
/// Index of the shared focal parameter.
|
||||
shared: Option<usize>,
|
||||
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<u32>> = 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<f64>, bc: &mut Vec<f64>| -> 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<f64>, Vec<f64>, Vec<PtSys>) = if parallel && npts > 64 {
|
||||
let chunks = rayon::current_num_threads().clamp(1, 16);
|
||||
let per = npts.div_ceil(chunks);
|
||||
let parts: Vec<(Vec<f64>, Vec<f64>, Vec<PtSys>)> = (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::<f64>() / 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<Option<M3>> = 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<V3> = 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<BaCam> = 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<V3> = 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<V3> = (0..80).map(|_| [u() * 6.0, u() * 4.0, 6.0 + u() * 4.0]).collect();
|
||||
let f = 900.0;
|
||||
let cams_true: Vec<BaCam> = (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<V3> = 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:?}");
|
||||
}
|
||||
}
|
||||
|
|
@ -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<GrayPyramid>,
|
||||
/// (track index, level-0 plane position) of the live features.
|
||||
active: Vec<(usize, [f64; 2])>,
|
||||
tracks: Vec<Track2D>,
|
||||
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<Track2D> = 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 }
|
||||
}
|
||||
}
|
||||
|
|
@ -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<usize>) {
|
||||
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::<f64>() / n, p.iter().map(|q| q[1]).sum::<f64>() / n];
|
||||
let d = p.iter().map(|q| (q[0] - c[0]).hypot(q[1] - c[1])).sum::<f64>() / 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<M3> {
|
||||
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<bool>)> {
|
||||
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<V3> {
|
||||
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<f64> = (0..4).map(|j| x[0] * rows[2][j] - rows[0][j]).collect();
|
||||
let a1: Vec<f64> = (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::<f64>().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<f64> {
|
||||
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<f64> = 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<M3> {
|
||||
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<V3>, Pose) {
|
||||
let mut rng = Rng(12345);
|
||||
let pts: Vec<V3> = (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<V3> = (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<V3> = (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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<M3> {
|
||||
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<f64>, Vec<f64>, Vec<f64>) {
|
||||
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<f64> = (0..n).map(|j| (0..rows).map(|r| u[r * n + j] * u[r * n + j]).sum::<f64>().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<usize> = (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<f64> = 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<f64> {
|
||||
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<f64>, Vec<f64>) {
|
||||
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<usize> = (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::<f64>();
|
||||
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<Similarity> {
|
||||
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<V3> = 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Track2D>,
|
||||
}
|
||||
|
||||
impl CameraTracks {
|
||||
pub fn to_json(&self) -> String {
|
||||
serde_json::to_string(self).unwrap_or_default()
|
||||
}
|
||||
pub fn from_json(s: &str) -> Option<CameraTracks> {
|
||||
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<ShotType> {
|
||||
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<SolveMethod> {
|
||||
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<u32>,
|
||||
}
|
||||
|
||||
/// 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<SolvedFrame>,
|
||||
pub points: Vec<SolvedPoint>,
|
||||
#[serde(skip_serializing_if = "Option::is_none")]
|
||||
pub ground: Option<Ground>,
|
||||
}
|
||||
|
||||
impl CameraSolve {
|
||||
pub fn to_json(&self) -> String {
|
||||
serde_json::to_string(self).unwrap_or_default()
|
||||
}
|
||||
pub fn from_json(s: &str) -> Option<CameraSolve> {
|
||||
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<usize> {
|
||||
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<Item = &SolvedPoint> {
|
||||
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<f64> = 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<Target> {
|
||||
let pts: Vec<V3> = 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<Ground> {
|
||||
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<usize> {
|
||||
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]
|
||||
}
|
||||
|
|
@ -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<T>(m: &str) -> Result<T, SolveError> {
|
||||
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<Vec<(u32, [f64; 2])>>,
|
||||
/// 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<u32> = 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<usize> {
|
||||
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<usize, BaCam>,
|
||||
pts: HashMap<u32, V3>,
|
||||
/// 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<BaCam>, Vec<V3>, Vec<BaObs>, Vec<u32>) {
|
||||
let cams: Vec<BaCam> = frames.iter().map(|k| self.cams[k]).collect();
|
||||
let mut pidx: HashMap<u32, u32> = 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<u32, usize> = 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<u32, Vec<usize>> = 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<BaObs> =
|
||||
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<BaCam> {
|
||||
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<u32, V3>)> {
|
||||
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<Pose> = 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<u32, V3>), f: f64, cfg: &Cfg, cancel: Option<&AtomicBool>) -> Option<Recon> {
|
||||
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<usize> = 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<usize> = r.cams.keys().copied().collect();
|
||||
if done % 3 == 0 || frames.len() <= 6 {
|
||||
inner(&mut r, &frames);
|
||||
}
|
||||
}
|
||||
let frames: Vec<usize> = 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<Recon> {
|
||||
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<V3> = sh.iter().map(|s| ray(s.1, f)).collect();
|
||||
let rb: Vec<V3> = sh.iter().map(|s| ray(s.2, f)).collect();
|
||||
let mut rel = geometry::procrustes(&ra, &rb)?;
|
||||
// One round of outlier trimming.
|
||||
let errs: Vec<f64> = 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<V3>, Vec<V3>) = 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<usize> = 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<u32, V3>), 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<u32, V3>), 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<f64> = 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<u32, V3>), 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<Attempt> {
|
||||
let cands = init_candidates(d, kfs, f, method);
|
||||
let res: Vec<Attempt> = 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<Attempt> {
|
||||
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<f64> = fovs.iter().map(|a| focal_of(w, *a)).collect();
|
||||
let errs: Vec<f64> = 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::<Vec<_>>());
|
||||
}
|
||||
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<CameraSolve, SolveError> {
|
||||
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<usize> = 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<V3> = 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<f64> = 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<usize> = 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<usize> = (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<BaCam>)> = 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<usize> = 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<BaCam>)> = 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<V3> = 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<f64> = 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<usize> = (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,
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Track2D> = vec![];
|
||||
for x in pts {
|
||||
let mut cur: Option<Track2D> = 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<V3> {
|
||||
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<usize> = (0..s.frames.len()).filter(|k| s.frames[*k].solved).collect();
|
||||
let src: Vec<V3> = solved.iter().map(|k| s.frames[*k].center).collect();
|
||||
let dst: Vec<V3> = 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::<f64>() / 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::<Vec<_>>();
|
||||
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<V3> = (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<u32> = 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<u32> = (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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue