GitCorner

Code

ossartcraftlightcraft🌐
5a24289898fd
//! Panorama registration: pairwise matching → camera rotations and focal lengths.
//!
//! After M. Brown & D. G. Lowe, "Automatic Panoramic Image Stitching using Invariant Features"
//! (IJCV 2007):
//! - pairwise feature matches verified with a RANSAC homography; a pair is accepted when the
//!   inliers explain enough of the matches (`nᵢ > 8 + 0.3·n_f`);
//! - the largest connected set of images is kept;
//! - focal length initialised from the homographies (Szeliski & Shum 1997 relations, derived here
//!   from the orthonormality of `K₁⁻¹ H K₀`), rotations by chaining the pairwise rotations along a
//!   maximum spanning tree (most inliers first);
//! - bundle adjustment of every camera's rotation and focal length (Levenberg–Marquardt on the
//!   symmetric reprojection error, Huber-weighted, with a numeric Jacobian per match);
//! - automatic straightening: the world "up" is the direction most perpendicular to every
//!   camera's x axis.

use lightcraft_geom::{Homography, Point};
use rayon::prelude::*;

use crate::features::{self, Features};
use crate::linalg::{self, M3};
use crate::ransac::{self, Model};

/// A pinhole camera: `u = f · (R d)ₓᵧ / (R d)_z + c` for a world direction `d`.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Camera {
    /// Focal length (pixels of the image it belongs to).
    pub f: f64,
    /// World → camera rotation.
    pub r: M3,
    pub cx: f64,
    pub cy: f64,
}

impl Camera {
    /// World direction of image pixel `(x, y)`.
    #[inline]
    pub fn ray(&self, x: f64, y: f64) -> [f64; 3] {
        let d = [(x - self.cx) / self.f, (y - self.cy) / self.f, 1.0];
        linalg::apply3(&linalg::transpose3(&self.r), d)
    }
    /// Image pixel of world direction `d` (`None` behind the camera).
    #[inline]
    pub fn project(&self, d: [f64; 3]) -> Option<(f64, f64)> {
        let p = linalg::apply3(&self.r, d);
        (p[2] > 1e-9).then(|| (self.f * p[0] / p[2] + self.cx, self.f * p[1] / p[2] + self.cy))
    }
}

/// Verified matches between two images (full-resolution pixel coordinates).
#[derive(Clone, Debug)]
pub struct PairMatch {
    pub i: usize,
    pub j: usize,
    /// Maps image `i` pixels to image `j` pixels.
    pub h: Homography,
    pub pts: Vec<(Point, Point)>,
    /// Coarsest pyramid level of the two features of each match (finer = more precise).
    pub levels: Vec<u8>,
}

/// Match every pair of images. `feats[k]` are in work coordinates scaled by `scales[k]`.
pub fn match_pairs(feats: &[Features], scales: &[f64]) -> Vec<PairMatch> {
    let n = feats.len();
    // the pairs are generated lazily: materialising all n(n−1)/2 up front is quadratic memory, including
    // when every comparison comes up empty (no usable features)
    let mut out: Vec<PairMatch> = (0..n)
        .into_par_iter()
        .flat_map_iter(|i| ((i + 1)..n).map(move |j| (i, j)))
        .filter_map(|(i, j)| {
            let m = features::match_features(&feats[i], &feats[j], 0.8);
            if m.len() < 12 {
                return None;
            }
            let a: Vec<Point> = m.iter().map(|(p, _)| Point::new(feats[i].points[*p].x as f64, feats[i].points[*p].y as f64)).collect();
            let b: Vec<Point> = m.iter().map(|(_, q)| Point::new(feats[j].points[*q].x as f64, feats[j].points[*q].y as f64)).collect();
            let fit = ransac::ransac(Model::Homography, &a, &b, 3.0, 12, (i * 1000 + j) as u64)?;
            // Brown & Lowe's probabilistic verification
            if (fit.inliers.len() as f64) <= 8.0 + 0.3 * m.len() as f64 {
                return None;
            }
            let (si, sj) = (scales[i], scales[j]);
            let pts = fit.inliers.iter().map(|&k| (Point::new(a[k].x / si, a[k].y / si), Point::new(b[k].x / sj, b[k].y / sj))).collect();
            let levels = fit.inliers.iter().map(|&k| feats[i].points[m[k].0].level.max(feats[j].points[m[k].1].level)).collect();
            // conjugate to full resolution: H_full = S_j⁻¹ · H · S_i
            let s_i = Homography([si, 0.0, 0.0, 0.0, si, 0.0, 0.0, 0.0, 1.0]);
            let s_j_inv = Homography([1.0 / sj, 0.0, 0.0, 0.0, 1.0 / sj, 0.0, 0.0, 0.0, 1.0]);
            Some(PairMatch { i, j, h: s_j_inv.mul(&fit.h).mul(&s_i), pts, levels })
        })
        .collect();
    out.sort_by_key(|p| (p.i, p.j));
    out
}

/// The largest connected component of the match graph (sorted image indices).
pub fn largest_component(n: usize, pairs: &[PairMatch]) -> Vec<usize> {
    let mut parent: Vec<usize> = (0..n).collect();
    fn find(p: &mut [usize], x: usize) -> usize {
        let mut r = x;
        while p[r] != r {
            r = p[r];
        }
        let mut c = x;
        while p[c] != r {
            let nx = p[c];
            p[c] = r;
            c = nx;
        }
        r
    }
    for m in pairs {
        let (a, b) = (find(&mut parent, m.i), find(&mut parent, m.j));
        if a != b {
            parent[a] = b;
        }
    }
    let mut groups: std::collections::BTreeMap<usize, Vec<usize>> = Default::default();
    for i in 0..n {
        let r = find(&mut parent, i);
        groups.entry(r).or_default().push(i);
    }
    groups.into_values().max_by_key(|g| g.len()).unwrap_or_default()
}

/// Focal length candidates (f₀, f₁) from a homography between centred image coordinates.
pub fn focals_from_homography(h: &Homography) -> (Option<f64>, Option<f64>) {
    let m = h.0;
    let (h00, h01, h02, h10, h11, h12, h20, h21) = (m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7]);
    let pick = |n1: f64, d1: f64, n2: f64, d2: f64| -> Option<f64> {
        let a = (d1.abs() > 1e-12).then(|| n1 / d1).filter(|v| *v > 0.0);
        let b = (d2.abs() > 1e-12).then(|| n2 / d2).filter(|v| *v > 0.0);
        let v = match (a, b) {
            (Some(a), Some(b)) => {
                if d1.abs() > d2.abs() {
                    a
                } else {
                    b
                }
            }
            (Some(a), None) => a,
            (None, Some(b)) => b,
            _ => return None,
        };
        Some(v.sqrt())
    };
    // rows of K₁⁻¹HK₀ orthogonal / equal length → f₀; columns → f₁
    let f0 = pick(-h02 * h12, h00 * h10 + h01 * h11, h12 * h12 - h02 * h02, h00 * h00 + h01 * h01 - h10 * h10 - h11 * h11);
    let f1 = pick(-(h00 * h01 + h10 * h11), h20 * h21, h01 * h01 + h11 * h11 - h00 * h00 - h10 * h10, h20 * h20 - h21 * h21);
    (f0, f1)
}

fn centred(h: &Homography, ci: (f64, f64), cj: (f64, f64)) -> Homography {
    let ti = Homography([1.0, 0.0, ci.0, 0.0, 1.0, ci.1, 0.0, 0.0, 1.0]);
    let tj_inv = Homography([1.0, 0.0, -cj.0, 0.0, 1.0, -cj.1, 0.0, 0.0, 1.0]);
    tj_inv.mul(h).mul(&ti)
}

/// Initial cameras for `ids` (a connected set): shared focal `f0`, rotations chained along a
/// maximum spanning tree rooted at the best-connected image.
pub fn initial_cameras(ids: &[usize], sizes: &[(usize, usize)], pairs: &[PairMatch], f0: f64) -> Vec<Option<Camera>> {
    let n = sizes.len();
    let mut cams: Vec<Option<Camera>> = vec![None; n];
    let deg = |k: usize| pairs.iter().filter(|p| p.i == k || p.j == k).map(|p| p.pts.len()).sum::<usize>();
    let Some(&root) = ids.iter().max_by_key(|&&k| (deg(k), usize::MAX - k)) else { return cams };
    let c = |k: usize| (sizes[k].0 as f64 / 2.0, sizes[k].1 as f64 / 2.0);
    cams[root] = Some(Camera { f: f0, r: linalg::I3, cx: c(root).0, cy: c(root).1 });
    let mut edges: Vec<&PairMatch> = pairs.iter().filter(|p| ids.contains(&p.i) && ids.contains(&p.j)).collect();
    edges.sort_by_key(|p| std::cmp::Reverse(p.pts.len()));
    loop {
        let mut grew = false;
        for e in &edges {
            let (known, new, h) = match (cams[e.i].is_some(), cams[e.j].is_some()) {
                (true, false) => (e.i, e.j, e.h),
                (false, true) => (
                    e.j,
                    e.i,
                    match e.h.inverse() {
                        Some(h) => h,
                        None => continue,
                    },
                ),
                _ => continue,
            };
            // h maps `known` → `new`; R_new R_knownᵀ ≈ K_new⁻¹ H K_known
            let hc = centred(&h, c(known), c(new));
            let k = [[f0, 0.0, 0.0], [0.0, f0, 0.0], [0.0, 0.0, 1.0]];
            let kinv = [[1.0 / f0, 0.0, 0.0], [0.0, 1.0 / f0, 0.0], [0.0, 0.0, 1.0]];
            let hm = [[hc.0[0], hc.0[1], hc.0[2]], [hc.0[3], hc.0[4], hc.0[5]], [hc.0[6], hc.0[7], hc.0[8]]];
            let rel = linalg::mul3(&linalg::mul3(&kinv, &hm), &k);
            let d = linalg::det3(&rel);
            if !d.is_finite() || d.abs() < 1e-12 {
                continue;
            }
            let s = d.signum() * d.abs().cbrt();
            let rel = rel.map(|r| r.map(|v| v / s));
            let rel = linalg::orthonormalize(&rel);
            let Some(rk) = cams[known].map(|c| c.r) else { continue };
            cams[new] = Some(Camera { f: f0, r: linalg::mul3(&rel, &rk), cx: c(new).0, cy: c(new).1 });
            grew = true;
        }
        if !grew {
            break;
        }
    }
    cams
}

/// Bundle adjustment result.
#[derive(Clone, Debug)]
pub struct Adjusted {
    pub cameras: Vec<Option<Camera>>,
    /// RMS reprojection error (pixels) before and after.
    pub rms_before: f64,
    pub rms: f64,
    pub iterations: usize,
}

struct Obs {
    i: usize,
    j: usize,
    a: Point,
    b: Point,
}

/// Residuals of one match: image-j point projected into i, and image-i point into j.
fn residual(ci: &Camera, cj: &Camera, a: Point, b: Point) -> [f64; 4] {
    let big = 1e3;
    let p = ci.project(cj.ray(b.x, b.y));
    let q = cj.project(ci.ray(a.x, a.y));
    let (r0, r1) = p.map_or((big, big), |(x, y)| (x - a.x, y - a.y));
    let (r2, r3) = q.map_or((big, big), |(x, y)| (x - b.x, y - b.y));
    [r0, r1, r2, r3]
}

fn perturb(c: &Camera, k: usize, eps: f64) -> Camera {
    let mut c = *c;
    if k < 3 {
        let mut w = [0.0; 3];
        w[k] = eps;
        c.r = linalg::mul3(&linalg::rodrigues(w), &c.r);
    } else {
        c.f += eps;
    }
    c
}

/// Levenberg–Marquardt over (rotation increment, focal) of every camera except the anchor's
/// rotation (gauge freedom). At most `max_per_pair` matches per pair are used.
pub fn bundle_adjust(cams: &[Option<Camera>], pairs: &[PairMatch], anchor: usize, max_per_pair: usize) -> Adjusted {
    let n = cams.len();
    let idx: Vec<Option<usize>> = {
        let mut k = 0;
        cams.iter()
            .map(|c| {
                c.map(|_| {
                    k += 1;
                    k - 1
                })
            })
            .collect()
    };
    let m = idx.iter().flatten().count();
    let np = 4 * m;
    let mut obs = Vec::new();
    for p in pairs {
        if cams[p.i].is_none() || cams[p.j].is_none() {
            continue;
        }
        // prefer the precisely located (fine-level) matches when there are enough of them
        let fine: Vec<usize> = (0..p.pts.len()).filter(|&k| p.levels.get(k).is_none_or(|l| *l <= 1)).collect();
        let use_idx: Vec<usize> = if fine.len() >= 16 { fine } else { (0..p.pts.len()).collect() };
        let step = (use_idx.len() / max_per_pair.max(1)).max(1);
        for &k in use_idx.iter().step_by(step) {
            let (a, b) = p.pts[k];
            obs.push(Obs { i: p.i, j: p.j, a, b });
        }
    }
    let mut cur: Vec<Option<Camera>> = cams.to_vec();
    let sigma = 2.0f64;
    let cost = |cs: &[Option<Camera>]| -> (f64, f64) {
        let (mut c, mut sq) = (0.0, 0.0);
        for o in &obs {
            // every observation joins two posed cameras (see `obs` above)
            let (Some(ci), Some(cj)) = (cs[o.i].as_ref(), cs[o.j].as_ref()) else { continue };
            let r = residual(ci, cj, o.a, o.b);
            for v in r {
                let a = v.abs();
                sq += v * v;
                c += if a <= sigma { 0.5 * v * v } else { sigma * (a - 0.5 * sigma) };
            }
        }
        (c, (sq / (obs.len().max(1) * 4) as f64).sqrt())
    };
    let (mut c0, rms_before) = cost(&cur);
    let mut lambda = 1e-3;
    let mut iterations = 0;
    let _ = n;
    for _ in 0..100 {
        iterations += 1;
        // normal equations (Huber IRLS weights)
        let (jtj, jtr) = obs
            .par_iter()
            .fold(
                || (vec![0.0f64; np * np], vec![0.0f64; np]),
                |(mut jtj, mut jtr), o| {
                    let (Some(ci), Some(cj)) = (cur[o.i].as_ref(), cur[o.j].as_ref()) else { return (jtj, jtr) };
                    let r = residual(ci, cj, o.a, o.b);
                    let mut cols: Vec<(usize, [f64; 4])> = Vec::with_capacity(8);
                    for (cam, which) in [(o.i, 0), (o.j, 1)] {
                        let Some(i) = idx[cam] else { continue };
                        let base = 4 * i;
                        for k in 0..4 {
                            if k < 3 && cam == anchor {
                                continue;
                            }
                            let c = if which == 0 { ci } else { cj };
                            let eps = if k < 3 { 1e-6 } else { 1e-4 * c.f };
                            let pc = perturb(c, k, eps);
                            let rr = if which == 0 { residual(&pc, cj, o.a, o.b) } else { residual(ci, &pc, o.a, o.b) };
                            let mut d = [0.0; 4];
                            for t in 0..4 {
                                d[t] = (rr[t] - r[t]) / eps;
                            }
                            cols.push((base + k, d));
                        }
                    }
                    for t in 0..4 {
                        let a = r[t].abs();
                        let w = if a <= sigma { 1.0 } else { sigma / a };
                        for &(p, dp) in &cols {
                            jtr[p] += w * dp[t] * r[t];
                            for &(q, dq) in &cols {
                                jtj[p * np + q] += w * dp[t] * dq[t];
                            }
                        }
                    }
                    (jtj, jtr)
                },
            )
            .reduce(
                || (vec![0.0f64; np * np], vec![0.0f64; np]),
                |(mut a, mut b), (c, d)| {
                    for (x, y) in a.iter_mut().zip(&c) {
                        *x += y;
                    }
                    for (x, y) in b.iter_mut().zip(&d) {
                        *x += y;
                    }
                    (a, b)
                },
            );
        let mut improved = false;
        for _ in 0..10 {
            let mut a = jtj.clone();
            for p in 0..np {
                let d = a[p * np + p];
                a[p * np + p] = d + lambda * d.max(1e-9) + 1e-12;
            }
            let Some(delta) = linalg::solve(a, jtr.iter().map(|v| -v).collect(), np) else {
                lambda *= 10.0;
                continue;
            };
            let mut trial = cur.clone();
            for (cam, c) in trial.iter_mut().enumerate() {
                let (Some(c), Some(b)) = (c.as_mut(), idx[cam]) else { continue };
                let b = 4 * b;
                if cam != anchor {
                    c.r = linalg::orthonormalize(&linalg::mul3(&linalg::rodrigues([delta[b], delta[b + 1], delta[b + 2]]), &c.r));
                }
                c.f = (c.f + delta[b + 3]).max(1.0);
            }
            let (c1, _) = cost(&trial);
            if c1 < c0 {
                let rel = (c0 - c1) / c0.max(1e-300);
                cur = trial;
                c0 = c1;
                lambda = (lambda * 0.3).max(1e-9);
                improved = rel > 1e-9;
                break;
            }
            lambda *= 10.0;
        }
        if !improved {
            break;
        }
    }
    let (_, rms) = cost(&cur);
    Adjusted { cameras: cur, rms_before, rms, iterations }
}

/// Rotate the world so that it is level: "down" (+y) is the direction most perpendicular to every
/// camera's x axis, and "forward" (+z) the mean viewing direction.
pub fn straighten(cams: &mut [Option<Camera>]) {
    let rs: Vec<M3> = cams.iter().flatten().map(|c| c.r).collect();
    if rs.is_empty() {
        return;
    }
    let mut cov = [0.0f64; 9];
    let (mut ysum, mut zsum, mut xsum) = ([0.0; 3], [0.0; 3], [0.0; 3]);
    for r in &rs {
        let (x, y, z) = (r[0], r[1], r[2]);
        for a in 0..3 {
            for b in 0..3 {
                cov[a * 3 + b] += x[a] * x[b];
            }
            ysum[a] += y[a];
            zsum[a] += z[a];
            xsum[a] += x[a];
        }
    }
    let (vals, vecs) = linalg::sym_eigen(&cov, 3);
    let total = vals.iter().sum::<f64>().max(1e-12);
    let mut down = [vecs[0][0], vecs[0][1], vecs[0][2]];
    if vals[1] < 1e-3 * total {
        // all x axes (nearly) parallel (a vertical sweep): use the mean camera "down" made
        // perpendicular to the common x axis
        let xa = linalg::normalize(xsum);
        let y = linalg::normalize(ysum);
        let k = linalg::dot(y, xa);
        down = linalg::normalize([y[0] - k * xa[0], y[1] - k * xa[1], y[2] - k * xa[2]]);
    }
    if linalg::dot(down, ysum) < 0.0 {
        down = down.map(|v| -v);
    }
    let z = linalg::normalize(zsum);
    let k = linalg::dot(z, down);
    let fwd = linalg::normalize([z[0] - k * down[0], z[1] - k * down[1], z[2] - k * down[2]]);
    let right = linalg::cross(down, fwd);
    let w = [right, down, fwd];
    let wt = linalg::transpose3(&w);
    for c in cams.iter_mut().flatten() {
        c.r = linalg::mul3(&c.r, &wt);
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn focal_from_a_rotation_homography() {
        let f = 900.0;
        let r = linalg::rodrigues([0.05, 0.3, 0.02]);
        let k = [[f, 0.0, 0.0], [0.0, f, 0.0], [0.0, 0.0, 1.0]];
        let kinv = [[1.0 / f, 0.0, 0.0], [0.0, 1.0 / f, 0.0], [0.0, 0.0, 1.0]];
        let h = linalg::mul3(&linalg::mul3(&k, &r), &kinv);
        let hh = Homography([h[0][0], h[0][1], h[0][2], h[1][0], h[1][1], h[1][2], h[2][0], h[2][1], h[2][2]]);
        let (a, b) = focals_from_homography(&hh);
        assert!((a.unwrap() - f).abs() < 1.0, "{a:?}");
        assert!((b.unwrap() - f).abs() < 1.0, "{b:?}");
    }
}