checkerboard_calibrate/calibrate/
intrinsics.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Initial intrinsic-matrix estimate from per-view homographies.
5//!
6//! Mirrors the strategy of OpenCV's `cvInitIntrinsicParams2D`: fix the
7//! principal point at the image center and solve a linear least-squares for the
8//! focal lengths using the rotation orthonormality constraints implied by each
9//! homography. This is only an initializer for the subsequent
10//! Levenberg-Marquardt refinement, which recovers the principal point too.
11//!
12//! For a homography `H = [h1 h2 h3]` mapping the planar target to the image,
13//! `K^{-1} H = scale * [r1 r2 t]` with `r1 ⟂ r2` and `|r1| = |r2|`. With the
14//! principal point known and zero skew, writing `a = 1/fx^2`, `c = 1/fy^2`, and
15//! for each homography column `pⱼ = h0ⱼ - cx·h2ⱼ`, `qⱼ = h1ⱼ - cy·h2ⱼ`,
16//! `rⱼ = h2ⱼ`, the two constraints become linear in `(a, c)`:
17//!
18//! ```text
19//!   a·p1p2        + c·q1q2          = -r1·r2
20//!   a·(p1²-p2²)   + c·(q1²-q2²)     = -(r1²-r2²)
21//! ```
22
23use nalgebra::{DMatrix, DVector, Matrix3};
24
25/// Initial pinhole intrinsics `(fx, fy, cx, cy)`.
26#[derive(Clone, Copy, Debug)]
27pub struct InitialIntrinsics {
28    pub fx: f64,
29    pub fy: f64,
30    pub cx: f64,
31    pub cy: f64,
32}
33
34/// Estimate initial intrinsics from per-view homographies and the image size.
35///
36/// Returns `None` if there are too few views or the focal-length solution is
37/// non-physical (non-positive squared focal length).
38pub fn init_intrinsics(
39    homographies: &[Matrix3<f64>],
40    width: u32,
41    height: u32,
42) -> Option<InitialIntrinsics> {
43    if homographies.is_empty() {
44        return None;
45    }
46
47    let cx = (width as f64 - 1.0) * 0.5;
48    let cy = (height as f64 - 1.0) * 0.5;
49
50    let mut a = DMatrix::<f64>::zeros(2 * homographies.len(), 2);
51    let mut b = DVector::<f64>::zeros(2 * homographies.len());
52
53    for (i, h) in homographies.iter().enumerate() {
54        // Column j of H, with the principal point removed.
55        let col = |j: usize| {
56            let h0 = h[(0, j)];
57            let h1 = h[(1, j)];
58            let h2 = h[(2, j)];
59            (h0 - cx * h2, h1 - cy * h2, h2)
60        };
61        let (p1, q1, r1) = col(0);
62        let (p2, q2, r2) = col(1);
63
64        let row0 = 2 * i;
65        let row1 = row0 + 1;
66
67        a[(row0, 0)] = p1 * p2;
68        a[(row0, 1)] = q1 * q2;
69        b[row0] = -r1 * r2;
70
71        a[(row1, 0)] = p1 * p1 - p2 * p2;
72        a[(row1, 1)] = q1 * q1 - q2 * q2;
73        b[row1] = -(r1 * r1 - r2 * r2);
74    }
75
76    // Least-squares solve for (a, c) = (1/fx^2, 1/fy^2) via the normal equations.
77    let ata = a.transpose() * &a;
78    let atb = a.transpose() * &b;
79    let sol = ata.lu().solve(&atb)?;
80    let (inv_fx2, inv_fy2) = (sol[0], sol[1]);
81
82    if !(inv_fx2 > 0.0 && inv_fy2 > 0.0) {
83        return None;
84    }
85
86    Some(InitialIntrinsics {
87        fx: (1.0 / inv_fx2).sqrt(),
88        fy: (1.0 / inv_fy2).sqrt(),
89        cx,
90        cy,
91    })
92}
93
94#[cfg(test)]
95mod tests {
96    use super::*;
97    use nalgebra::{Matrix3, Rotation3, Vector3};
98
99    /// Build a homography `H = K [r1 r2 t]` for a planar target.
100    fn homography_for_view(k: &Matrix3<f64>, r: &Rotation3<f64>, t: &Vector3<f64>) -> Matrix3<f64> {
101        let rm = r.matrix();
102        let mut m = Matrix3::zeros();
103        m.set_column(0, &rm.column(0));
104        m.set_column(1, &rm.column(1));
105        m.set_column(2, t);
106        k * m
107    }
108
109    #[test]
110    fn recovers_focal_lengths() {
111        let (w, h) = (640u32, 480u32);
112        let cx = (w as f64 - 1.0) * 0.5;
113        let cy = (h as f64 - 1.0) * 0.5;
114        let (fx, fy) = (520.0, 510.0);
115        let k = Matrix3::new(fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0);
116
117        // A handful of distinct views looking at the target.
118        let views = [
119            (
120                Rotation3::from_euler_angles(0.1, -0.2, 0.05),
121                Vector3::new(-1.0, -0.5, 8.0),
122            ),
123            (
124                Rotation3::from_euler_angles(-0.15, 0.1, -0.1),
125                Vector3::new(0.5, -1.0, 7.0),
126            ),
127            (
128                Rotation3::from_euler_angles(0.2, 0.25, 0.0),
129                Vector3::new(1.0, 0.5, 9.0),
130            ),
131            (
132                Rotation3::from_euler_angles(-0.05, -0.3, 0.15),
133                Vector3::new(-0.5, 1.0, 6.5),
134            ),
135        ];
136
137        let homs: Vec<Matrix3<f64>> = views
138            .iter()
139            .map(|(r, t)| homography_for_view(&k, r, t))
140            .collect();
141
142        let est = init_intrinsics(&homs, w, h).expect("intrinsics");
143        approx::assert_abs_diff_eq!(est.fx, fx, epsilon = 1e-6);
144        approx::assert_abs_diff_eq!(est.fy, fy, epsilon = 1e-6);
145        approx::assert_abs_diff_eq!(est.cx, cx, epsilon = 1e-12);
146        approx::assert_abs_diff_eq!(est.cy, cy, epsilon = 1e-12);
147    }
148}
checkerboard_calibrate/calibrate/intrinsics.rs

checkerboard_calibrate/calibrate/
intrinsics.rs
