checkerboard_calibrate/chessboard/
board.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Board validation and corner extraction — stage 4 of `findChessboardCorners`.
5//!
6//! Given the lattice-assigned quad graph from stage 3, this checks that the
7//! recovered inner corners form the requested `pattern_w x pattern_h` grid and
8//! that the grid is geometrically sane (no folding), then emits the corners in
9//! row-major order. The monotonicity test is a port of OpenCV's
10//! `checkBoardMonotony`.
11//!
12//! Note: final *orientation* canonicalization — matching the exact start corner
13//! and direction OpenCV emits — is handled when the end-to-end detector is
14//! wired against the detection golden; here the corners are returned row-major
15//! in lattice order.
16
17use std::collections::HashMap;
18
19use nalgebra::Vector3;
20
21use crate::calibrate::find_homography;
22
23use super::link::LinkedQuad;
24use super::order::{QuadGrid, corner_lattice, inner_corner_lattice};
25
26/// Port of OpenCV `icvCheckBoardMonotony`.
27///
28/// `corners` are in row-major order with `w` columns and `h` rows. For each row
29/// and each column, the intermediate corners must project monotonically (and
30/// within `[0, 1]`) onto the segment between that row's/column's endpoints —
31/// i.e. the board maps to a non-self-intersecting grid.
32pub fn check_board_monotony(corners: &[(f32, f32)], w: usize, h: usize) -> bool {
33    if corners.len() != w * h || w == 0 || h == 0 {
34        return false;
35    }
36
37    for k in 0..2 {
38        let max_i = if k == 0 { h } else { w };
39        let max_j = (if k == 0 { w } else { h }) - 1;
40        for i in 0..max_i {
41            let (a, b) = if k == 0 {
42                (corners[i * w], corners[i * w + (w - 1)])
43            } else {
44                (corners[i], corners[(h - 1) * w + i])
45            };
46            let dx0 = b.0 - a.0;
47            let dy0 = b.1 - a.1;
48            if dx0.abs() + dy0.abs() < f32::EPSILON {
49                return false;
50            }
51            let denom = dx0 * dx0 + dy0 * dy0;
52            let mut prevt = 0.0f32;
53            for j in 1..max_j {
54                let c = if k == 0 {
55                    corners[i * w + j]
56                } else {
57                    corners[j * w + i]
58                };
59                let t = ((c.0 - a.0) * dx0 + (c.1 - a.1) * dy0) / denom;
60                if t < prevt || t > 1.0 {
61                    return false;
62                }
63                prevt = t;
64            }
65        }
66    }
67    true
68}
69
70/// Validate and extract a `pattern_w x pattern_h` board from a lattice-assigned
71/// connected component.
72///
73/// The reliable inner corners (referenced by >= 2 quads) must span a
74/// `pattern_w x pattern_h` lattice rectangle. Interior corners missing from
75/// that rectangle (because a neighboring square was not detected) are filled
76/// in — analogous to OpenCV's board augmentation: a corner referenced by a
77/// single quad is taken directly, otherwise its position is predicted from a
78/// homography fit to the reliable corners (lattice -> pixel). The caller's
79/// sub-pixel refinement then snaps any predicted corner to the true saddle.
80///
81/// Returns the corners row-major (lattice row then column), after a
82/// monotonicity check.
83pub fn extract_board(
84    quads: &[LinkedQuad],
85    coords: &HashMap<usize, QuadGrid>,
86    pattern_w: usize,
87    pattern_h: usize,
88) -> Option<Vec<(f32, f32)>> {
89    let inner = inner_corner_lattice(quads, coords);
90    if inner.len() < 4 {
91        return None;
92    }
93
94    let min_x = inner.iter().map(|(l, _)| l.0).min().unwrap();
95    let max_x = inner.iter().map(|(l, _)| l.0).max().unwrap();
96    let min_y = inner.iter().map(|(l, _)| l.1).min().unwrap();
97    let max_y = inner.iter().map(|(l, _)| l.1).max().unwrap();
98    let width = (max_x - min_x + 1) as usize;
99    let height = (max_y - min_y + 1) as usize;
100
101    if width != pattern_w || height != pattern_h {
102        return None;
103    }
104
105    // All detected corners (count >= 1), origin-shifted to grid coordinates.
106    let full = corner_lattice(quads, coords);
107    let detected: HashMap<(i32, i32), (f32, f32)> = full
108        .iter()
109        .map(|(l, (p, _))| ((l.0 - min_x, l.1 - min_y), *p))
110        .collect();
111
112    // Homography from grid coordinates to pixels, fit on the reliable inner
113    // corners, used only to fill holes.
114    let needs_fill = (0..height as i32)
115        .flat_map(|gy| (0..width as i32).map(move |gx| (gx, gy)))
116        .any(|cell| !detected.contains_key(&cell));
117    let homography = if needs_fill {
118        let src: Vec<(f64, f64)> = inner
119            .iter()
120            .map(|(l, _)| ((l.0 - min_x) as f64, (l.1 - min_y) as f64))
121            .collect();
122        let dst: Vec<(f64, f64)> = inner
123            .iter()
124            .map(|(_, p)| (p.0 as f64, p.1 as f64))
125            .collect();
126        Some(find_homography(&src, &dst)?)
127    } else {
128        None
129    };
130
131    let mut ordered = Vec::with_capacity(pattern_w * pattern_h);
132    for gy in 0..height as i32 {
133        for gx in 0..width as i32 {
134            if let Some(p) = detected.get(&(gx, gy)) {
135                ordered.push(*p);
136            } else {
137                // Predict the missing corner from the grid homography.
138                let h = homography.as_ref()?;
139                let v = h * Vector3::new(gx as f64, gy as f64, 1.0);
140                if v[2].abs() < f64::EPSILON {
141                    return None;
142                }
143                ordered.push(((v[0] / v[2]) as f32, (v[1] / v[2]) as f32));
144            }
145        }
146    }
147
148    if !check_board_monotony(&ordered, pattern_w, pattern_h) {
149        return None;
150    }
151    Some(ordered)
152}
153
154#[cfg(test)]
155mod tests {
156    use super::*;
157    use crate::chessboard::link::{connected_components, link_quads};
158    use crate::chessboard::order::{assign_grid, order_all_corners};
159    use crate::chessboard::quad::Quad;
160
161    /// Black squares of a `cells_x` x `cells_y` checkerboard, side `side`.
162    fn black_square_board(cells_x: i32, cells_y: i32, side: i32) -> Vec<Quad> {
163        let mut quads = Vec::new();
164        for cy in 0..cells_y {
165            for cx in 0..cells_x {
166                if (cx + cy) % 2 != 0 {
167                    continue;
168                }
169                let (x, y) = (cx * side, cy * side);
170                quads.push(Quad {
171                    corners: [(x, y), (x + side, y), (x + side, y + side), (x, y + side)],
172                });
173            }
174        }
175        quads
176    }
177
178    fn board_corners(
179        cells_x: i32,
180        cells_y: i32,
181        side: i32,
182        pw: usize,
183        ph: usize,
184    ) -> Option<Vec<(f32, f32)>> {
185        let quads = black_square_board(cells_x, cells_y, side);
186        let mut linked = link_quads(&quads);
187        order_all_corners(&mut linked);
188        let comps = connected_components(&linked);
189        let grid = assign_grid(&linked, &comps[0]);
190        extract_board(&linked, &grid, pw, ph)
191    }
192
193    #[test]
194    fn monotony_accepts_regular_grid() {
195        // 3x2 regular grid.
196        let corners = [
197            (0.0, 0.0),
198            (10.0, 0.0),
199            (20.0, 0.0),
200            (0.0, 10.0),
201            (10.0, 10.0),
202            (20.0, 10.0),
203        ];
204        assert!(check_board_monotony(&corners, 3, 2));
205    }
206
207    #[test]
208    fn monotony_rejects_folded_grid() {
209        // Swap two corners in a row so the projection is non-monotonic.
210        let corners = [
211            (0.0, 0.0),
212            (20.0, 0.0),
213            (10.0, 0.0),
214            (0.0, 10.0),
215            (10.0, 10.0),
216            (20.0, 10.0),
217        ];
218        assert!(!check_board_monotony(&corners, 3, 2));
219    }
220
221    #[test]
222    fn extracts_non_square_board() {
223        // 4x3 cells -> interior lattice x in 1..=3 (3), y in 1..=2 (2) -> 3x2.
224        let side = 20;
225        let corners = board_corners(4, 3, side, 3, 2).expect("board");
226        assert_eq!(corners.len(), 6);
227        let mut expected = Vec::new();
228        for y in 1..=2 {
229            for x in 1..=3 {
230                expected.push(((x * side) as f32, (y * side) as f32));
231            }
232        }
233        for (got, want) in corners.iter().zip(expected.iter()) {
234            approx::assert_abs_diff_eq!(got.0, want.0, epsilon = 1e-3);
235            approx::assert_abs_diff_eq!(got.1, want.1, epsilon = 1e-3);
236        }
237    }
238
239    #[test]
240    fn rejects_wrong_pattern_size() {
241        // The board is 3x2; asking for 4x4 must fail.
242        assert!(board_corners(4, 3, 20, 4, 4).is_none());
243    }
244}
checkerboard_calibrate/chessboard/board.rs

checkerboard_calibrate/chessboard/
board.rs
