calib_targets_chessboard/
rectified_view.rs

1use calib_targets_core::{
2    estimate_homography_rect_to_img, warp_perspective_gray, GrayImage, GrayImageView, Homography,
3    LabeledCorner,
4};
5use nalgebra::Point2;
6
7#[derive(thiserror::Error, Debug)]
8pub enum RectifyError {
9    #[error("not enough labeled inlier corners with grid coords (need >=4)")]
10    NotEnoughPoints,
11    #[error("homography estimation failed")]
12    HomographyFailed,
13    #[error("homography not invertible")]
14    NonInvertible,
15}
16
17#[derive(Clone, Debug)]
18pub struct RectifiedBoardView {
19    pub rect: GrayImage,
20    pub h_img_from_rect: Homography,
21    pub h_rect_from_img: Homography,
22    pub min_i: i32,
23    pub min_j: i32,
24    pub max_i: i32,
25    pub max_j: i32,
26    pub px_per_square: f32,
27}
28
29pub fn rectify_from_chessboard_result(
30    src: &GrayImageView<'_>,
31    det_corners: &[LabeledCorner],
32    inliers: &[usize],
33    px_per_square: f32,
34    margin_squares: f32, // e.g. 0.5..1.0
35) -> Result<RectifiedBoardView, RectifyError> {
36    // 1) collect correspondences (rect_pt -> img_pt)
37    let mut img_pts = Vec::<Point2<f32>>::new();
38    let mut grid = Vec::<(i32, i32)>::new();
39
40    for &idx in inliers {
41        if let Some(c) = det_corners.get(idx) {
42            if let Some(g) = c.grid {
43                img_pts.push(Point2::new(c.position.x, c.position.y));
44                grid.push((g.i, g.j));
45            }
46        }
47    }
48    if img_pts.len() < 4 {
49        return Err(RectifyError::NotEnoughPoints);
50    }
51
52    // 2) bounding box in grid space
53    let (mut min_i, mut min_j) = (i32::MAX, i32::MAX);
54    let (mut max_i, mut max_j) = (i32::MIN, i32::MIN);
55    for &(i, j) in &grid {
56        min_i = min_i.min(i);
57        min_j = min_j.min(j);
58        max_i = max_i.max(i);
59        max_j = max_j.max(j);
60    }
61
62    // Add margin (in squares) to include some border region for ArUco later
63    let mi = (min_i as f32 - margin_squares).floor();
64    let mj = (min_j as f32 - margin_squares).floor();
65    let ma = (max_i as f32 + margin_squares).ceil();
66    let mb = (max_j as f32 + margin_squares).ceil();
67
68    let min_i_m = mi as i32;
69    let min_j_m = mj as i32;
70    let max_i_m = ma as i32;
71    let max_j_m = mb as i32;
72
73    let out_w = ((max_i_m - min_i_m) as f32 * px_per_square)
74        .round()
75        .max(1.0) as usize;
76    let out_h = ((max_j_m - min_j_m) as f32 * px_per_square)
77        .round()
78        .max(1.0) as usize;
79
80    // 3) build rectified points for DLT
81    let mut rect_pts = Vec::<Point2<f32>>::with_capacity(grid.len());
82    for &(i, j) in &grid {
83        let x = (i - min_i_m) as f32 * px_per_square;
84        let y = (j - min_j_m) as f32 * px_per_square;
85        rect_pts.push(Point2::new(x, y));
86    }
87
88    // 4) estimate H_img_from_rect
89    let h_img_from_rect = estimate_homography_rect_to_img(&rect_pts, &img_pts)
90        .ok_or(RectifyError::HomographyFailed)?;
91
92    let h_rect_from_img = h_img_from_rect
93        .inverse()
94        .ok_or(RectifyError::NonInvertible)?;
95
96    // 5) warp
97    let rect = warp_perspective_gray(src, h_img_from_rect, out_w, out_h);
98
99    Ok(RectifiedBoardView {
100        rect,
101        h_img_from_rect,
102        h_rect_from_img,
103        min_i: min_i_m,
104        min_j: min_j_m,
105        max_i: max_i_m,
106        max_j: max_j_m,
107        px_per_square,
108    })
109}
calib_targets_chessboard/rectified_view.rs

calib_targets_chessboard/
rectified_view.rs
