checkerboard_calibrate/calibrate/
solver.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Joint Levenberg-Marquardt refinement and the top-level `calibrate_camera`.
5//!
6//! Pipeline (planar calibration target, Zhang's method):
7//!   1. per-view homography ([`super::find_homography`]),
8//!   2. initial intrinsics ([`super::init_intrinsics`]),
9//!   3. initial per-view extrinsics ([`super::init_extrinsics`]),
10//!   4. joint LM refinement of `(fx, fy, cx, cy, k1, k2, p1, p2)` plus each
11//!      view's `(rvec, tvec)`, minimizing reprojection error.
12//!
13//! The distortion model matches OpenCV's `(k1, k2, p1, p2, k3)` with `k3` (and
14//! the rational terms k4..k6) fixed at zero, exactly as OpenCV's
15//! `CALIB_FIX_K3..K6` flags request. Because the refined optimum of
16//! the reprojection cost is what we compare against OpenCV, the initialization
17//! only has to be good enough to converge there.
18
19use nalgebra::{DMatrix, DVector, Dyn, Matrix3, Owned, Rotation3, Vector3};
20
21use levenberg_marquardt_sparse::{LeastSquaresProblem, LevenbergMarquardt, SparseJacobian};
22
23use super::{find_homography, init_extrinsics, init_intrinsics};
24
25/// One object<->image correspondence. The object point must be planar
26/// (`z == 0`) for the homography-based initialization.
27#[derive(Clone, Copy, Debug)]
28pub struct CorrespondingPoint {
29    pub object_point: (f64, f64, f64),
30    pub image_point: (f64, f64),
31}
32
33/// Result of [`calibrate_camera`].
34#[derive(Clone, Debug)]
35pub struct CalibrationResult {
36    /// Overall RMS reprojection error in pixels (matches OpenCV's return value).
37    pub rms_reprojection_error: f64,
38    /// Camera matrix `[fx 0 cx; 0 fy cy; 0 0 1]`, row-major.
39    pub camera_matrix: [f64; 9],
40    /// Distortion `(k1, k2, p1, p2, k3)`; `k3` is fixed at 0.
41    pub distortion_coeffs: [f64; 5],
42    /// Per-view Rodrigues rotation vectors.
43    pub rvecs: Vec<[f64; 3]>,
44    /// Per-view translation vectors.
45    pub tvecs: Vec<[f64; 3]>,
46    pub image_width: u32,
47    pub image_height: u32,
48}
49
50#[derive(Debug, PartialEq, Eq)]
51pub enum CalibrateError {
52    TooFewViews,
53    HomographyFailed,
54    IntrinsicsFailed,
55    ExtrinsicsFailed,
56}
57
58impl std::fmt::Display for CalibrateError {
59    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
60        write!(f, "{self:?}")
61    }
62}
63
64impl std::error::Error for CalibrateError {}
65
66/// Number of shared (intrinsic + distortion) parameters: fx, fy, cx, cy, k1,
67/// k2, p1, p2.
68const NUM_SHARED: usize = 8;
69/// Parameters per view: rvec(3) + tvec(3).
70const NUM_PER_VIEW: usize = 6;
71
72/// Shared intrinsic + distortion parameters: `[fx, fy, cx, cy, k1, k2, p1, p2]`.
73type SharedParams = [f64; NUM_SHARED];
74
75/// Project one object point. Mirrors OpenCV's radial+tangential model (k3 = 0).
76fn project(
77    s: &SharedParams,
78    rot: &Rotation3<f64>,
79    t: &Vector3<f64>,
80    obj: (f64, f64, f64),
81) -> (f64, f64) {
82    let [fx, fy, cx, cy, k1, k2, p1, p2] = *s;
83    let xc = rot * Vector3::new(obj.0, obj.1, obj.2) + t;
84    let xp = xc.x / xc.z;
85    let yp = xc.y / xc.z;
86    let r2 = xp * xp + yp * yp;
87    let radial = 1.0 + (k1 + k2 * r2) * r2;
88    let xpp = xp * radial + 2.0 * p1 * xp * yp + p2 * (r2 + 2.0 * xp * xp);
89    let ypp = yp * radial + p1 * (r2 + 2.0 * yp * yp) + 2.0 * p2 * xp * yp;
90    (fx * xpp + cx, fy * ypp + cy)
91}
92
93struct CalibProblem {
94    /// Per view: object points and observed image points.
95    views: Vec<Vec<CorrespondingPoint>>,
96    num_points: usize,
97    params: DVector<f64>,
98}
99
100impl CalibProblem {
101    fn rot_t(p: &DVector<f64>, view: usize) -> (Rotation3<f64>, Vector3<f64>) {
102        let base = NUM_SHARED + view * NUM_PER_VIEW;
103        let rvec = Vector3::new(p[base], p[base + 1], p[base + 2]);
104        let tvec = Vector3::new(p[base + 3], p[base + 4], p[base + 5]);
105        (Rotation3::new(rvec), tvec)
106    }
107
108    /// Stacked residuals (observed - projected), x then y per point, at `p`.
109    fn residuals_at(&self, p: &DVector<f64>) -> DVector<f64> {
110        let s: SharedParams = [p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7]];
111
112        let mut out = DVector::zeros(2 * self.num_points);
113        let mut i = 0;
114        for (vi, view) in self.views.iter().enumerate() {
115            let (rot, t) = Self::rot_t(p, vi);
116            for cp in view {
117                let (u, v) = project(&s, &rot, &t, cp.object_point);
118                out[i] = cp.image_point.0 - u;
119                out[i + 1] = cp.image_point.1 - v;
120                i += 2;
121            }
122        }
123        out
124    }
125}
126
127impl LeastSquaresProblem<f64, Dyn, Dyn> for CalibProblem {
128    type ParameterStorage = Owned<f64, Dyn>;
129    type ResidualStorage = Owned<f64, Dyn>;
130
131    fn set_params(&mut self, x: &DVector<f64>) {
132        self.params = x.clone();
133    }
134
135    fn params(&self) -> DVector<f64> {
136        self.params.clone()
137    }
138
139    fn residuals(&self) -> Option<DVector<f64>> {
140        Some(self.residuals_at(&self.params))
141    }
142
143    fn jacobian(&self) -> Option<SparseJacobian<f64>> {
144        // Central-difference Jacobian of the residuals w.r.t. each parameter.
145        let n = self.params.len();
146        let m = 2 * self.num_points;
147        let mut j = DMatrix::<f64>::zeros(m, n);
148
149        let mut p = self.params.clone();
150        for col in 0..n {
151            let x0 = p[col];
152            // Step scaled to the parameter magnitude for good conditioning.
153            let h = 1e-6 * x0.abs().max(1e-3);
154            p[col] = x0 + h;
155            let rp = self.residuals_at(&p);
156            p[col] = x0 - h;
157            let rm = self.residuals_at(&p);
158            p[col] = x0;
159
160            let inv = 0.5 / h;
161            for row in 0..m {
162                j[(row, col)] = (rp[row] - rm[row]) * inv;
163            }
164        }
165        Some(SparseJacobian::from_dense(j))
166    }
167}
168
169/// Calibrate a pinhole+distortion camera from planar-target correspondences.
170pub fn calibrate_camera(
171    views: &[Vec<CorrespondingPoint>],
172    width: u32,
173    height: u32,
174) -> Result<CalibrationResult, CalibrateError> {
175    if views.len() < 3 {
176        return Err(CalibrateError::TooFewViews);
177    }
178
179    // 1. Homographies.
180    let mut homographies = Vec::with_capacity(views.len());
181    for view in views {
182        let src: Vec<(f64, f64)> = view
183            .iter()
184            .map(|c| (c.object_point.0, c.object_point.1))
185            .collect();
186        let dst: Vec<(f64, f64)> = view.iter().map(|c| c.image_point).collect();
187        let h = find_homography(&src, &dst).ok_or(CalibrateError::HomographyFailed)?;
188        homographies.push(h);
189    }
190
191    // 2. Initial intrinsics.
192    let intr =
193        init_intrinsics(&homographies, width, height).ok_or(CalibrateError::IntrinsicsFailed)?;
194    let k = Matrix3::new(intr.fx, 0.0, intr.cx, 0.0, intr.fy, intr.cy, 0.0, 0.0, 1.0);
195
196    // 3. Initial extrinsics + parameter vector.
197    let n = views.len();
198    let mut params = DVector::<f64>::zeros(NUM_SHARED + n * NUM_PER_VIEW);
199    params[0] = intr.fx;
200    params[1] = intr.fy;
201    params[2] = intr.cx;
202    params[3] = intr.cy;
203    // distortion starts at zero (indices 4..8).
204
205    for (vi, h) in homographies.iter().enumerate() {
206        let ext = init_extrinsics(&k, h).ok_or(CalibrateError::ExtrinsicsFailed)?;
207        let rvec = ext.rotation.scaled_axis();
208        let base = NUM_SHARED + vi * NUM_PER_VIEW;
209        params[base] = rvec[0];
210        params[base + 1] = rvec[1];
211        params[base + 2] = rvec[2];
212        params[base + 3] = ext.translation[0];
213        params[base + 4] = ext.translation[1];
214        params[base + 5] = ext.translation[2];
215    }
216
217    // 4. Joint LM refinement.
218    let num_points: usize = views.iter().map(|v| v.len()).sum();
219    let problem = CalibProblem {
220        views: views.to_vec(),
221        num_points,
222        params,
223    };
224    let (problem, _report) = LevenbergMarquardt::new().minimize(problem);
225
226    let p = problem.params();
227    let ssq: f64 = problem.residuals_at(&p).iter().map(|r| r * r).sum();
228    let rms = (ssq / num_points as f64).sqrt();
229
230    let mut rvecs = Vec::with_capacity(n);
231    let mut tvecs = Vec::with_capacity(n);
232    for vi in 0..n {
233        let base = NUM_SHARED + vi * NUM_PER_VIEW;
234        rvecs.push([p[base], p[base + 1], p[base + 2]]);
235        tvecs.push([p[base + 3], p[base + 4], p[base + 5]]);
236    }
237
238    Ok(CalibrationResult {
239        rms_reprojection_error: rms,
240        camera_matrix: [p[0], 0.0, p[2], 0.0, p[1], p[3], 0.0, 0.0, 1.0],
241        distortion_coeffs: [p[4], p[5], p[6], p[7], 0.0],
242        rvecs,
243        tvecs,
244        image_width: width,
245        image_height: height,
246    })
247}
248
249#[cfg(test)]
250mod tests {
251    use super::*;
252
253    /// Generate a synthetic planar-target dataset with known intrinsics,
254    /// distortion, and per-view poses, then check calibration recovers them.
255    #[test]
256    fn recovers_synthetic_calibration() {
257        let (w, h) = (640u32, 480u32);
258        let (fx, fy, cx, cy) = (525.0, 530.0, 320.0, 240.0);
259        let (k1, k2, p1, p2) = (-0.25, 0.08, 0.001, -0.0015);
260
261        // A 9x6 planar grid, unit spacing, centered at origin.
262        let mut obj = Vec::new();
263        for r in 0..6 {
264            for c in 0..9 {
265                obj.push((c as f64 - 4.0, r as f64 - 2.5, 0.0));
266            }
267        }
268
269        let poses = [
270            (
271                Vector3::new(0.05, -0.1, 0.02),
272                Vector3::new(-1.0, -0.5, 12.0),
273            ),
274            (
275                Vector3::new(-0.2, 0.15, -0.05),
276                Vector3::new(0.8, -1.0, 11.0),
277            ),
278            (Vector3::new(0.1, 0.25, 0.1), Vector3::new(1.0, 0.5, 13.0)),
279            (
280                Vector3::new(-0.15, -0.2, 0.07),
281                Vector3::new(-0.7, 1.0, 10.5),
282            ),
283            (
284                Vector3::new(0.22, 0.05, -0.12),
285                Vector3::new(0.2, 0.3, 12.5),
286            ),
287        ];
288
289        let views: Vec<Vec<CorrespondingPoint>> = poses
290            .iter()
291            .map(|(rvec, t)| {
292                let rot = Rotation3::new(*rvec);
293                let s = [fx, fy, cx, cy, k1, k2, p1, p2];
294                obj.iter()
295                    .map(|&o| {
296                        let (u, v) = project(&s, &rot, t, o);
297                        CorrespondingPoint {
298                            object_point: o,
299                            image_point: (u, v),
300                        }
301                    })
302                    .collect()
303            })
304            .collect();
305
306        let res = calibrate_camera(&views, w, h).expect("calibration");
307
308        approx::assert_abs_diff_eq!(res.camera_matrix[0], fx, epsilon = 0.5);
309        approx::assert_abs_diff_eq!(res.camera_matrix[4], fy, epsilon = 0.5);
310        approx::assert_abs_diff_eq!(res.camera_matrix[2], cx, epsilon = 0.5);
311        approx::assert_abs_diff_eq!(res.camera_matrix[5], cy, epsilon = 0.5);
312        approx::assert_abs_diff_eq!(res.distortion_coeffs[0], k1, epsilon = 1e-3);
313        approx::assert_abs_diff_eq!(res.distortion_coeffs[1], k2, epsilon = 1e-3);
314        // Noise-free data: reprojection error should be ~0.
315        assert!(
316            res.rms_reprojection_error < 1e-3,
317            "rms {}",
318            res.rms_reprojection_error
319        );
320    }
321}
checkerboard_calibrate/calibrate/solver.rs

checkerboard_calibrate/calibrate/
solver.rs
