checkerboard_calibrate/calibrate/
homography.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Plane-to-plane homography estimation via the normalized DLT.
5//!
6//! Used to initialize camera calibration: for a planar calibration target
7//! (all object points at `z = 0`), each view's object->image mapping is a
8//! homography, from which initial intrinsics and extrinsics are recovered.
9//!
10//! This only needs to be a good *initialization* for the later
11//! Levenberg-Marquardt refinement, so it does not have to bit-match OpenCV's
12//! `cvFindHomography`; the normalized DLT (Hartley & Zisserman, Alg. 4.2) is a
13//! standard, well-conditioned least-squares estimate.
14
15use nalgebra::{DMatrix, Matrix3};
16
17/// Similarity transform that maps a point set to zero centroid and mean
18/// distance `sqrt(2)` from the origin, returned as a 3x3 matrix together with
19/// the transformed points.
20fn normalize(pts: &[(f64, f64)]) -> (Matrix3<f64>, Vec<(f64, f64)>) {
21    let n = pts.len() as f64;
22    let (mut cx, mut cy) = (0.0, 0.0);
23    for &(x, y) in pts {
24        cx += x;
25        cy += y;
26    }
27    cx /= n;
28    cy /= n;
29
30    let mut mean_dist = 0.0;
31    for &(x, y) in pts {
32        mean_dist += ((x - cx).powi(2) + (y - cy).powi(2)).sqrt();
33    }
34    mean_dist /= n;
35
36    // Degenerate (all points coincident): fall back to identity scale.
37    let scale = if mean_dist > f64::EPSILON {
38        2.0f64.sqrt() / mean_dist
39    } else {
40        1.0
41    };
42
43    let t = Matrix3::new(
44        scale,
45        0.0,
46        -scale * cx,
47        0.0,
48        scale,
49        -scale * cy,
50        0.0,
51        0.0,
52        1.0,
53    );
54
55    let out = pts
56        .iter()
57        .map(|&(x, y)| (scale * (x - cx), scale * (y - cy)))
58        .collect();
59
60    (t, out)
61}
62
63/// Estimate the homography `H` mapping `src` points to `dst` points so that
64/// `dst ~ H * src` (in homogeneous coordinates), normalized to `H[(2,2)] = 1`.
65///
66/// Returns `None` if fewer than 4 correspondences are given, the counts differ,
67/// or the linear system is degenerate.
68pub fn find_homography(src: &[(f64, f64)], dst: &[(f64, f64)]) -> Option<Matrix3<f64>> {
69    if src.len() != dst.len() || src.len() < 4 {
70        return None;
71    }
72
73    let (t_src, src_n) = normalize(src);
74    let (t_dst, dst_n) = normalize(dst);
75
76    // Build the 2n x 9 system A h = 0.
77    let n = src_n.len();
78    let mut a = DMatrix::<f64>::zeros(2 * n, 9);
79    for (i, (&(x, y), &(u, v))) in src_n.iter().zip(dst_n.iter()).enumerate() {
80        let r0 = 2 * i;
81        let r1 = r0 + 1;
82        a[(r0, 0)] = -x;
83        a[(r0, 1)] = -y;
84        a[(r0, 2)] = -1.0;
85        a[(r0, 6)] = u * x;
86        a[(r0, 7)] = u * y;
87        a[(r0, 8)] = u;
88
89        a[(r1, 3)] = -x;
90        a[(r1, 4)] = -y;
91        a[(r1, 5)] = -1.0;
92        a[(r1, 6)] = v * x;
93        a[(r1, 7)] = v * y;
94        a[(r1, 8)] = v;
95    }
96
97    // The solution is the right singular vector of the smallest singular value.
98    let svd = a.svd(false, true);
99    let vt = svd.v_t?;
100    let h = vt.row(vt.nrows() - 1).transpose();
101
102    let h_norm = Matrix3::new(h[0], h[1], h[2], h[3], h[4], h[5], h[6], h[7], h[8]);
103
104    // Denormalize: H = T_dst^{-1} * H_norm * T_src.
105    let t_dst_inv = t_dst.try_inverse()?;
106    let mut hmat = t_dst_inv * h_norm * t_src;
107
108    let scale = hmat[(2, 2)];
109    if scale.abs() < f64::EPSILON {
110        return None;
111    }
112    hmat /= scale;
113    Some(hmat)
114}
115
116#[cfg(test)]
117mod tests {
118    use super::*;
119
120    fn apply(h: &Matrix3<f64>, p: (f64, f64)) -> (f64, f64) {
121        let v = h * nalgebra::Vector3::new(p.0, p.1, 1.0);
122        (v[0] / v[2], v[1] / v[2])
123    }
124
125    #[test]
126    fn recovers_known_homography() {
127        // A non-trivial homography (rotation + perspective).
128        let h_true = Matrix3::new(0.8, -0.2, 30.0, 0.15, 0.9, -10.0, 0.0005, -0.0003, 1.0);
129
130        let src = [
131            (0.0, 0.0),
132            (1.0, 0.0),
133            (2.0, 0.0),
134            (3.0, 1.0),
135            (0.0, 1.0),
136            (1.0, 2.0),
137            (2.0, 3.0),
138            (4.0, 4.0),
139        ];
140        let dst: Vec<(f64, f64)> = src.iter().map(|&p| apply(&h_true, p)).collect();
141
142        let h = find_homography(&src, &dst).expect("homography");
143
144        // Compare by action on points (H is only defined up to scale).
145        for &p in &src {
146            let (ex, ey) = apply(&h_true, p);
147            let (gx, gy) = apply(&h, p);
148            approx::assert_abs_diff_eq!(gx, ex, epsilon = 1e-9);
149            approx::assert_abs_diff_eq!(gy, ey, epsilon = 1e-9);
150        }
151    }
152
153    #[test]
154    fn rejects_too_few_points() {
155        let pts = [(0.0, 0.0), (1.0, 0.0), (0.0, 1.0)];
156        assert!(find_homography(&pts, &pts).is_none());
157    }
158}
checkerboard_calibrate/calibrate/homography.rs

checkerboard_calibrate/calibrate/
homography.rs
