calib_targets_charuco/
alignment.rs

1//! Marker-to-board alignment and corner ID assignment.
2
3use crate::board::CharucoBoard;
4use calib_targets_aruco::{BoardCell, GridCell, MarkerDetection};
5use calib_targets_core::{GridAlignment, GridTransform, GRID_TRANSFORMS_D4};
6use log::debug;
7use serde::{Deserialize, Serialize};
8
9#[cfg(feature = "tracing")]
10use tracing::instrument;
11
12/// Alignment result between detected markers and a board specification.
13#[derive(Clone, Debug, Serialize, Deserialize)]
14pub struct CharucoAlignment {
15    pub alignment: GridAlignment,
16    pub marker_inliers: Vec<usize>,
17}
18
19impl CharucoAlignment {
20    /// Map grid coordinates `(i, j)` into board coordinates.
21    #[inline]
22    pub fn map(&self, i: i32, j: i32) -> [i32; 2] {
23        self.alignment.map(i, j)
24    }
25}
26
27fn dominant_rotation(markers: &[MarkerDetection]) -> u8 {
28    let mut hist = [0.0f32; 4];
29    for m in markers {
30        hist[(m.rotation & 3) as usize] += m.score;
31    }
32    hist.iter()
33        .enumerate()
34        .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap())
35        .map(|(i, _)| i as u8)
36        .unwrap_or(0)
37}
38
39#[derive(Clone, Copy)]
40struct Pair {
41    idx: usize,
42    bc: BoardCell,
43    gc: GridCell,
44    weight: f32,
45}
46
47/// Estimate a grid transform + translation that best aligns marker detections to the board.
48#[cfg_attr(feature = "tracing", instrument(level = "info", skip(board, markers)))]
49pub(crate) fn solve_alignment(
50    board: &CharucoBoard,
51    markers: &[MarkerDetection],
52) -> Option<CharucoAlignment> {
53    let pairs = marker_pairs(board, markers);
54    if pairs.is_empty() {
55        return None;
56    }
57
58    let rot_mode = dominant_rotation(markers);
59    let transform = GRID_TRANSFORMS_D4[rot_mode as usize];
60    let (translation, weight_sum, count) = best_translation(&pairs, transform)?;
61    let inliers = inliers_for_transform(&pairs, transform, translation);
62    debug!("Dominant rotation is {rot_mode}, {} inliers", inliers.len());
63    let candidate = (weight_sum, count, transform, translation, inliers);
64
65    let (_, _, transform, translation, marker_inliers) = candidate;
66    Some(CharucoAlignment {
67        alignment: GridAlignment {
68            transform,
69            translation,
70        },
71        marker_inliers,
72    })
73}
74
75fn marker_pairs(board: &CharucoBoard, markers: &[MarkerDetection]) -> Vec<Pair> {
76    markers
77        .iter()
78        .enumerate()
79        .filter_map(|(idx, m)| {
80            board.marker_position(m.id).map(|bc| Pair {
81                idx,
82                gc: m.gc,
83                bc,
84                weight: m.score.max(0.0),
85            })
86        })
87        .collect()
88}
89
90fn best_translation(pairs: &[Pair], transform: GridTransform) -> Option<([i32; 2], f32, usize)> {
91    let mut counts: std::collections::HashMap<[i32; 2], (f32, usize)> =
92        std::collections::HashMap::new();
93    for p in pairs {
94        let [rx, ry] = transform.apply(p.gc.gx, p.gc.gy);
95        let t = [p.bc.sx - rx, p.bc.sy - ry];
96        let entry = counts.entry(t).or_insert((0.0, 0));
97        entry.0 += p.weight;
98        entry.1 += 1;
99    }
100
101    let (translation, (weight_sum, count)) = counts.into_iter().max_by(|(_, a), (_, b)| {
102        a.0.partial_cmp(&b.0)
103            .unwrap_or(std::cmp::Ordering::Equal)
104            .then_with(|| a.1.cmp(&b.1))
105    })?;
106    Some((translation, weight_sum, count))
107}
108
109fn inliers_for_transform(
110    pairs: &[Pair],
111    transform: GridTransform,
112    translation: [i32; 2],
113) -> Vec<usize> {
114    let mut inliers = Vec::new();
115    for p in pairs {
116        let [x, y] = transform.apply(p.gc.gx, p.gc.gy);
117        if x + translation[0] == p.bc.sx && y + translation[1] == p.bc.sy {
118            inliers.push(p.idx);
119        }
120    }
121    inliers
122}
123
124#[cfg(test)]
125mod tests {
126    use super::*;
127    use crate::board::{CharucoBoard, CharucoBoardSpec, MarkerLayout};
128    use calib_targets_aruco::builtins;
129    use nalgebra::Point2;
130
131    fn build_board() -> CharucoBoard {
132        let dict = builtins::builtin_dictionary("DICT_4X4_50").expect("dict");
133        CharucoBoard::new(CharucoBoardSpec {
134            rows: 6,
135            cols: 6,
136            cell_size: 1.0,
137            marker_size_rel: 0.75,
138            dictionary: dict,
139            marker_layout: MarkerLayout::OpenCvCharuco,
140        })
141        .expect("board")
142    }
143
144    #[test]
145    fn alignment_identity_transform() {
146        let board = build_board();
147        let mut markers = Vec::new();
148
149        for id in 0..6u32 {
150            let Some(bc) = board.marker_position(id) else {
151                continue;
152            };
153            markers.push(MarkerDetection {
154                id,
155                gc: GridCell {
156                    gx: bc.sx,
157                    gy: bc.sy,
158                },
159                rotation: 0,
160                hamming: 0,
161                score: 1.0,
162                border_score: 1.0,
163                code: 0,
164                inverted: false,
165                corners_rect: [Point2::new(0.0, 0.0); 4],
166                corners_img: None,
167            });
168        }
169
170        let alignment = solve_alignment(&board, &markers).expect("alignment");
171        assert_eq!(alignment.alignment.transform, GridTransform::IDENTITY);
172        assert_eq!(alignment.alignment.translation, [0, 0]);
173        assert!(!alignment.marker_inliers.is_empty());
174    }
175}
calib_targets_charuco/alignment.rs

calib_targets_charuco/
alignment.rs
