Skip to content

EffectCraft: An open-source motion graphics and compositing tool written by scratch in pure Rust

Keep camera Jacobians on the stack during bundle adjustment

vangel7684f8fd8f6390
diff --git a/crates/track/src/camtrack/bundle.rs b/crates/track/src/camtrack/bundle.rs
index 077f04b71..c3045c51f 100644
--- a/crates/track/src/camtrack/bundle.rs
+++ b/crates/track/src/camtrack/bundle.rs
@@ -147,8 +147,52 @@ fn layout(ncam: usize, o: &BaOpts) -> Layout {
Layout { start, fidx, shared, dist, n, rot_only: o.fix_centers }
}
+// Six pose parameters, one shared OR per-camera focal parameter, two distortion parameters.
+const MAX_CAMERA_JACOBIAN_ENTRIES: usize = 6 + 1 + 2;
+type CameraJacobianEntry = (usize, [f64; 2]);
+
+/// Keeps the current parameter set on the stack. Extra parameters spill rather than panic
+/// if a future extension grows beyond the inline capacity.
+struct CameraJacobian {
+ entries: [CameraJacobianEntry; MAX_CAMERA_JACOBIAN_ENTRIES],
+ len: usize,
+ overflow: Vec<CameraJacobianEntry>,
+}
+
+impl CameraJacobian {
+ fn new() -> Self {
+ Self { entries: [(0, [0.0; 2]); MAX_CAMERA_JACOBIAN_ENTRIES], len: 0, overflow: Vec::new() }
+ }
+
+ fn clear(&mut self) {
+ self.len = 0;
+ self.overflow.clear();
+ }
+
+ fn push(&mut self, entry: CameraJacobianEntry) {
+ if self.overflow.is_empty() {
+ if let Some(slot) = self.entries.get_mut(self.len) {
+ *slot = entry;
+ self.len += 1;
+ return;
+ }
+ self.overflow.extend_from_slice(&self.entries);
+ }
+ self.overflow.push(entry);
+ }
+
+ fn as_slice(&self) -> &[CameraJacobianEntry] {
+ if self.overflow.is_empty() {
+ // len only grows after a successful get_mut, so it cannot exceed capacity.
+ self.entries.get(..self.len).unwrap_or_default()
+ } else {
+ &self.overflow
+ }
+ }
+}
+
/// 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])>) {
+fn cam_jac(l: &Layout, ci: usize, c: &BaCam, xc: V3, a: &[[f64; 3]; 2], p: [f64; 2], out: &mut CameraJacobian) {
out.clear();
let s = l.start[ci];
if s != usize::MAX {
@@ -206,7 +250,7 @@ pub fn adjust(cams: &mut [BaCam], pts: &mut [V3], obs: &[BaObs], o: &BaOpts) ->
}
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);
+ let mut jc = CameraJacobian::new();
for &oi in &by_pt[j] {
let ob = &obs[oi as usize];
let ci = ob.cam as usize;
@@ -231,6 +275,7 @@ pub fn adjust(cams: &mut [BaCam], pts: &mut [V3], obs: &[BaObs], o: &BaOpts) ->
jc.push((g, [dk[0][0], dk[1][0]]));
jc.push((g + 1, [dk[0][1], dk[1][1]]));
}
+ let jc = jc.as_slice();
// Point Jacobian: A R.
let jx = if o.fix_points {
[[0.0; 3]; 2]
@@ -239,9 +284,9 @@ pub fn adjust(cams: &mut [BaCam], pts: &mut [V3], obs: &[BaObs], o: &BaOpts) ->
let r1 = mtv(&c.pose.r, a[1]);
[r0, r1]
};
- for (ia, ja) in &jc {
+ for (ia, ja) in jc.iter() {
bc[*ia] -= w * (ja[0] * r[0] + ja[1] * r[1]);
- for (ib, jb) in &jc {
+ for (ib, jb) in jc.iter() {
s_acc[ia * n + ib] += w * (ja[0] * jb[0] + ja[1] * jb[1]);
}
}
@@ -252,7 +297,7 @@ pub fn adjust(cams: &mut [BaCam], pts: &mut [V3], obs: &[BaObs], o: &BaOpts) ->
ps.v[a1][b1] += w * (jx[0][a1] * jx[0][b1] + jx[1][a1] * jx[1][b1]);
}
}
- for (ia, ja) in &jc {
+ for (ia, ja) in jc.iter() {
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])];
// Each observation of a point is on another camera: only the shared focal
@@ -464,6 +509,26 @@ mod tests {
use super::*;
use crate::camtrack::geometry::Rng;
+ #[test]
+ fn camera_jacobian_keeps_extra_parameters_and_resets() {
+ let mut jac = CameraJacobian::new();
+ let count = MAX_CAMERA_JACOBIAN_ENTRIES + 3;
+ for i in 0..count {
+ jac.push((i, [i as f64, -(i as f64)]));
+ }
+ assert_eq!(jac.as_slice().iter().count(), count);
+ for (i, entry) in jac.as_slice().iter().enumerate() {
+ assert_eq!(*entry, (i, [i as f64, -(i as f64)]));
+ }
+ jac.clear();
+ assert_eq!(jac.as_slice().iter().count(), 0);
+ for i in 0..MAX_CAMERA_JACOBIAN_ENTRIES {
+ jac.push((i, [1.0, 2.0]));
+ }
+ assert_eq!(jac.as_slice().iter().count(), MAX_CAMERA_JACOBIAN_ENTRIES);
+ assert!(jac.overflow.is_empty());
+ }
+
#[test]
fn bundle_adjustment_converges_from_noise() {
let mut rng = Rng(99);