checkerboard_calibrate/
corner_subpix.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Pure-Rust port of OpenCV's `cv::cornerSubPix`.
5//!
6//! Given approximate corner locations and a grayscale image, this iteratively
7//! refines each corner to sub-pixel accuracy by exploiting the fact that, at a
8//! true corner, the image gradient at every nearby pixel is orthogonal to the
9//! vector from the corner to that pixel. Each iteration solves the resulting
10//! 2x2 weighted least-squares system for the corner position.
11//!
12//! The algorithm mirrors OpenCV's `modules/imgproc/src/cornersubpix.cpp` so the
13//! results match within a small tolerance. Notable fidelity points:
14//!
15//! - The window weight is the separable Gaussian
16//!   `exp(-x^2) * exp(-y^2)` with `x = (col - win_w)/win_w`,
17//!   `y = (row - win_h)/win_h`, matching OpenCV exactly.
18//! - Image values are sampled with bilinear interpolation and
19//!   `BORDER_REPLICATE`, matching `getRectSubPix`.
20//! - Iteration stops on either the max-count or the EPS criterion (squared
21//!   movement), and a result that drifts more than one half-window from the
22//!   start is reverted to the start, as OpenCV does.
23
24/// A borrowed 8-bit grayscale image (row-major, one byte per pixel).
25#[derive(Clone, Copy)]
26pub struct GrayImageRef<'a> {
27    pub data: &'a [u8],
28    pub width: usize,
29    pub height: usize,
30}
31
32impl<'a> GrayImageRef<'a> {
33    pub fn new(data: &'a [u8], width: usize, height: usize) -> Self {
34        assert_eq!(
35            data.len(),
36            width * height,
37            "data length must be width*height"
38        );
39        Self {
40            data,
41            width,
42            height,
43        }
44    }
45
46    /// Bilinear sample with `BORDER_REPLICATE`, matching OpenCV `getRectSubPix`.
47    fn sample(&self, x: f64, y: f64) -> f64 {
48        let ix = x.floor();
49        let iy = y.floor();
50        let fx = x - ix;
51        let fy = y - iy;
52
53        let x0 = self.clamp_x(ix as i64);
54        let x1 = self.clamp_x(ix as i64 + 1);
55        let y0 = self.clamp_y(iy as i64);
56        let y1 = self.clamp_y(iy as i64 + 1);
57
58        let p00 = self.data[y0 * self.width + x0] as f64;
59        let p01 = self.data[y0 * self.width + x1] as f64;
60        let p10 = self.data[y1 * self.width + x0] as f64;
61        let p11 = self.data[y1 * self.width + x1] as f64;
62
63        let top = p00 * (1.0 - fx) + p01 * fx;
64        let bot = p10 * (1.0 - fx) + p11 * fx;
65        top * (1.0 - fy) + bot * fy
66    }
67
68    fn clamp_x(&self, v: i64) -> usize {
69        v.clamp(0, self.width as i64 - 1) as usize
70    }
71
72    fn clamp_y(&self, v: i64) -> usize {
73        v.clamp(0, self.height as i64 - 1) as usize
74    }
75}
76
77/// Parameters for [`corner_subpix`], mirroring OpenCV's arguments.
78#[derive(Clone, Copy, Debug)]
79pub struct CornerSubPixParams {
80    /// Half-size of the search window: full window is `2*win_half + 1` per axis.
81    pub win_half: (usize, usize),
82    /// Half-size of a central "dead zone" excluded from the sums (used to avoid
83    /// a singular autocorrelation matrix at the exact center). `None` disables
84    /// it (OpenCV's `Size(-1, -1)`).
85    pub zero_zone_half: Option<(usize, usize)>,
86    /// Maximum number of refinement iterations per corner.
87    pub max_count: usize,
88    /// Convergence threshold on the corner movement, in pixels.
89    pub eps: f64,
90}
91
92impl Default for CornerSubPixParams {
93    /// The settings strand-braid uses for chessboard refinement:
94    /// `win = (11, 11)`, no zero-zone, `maxCount = 30`, `eps = 0.1`.
95    fn default() -> Self {
96        Self {
97            win_half: (11, 11),
98            zero_zone_half: None,
99            max_count: 30,
100            eps: 0.1,
101        }
102    }
103}
104
105/// Build the separable Gaussian window mask, identical to OpenCV's.
106fn build_mask(win_half: (usize, usize), zero_zone_half: Option<(usize, usize)>) -> Vec<f64> {
107    let (whw, whh) = win_half;
108    let win_w = whw * 2 + 1;
109    let win_h = whh * 2 + 1;
110    let mut mask = vec![0.0f64; win_w * win_h];
111
112    for i in 0..win_h {
113        let y = (i as f64 - whh as f64) / whh as f64;
114        let vy = (-y * y).exp();
115        for j in 0..win_w {
116            let x = (j as f64 - whw as f64) / whw as f64;
117            mask[i * win_w + j] = vy * (-x * x).exp();
118        }
119    }
120
121    if let Some((zw, zh)) = zero_zone_half {
122        for i in whh.saturating_sub(zh)..=(whh + zh).min(win_h - 1) {
123            for j in whw.saturating_sub(zw)..=(whw + zw).min(win_w - 1) {
124                mask[i * win_w + j] = 0.0;
125            }
126        }
127    }
128
129    mask
130}
131
132/// Refine `corners` to sub-pixel accuracy. Returns refined copies in the same
133/// order; corners are independent of one another.
134pub fn corner_subpix(
135    img: GrayImageRef,
136    corners: &[(f32, f32)],
137    params: &CornerSubPixParams,
138) -> Vec<(f32, f32)> {
139    let (whw, whh) = params.win_half;
140    let win_w = whw * 2 + 1;
141    let win_h = whh * 2 + 1;
142    let mask = build_mask(params.win_half, params.zero_zone_half);
143
144    let eps2 = params.eps * params.eps;
145    let det_thresh = f64::EPSILON * f64::EPSILON;
146
147    corners
148        .iter()
149        .map(|&(ct_x, ct_y)| {
150            let (ct_x, ct_y) = (ct_x as f64, ct_y as f64);
151            let mut c_x = ct_x;
152            let mut c_y = ct_y;
153
154            let mut iter = 0;
155            loop {
156                let (mut a, mut b, mut c) = (0.0, 0.0, 0.0);
157                let (mut bb1, mut bb2) = (0.0, 0.0);
158
159                for i in 0..win_h {
160                    let py = i as f64 - whh as f64;
161                    for j in 0..win_w {
162                        let px = j as f64 - whw as f64;
163                        let m = mask[i * win_w + j];
164                        if m == 0.0 {
165                            continue;
166                        }
167                        // Central differences on the bilinearly-sampled image,
168                        // at sub-pixel offset (px, py) from the current corner.
169                        let tgx = img.sample(c_x + px + 1.0, c_y + py)
170                            - img.sample(c_x + px - 1.0, c_y + py);
171                        let tgy = img.sample(c_x + px, c_y + py + 1.0)
172                            - img.sample(c_x + px, c_y + py - 1.0);
173
174                        let gxx = tgx * tgx * m;
175                        let gxy = tgx * tgy * m;
176                        let gyy = tgy * tgy * m;
177
178                        a += gxx;
179                        b += gxy;
180                        c += gyy;
181                        bb1 += gxx * px + gxy * py;
182                        bb2 += gxy * px + gyy * py;
183                    }
184                }
185
186                let det = a * c - b * b;
187                let (new_x, new_y) = if det.abs() > det_thresh {
188                    let scale = 1.0 / det;
189                    (
190                        c_x + (c * bb1 - b * bb2) * scale,
191                        c_y + (a * bb2 - b * bb1) * scale,
192                    )
193                } else {
194                    (c_x, c_y)
195                };
196
197                let err = (new_x - c_x) * (new_x - c_x) + (new_y - c_y) * (new_y - c_y);
198                c_x = new_x;
199                c_y = new_y;
200
201                iter += 1;
202                let out_of_bounds =
203                    c_x < 0.0 || c_x >= img.width as f64 || c_y < 0.0 || c_y >= img.height as f64;
204                if out_of_bounds || iter >= params.max_count || err <= eps2 {
205                    break;
206                }
207            }
208
209            // Poor convergence: a corner that wandered more than one half-window
210            // from its start is rejected (kept at the initial location).
211            if (c_x - ct_x).abs() > whw as f64 || (c_y - ct_y).abs() > whh as f64 {
212                (ct_x as f32, ct_y as f32)
213            } else {
214                (c_x as f32, c_y as f32)
215            }
216        })
217        .collect()
218}
219
220#[cfg(test)]
221mod tests {
222    use super::*;
223
224    /// Render a black/white checkerboard corner: the two step edges sit between
225    /// columns `bx-1,bx` and rows `by-1,by`, so by symmetry the true sub-pixel
226    /// corner (where `cornerSubPix`'s model is satisfied) is at
227    /// `(bx - 0.5, by - 0.5)`.
228    fn render_checker_corner(w: usize, h: usize, bx: usize, by: usize) -> Vec<u8> {
229        let mut data = vec![0u8; w * h];
230        for y in 0..h {
231            for x in 0..w {
232                let on = (x >= bx) == (y >= by);
233                data[y * w + x] = if on { 255 } else { 0 };
234            }
235        }
236        data
237    }
238
239    #[test]
240    fn converges_to_checker_corner() {
241        let (w, h) = (41, 41);
242        let (bx, by) = (20usize, 15usize);
243        let (cx, cy) = (bx as f64 - 0.5, by as f64 - 0.5); // true corner (19.5, 14.5)
244        let data = render_checker_corner(w, h, bx, by);
245        let img = GrayImageRef::new(&data, w, h);
246
247        let params = CornerSubPixParams {
248            win_half: (5, 5),
249            zero_zone_half: None,
250            max_count: 40,
251            eps: 1e-4,
252        };
253
254        let start = [(18.0_f32, 16.0_f32)];
255        let refined = corner_subpix(img, &start, &params);
256
257        let (rx, ry) = refined[0];
258        let start_err = (start[0].0 as f64 - cx).hypot(start[0].1 as f64 - cy);
259        let refined_err = (rx as f64 - cx).hypot(ry as f64 - cy);
260
261        assert!(
262            refined_err < start_err,
263            "refinement should improve: start {start_err:.3} -> refined {refined_err:.3}"
264        );
265        assert!(
266            refined_err < 0.2,
267            "refined corner ({rx:.3},{ry:.3}) not within 0.2px of true ({cx},{cy}); err={refined_err:.3}"
268        );
269    }
270
271    #[test]
272    fn true_corner_is_a_fixed_point() {
273        // Starting exactly at the symmetric crossing, the result must not drift.
274        let (w, h) = (41, 41);
275        let (bx, by) = (20usize, 15usize);
276        let (cx, cy) = (bx as f64 - 0.5, by as f64 - 0.5);
277        let data = render_checker_corner(w, h, bx, by);
278        let img = GrayImageRef::new(&data, w, h);
279
280        let start = [(cx as f32, cy as f32)];
281        let refined = corner_subpix(img, &start, &CornerSubPixParams::default());
282        let (rx, ry) = refined[0];
283        assert!(
284            (rx as f64 - cx).hypot(ry as f64 - cy) < 1e-3,
285            "fixed point drifted to ({rx},{ry})"
286        );
287    }
288
289    #[test]
290    fn flat_region_is_a_no_op() {
291        // No gradients => singular system => corner must not move.
292        let (w, h) = (31, 31);
293        let data = vec![100u8; w * h];
294        let img = GrayImageRef::new(&data, w, h);
295
296        let start = [(15.0_f32, 15.0_f32)];
297        let refined = corner_subpix(img, &start, &CornerSubPixParams::default());
298        assert_eq!(refined[0], (15.0, 15.0));
299    }
300
301    #[test]
302    fn mask_matches_opencv_formula() {
303        // Spot-check the separable Gaussian against a hand computation.
304        let mask = build_mask((11, 11), None);
305        let win_w = 23;
306        // center is exp(0)*exp(0) = 1
307        approx::assert_abs_diff_eq!(mask[11 * win_w + 11], 1.0, epsilon = 1e-12);
308        // corner (0,0): x=y=-1 => exp(-1)*exp(-1)
309        approx::assert_abs_diff_eq!(mask[0], (-1.0f64).exp() * (-1.0f64).exp(), epsilon = 1e-12);
310    }
311}
checkerboard_calibrate/corner_subpix.rs

checkerboard_calibrate/
corner_subpix.rs
