Files
effectcraft/crates/track/src/camtrack/solver.rs
T
Brandon ThomasandClaude Opus 5.5 cba8d7e737 M12.6: 3D Camera Tracker effect + UI
Effect > Perspective > 3D Camera Tracker (Shot Type, Horizontal Angle of
View, Show/Render Track Points, Track Point Size, Target Size, Advanced:
Solve Method, Method Used, Average Error, Detailed Analysis, Auto-delete
Points Across Time, Hide Warning Banner); tracks and solve stored in the
effect with keys (frame edits re-track, setting / deleted-point edits only
re-solve). Background job (Analyzing in background (step 1 of 2) / Solving
camera banner, cancel, one undo step). Commands: track.camera (replaces the
stub), camera.analyze/cancel/solveStatus/points/selectPoints/
createFromSolve/create/setGroundPlane/deletePoints; the one-node 3D Tracker
Camera keyed per frame and text / solids / nulls / shadow catcher + light on
the target plane, Create Multiple. Viewer: depth-sized coloured targets,
hover target plane, click / shift / marquee selection, AE right-click menu,
Delete; Effect Controls Analyze / Cancel / Create Camera; Tracker panel
Track Camera. Solver: adaptive noise thresholds, denser keyframes, quick
tripod check, faster Cholesky and parallel Schur complement.

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

954 lines
39 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//! 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): starts from the analysis resolution and is re-estimated
/// from the residuals once the keyframes are solved.
noise_bits: std::sync::atomic::AtomicU64,
}
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));
}
let noise = (0.5 * t.factor).max(0.5);
Data { n, w: t.size[0], tracks, by_frame, noise_bits: std::sync::atomic::AtomicU64::new(noise.to_bits()) }
}
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 noise(&self) -> f64 {
f64::from_bits(self.noise_bits.load(Ordering::Relaxed))
}
fn set_noise(&self, v: f64) {
self.noise_bits.store(v.to_bits(), Ordering::Relaxed);
}
fn inlier_thr(&self) -> f64 {
4.0 * self.noise()
}
fn huber(&self) -> f64 {
self.noise()
}
}
/// Parallax-based keyframes: a new keyframe when the median feature motion since the last
/// exceeds 1.5 % of the width, when fewer than 70 % of its features survive, after 12 frames, 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.015 * d.w || ratio < 0.7 || disp.len() < 24 || j - last >= 12 {
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 t0 = web_time::Instant::now();
let lap = |what: &str| {
if debug() {
eprintln!("{what}: {:.2} s", t0.elapsed().as_secs_f64());
}
};
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));
}
}
lap("general");
// Auto Detect: a quick tripod check at the general solve's focal length; when a rotation
// clearly cannot explain the tracks, the tripod search is skipped.
let tripod_plausible = match (s.method, results.first()) {
(SolveMethod::Auto, Some((g, f))) => {
let quick = tripod(&d, &mini, *f, &tcfg, cancel).map(|a| a.err).unwrap_or(f64::INFINITY);
if debug() {
eprintln!("quick tripod check: {quick:.3} vs general {:.3}", g.err);
}
quick <= 3.0 * g.err + d.noise()
}
_ => true,
};
if try_tripod && tripod_plausible && !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,
};
lap("tripod");
// 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);
// Re-estimate the tracking noise from the inlier residuals (robustly, from their median:
// for 2D Gaussian noise of deviation σ the median distance is 1.1774 σ), then tighten the
// thresholds: observations of moving objects and occlusion edges become outliers.
let mut res: Vec<f64> = vec![];
for (k, c) in &r.cams {
for (ti, uv) in &d.by_frame[*k] {
if r.outliers.contains(&(*ti, *k)) {
continue;
}
if let Some(e) = r.pts.get(ti).and_then(|x| bundle::residual(c, *x, *uv)) {
res.push(e[0].hypot(e[1]));
}
}
}
if res.len() > 20 {
res.sort_by(f64::total_cmp);
let sigma = res[res.len() / 2] / 1.1774;
let floor = 0.05 * tracks.factor.max(1.0);
d.set_noise((1.5 * sigma).clamp(floor, d.noise()));
if debug() {
eprintln!("noise: σ {sigma:.3} → {:.3}", d.noise());
}
r.outliers.clear();
r.reject(&d);
}
r.triangulate(&d, kind, 1.0, 1.0);
r.adjust(&d, &kf_frames, &cfg, focal, 40);
r.reject(&d);
r.adjust(&d, &kf_frames, &cfg, focal, 20);
if cancelled() {
return err("cancelled");
}
lap("keyframe adjustment");
// 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");
}
lap("resection");
// 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,
};
lap("final adjustment");
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,
}
}