1#[derive(Clone, Copy, Debug, PartialEq)]
13#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
14pub enum SegmentationConnectivity {
15 Four,
18 Eight,
21}
22
23#[derive(Clone, Copy, Debug, PartialEq)]
25#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
26pub enum CornerRefinementMode {
27 None,
29 Edge,
31 GridFit,
33 Erf,
35}
36
37#[derive(Clone, Copy, Debug, PartialEq)]
39#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
40pub enum DecodeMode {
41 Hard,
43 Soft,
45}
46
47#[derive(Clone, Copy, Debug, PartialEq)]
49#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
50pub enum PoseEstimationMode {
51 Fast,
53 Accurate,
59}
60
61#[derive(Clone, Copy, Debug, PartialEq)]
77#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
78pub struct DetectorConfig {
79 pub threshold_tile_size: usize,
83 pub threshold_min_range: u8,
86
87 pub enable_bilateral: bool,
90 pub bilateral_sigma_space: f32,
92 pub bilateral_sigma_color: f32,
95
96 pub enable_sharpening: bool,
98
99 pub enable_adaptive_window: bool,
101 pub threshold_min_radius: usize,
103 pub threshold_max_radius: usize,
105 pub adaptive_threshold_constant: i16,
107 pub adaptive_threshold_gradient_threshold: u8,
109
110 pub quad_min_area: u32,
113 pub quad_max_aspect_ratio: f32,
115 pub quad_min_fill_ratio: f32,
117 pub quad_max_fill_ratio: f32,
119 pub quad_min_edge_length: f64,
121 pub quad_min_edge_score: f64,
123 pub subpixel_refinement_sigma: f64,
125 pub segmentation_margin: i16,
127 pub segmentation_connectivity: SegmentationConnectivity,
129 pub upscale_factor: usize,
133
134 pub decoder_min_contrast: f64,
139 pub refinement_mode: CornerRefinementMode,
141 pub decode_mode: DecodeMode,
143}
144
145impl Default for DetectorConfig {
146 fn default() -> Self {
147 Self {
148 threshold_tile_size: 8,
149 threshold_min_range: 10,
150 enable_bilateral: false,
151 bilateral_sigma_space: 0.8,
152 bilateral_sigma_color: 30.0,
153 enable_sharpening: false,
154 enable_adaptive_window: false,
155 threshold_min_radius: 2,
156 threshold_max_radius: 15,
157 adaptive_threshold_constant: 0, adaptive_threshold_gradient_threshold: 10,
159 quad_min_area: 16,
160 quad_max_aspect_ratio: 10.0,
161 quad_min_fill_ratio: 0.10,
162 quad_max_fill_ratio: 0.98,
163 quad_min_edge_length: 2.0,
164 quad_min_edge_score: 0.1,
165 subpixel_refinement_sigma: 0.6,
166 segmentation_margin: 1,
167 segmentation_connectivity: SegmentationConnectivity::Eight,
168 upscale_factor: 1,
169 decoder_min_contrast: 20.0,
170 refinement_mode: CornerRefinementMode::Erf,
171 decode_mode: DecodeMode::Hard,
172 }
173 }
174}
175
176impl DetectorConfig {
177 #[must_use]
179 pub fn builder() -> DetectorConfigBuilder {
180 DetectorConfigBuilder::default()
181 }
182}
183
184#[derive(Default)]
186pub struct DetectorConfigBuilder {
187 threshold_tile_size: Option<usize>,
188 threshold_min_range: Option<u8>,
189 enable_bilateral: Option<bool>,
190 bilateral_sigma_space: Option<f32>,
191 bilateral_sigma_color: Option<f32>,
192 enable_sharpening: Option<bool>,
193 enable_adaptive_window: Option<bool>,
194 threshold_min_radius: Option<usize>,
195 threshold_max_radius: Option<usize>,
196 adaptive_threshold_constant: Option<i16>,
197 adaptive_threshold_gradient_threshold: Option<u8>,
198 quad_min_area: Option<u32>,
199 quad_max_aspect_ratio: Option<f32>,
200 quad_min_fill_ratio: Option<f32>,
201 quad_max_fill_ratio: Option<f32>,
202 quad_min_edge_length: Option<f64>,
203 pub quad_min_edge_score: Option<f64>,
205 pub subpixel_refinement_sigma: Option<f64>,
207 pub segmentation_margin: Option<i16>,
209 pub segmentation_connectivity: Option<SegmentationConnectivity>,
211 pub upscale_factor: Option<usize>,
213 pub decoder_min_contrast: Option<f64>,
215 pub refinement_mode: Option<CornerRefinementMode>,
217 pub decode_mode: Option<DecodeMode>,
219}
220
221impl DetectorConfigBuilder {
222 #[must_use]
224 pub fn threshold_tile_size(mut self, size: usize) -> Self {
225 self.threshold_tile_size = Some(size);
226 self
227 }
228
229 #[must_use]
231 pub fn threshold_min_range(mut self, range: u8) -> Self {
232 self.threshold_min_range = Some(range);
233 self
234 }
235
236 #[must_use]
238 pub fn quad_min_area(mut self, area: u32) -> Self {
239 self.quad_min_area = Some(area);
240 self
241 }
242
243 #[must_use]
245 pub fn quad_max_aspect_ratio(mut self, ratio: f32) -> Self {
246 self.quad_max_aspect_ratio = Some(ratio);
247 self
248 }
249
250 #[must_use]
252 pub fn quad_min_fill_ratio(mut self, ratio: f32) -> Self {
253 self.quad_min_fill_ratio = Some(ratio);
254 self
255 }
256
257 #[must_use]
259 pub fn quad_max_fill_ratio(mut self, ratio: f32) -> Self {
260 self.quad_max_fill_ratio = Some(ratio);
261 self
262 }
263
264 #[must_use]
266 pub fn quad_min_edge_length(mut self, length: f64) -> Self {
267 self.quad_min_edge_length = Some(length);
268 self
269 }
270
271 #[must_use]
273 pub fn quad_min_edge_score(mut self, score: f64) -> Self {
274 self.quad_min_edge_score = Some(score);
275 self
276 }
277
278 #[must_use]
280 pub fn enable_bilateral(mut self, enable: bool) -> Self {
281 self.enable_bilateral = Some(enable);
282 self
283 }
284
285 #[must_use]
287 pub fn bilateral_sigma_space(mut self, sigma: f32) -> Self {
288 self.bilateral_sigma_space = Some(sigma);
289 self
290 }
291
292 #[must_use]
294 pub fn bilateral_sigma_color(mut self, sigma: f32) -> Self {
295 self.bilateral_sigma_color = Some(sigma);
296 self
297 }
298
299 #[must_use]
301 pub fn enable_sharpening(mut self, enable: bool) -> Self {
302 self.enable_sharpening = Some(enable);
303 self
304 }
305
306 #[must_use]
308 pub fn enable_adaptive_window(mut self, enable: bool) -> Self {
309 self.enable_adaptive_window = Some(enable);
310 self
311 }
312
313 #[must_use]
315 pub fn threshold_min_radius(mut self, radius: usize) -> Self {
316 self.threshold_min_radius = Some(radius);
317 self
318 }
319
320 #[must_use]
322 pub fn threshold_max_radius(mut self, radius: usize) -> Self {
323 self.threshold_max_radius = Some(radius);
324 self
325 }
326
327 #[must_use]
329 pub fn adaptive_threshold_constant(mut self, c: i16) -> Self {
330 self.adaptive_threshold_constant = Some(c);
331 self
332 }
333
334 #[must_use]
336 pub fn adaptive_threshold_gradient_threshold(mut self, threshold: u8) -> Self {
337 self.adaptive_threshold_gradient_threshold = Some(threshold);
338 self
339 }
340
341 #[must_use]
343 pub fn build(self) -> DetectorConfig {
344 let d = DetectorConfig::default();
345 DetectorConfig {
346 threshold_tile_size: self.threshold_tile_size.unwrap_or(d.threshold_tile_size),
347 threshold_min_range: self.threshold_min_range.unwrap_or(d.threshold_min_range),
348 enable_bilateral: self.enable_bilateral.unwrap_or(d.enable_bilateral),
349 bilateral_sigma_space: self
350 .bilateral_sigma_space
351 .unwrap_or(d.bilateral_sigma_space),
352 bilateral_sigma_color: self
353 .bilateral_sigma_color
354 .unwrap_or(d.bilateral_sigma_color),
355 enable_sharpening: self.enable_sharpening.unwrap_or(d.enable_sharpening),
356 enable_adaptive_window: self
357 .enable_adaptive_window
358 .unwrap_or(d.enable_adaptive_window),
359 threshold_min_radius: self.threshold_min_radius.unwrap_or(d.threshold_min_radius),
360 threshold_max_radius: self.threshold_max_radius.unwrap_or(d.threshold_max_radius),
361 adaptive_threshold_constant: self
362 .adaptive_threshold_constant
363 .unwrap_or(d.adaptive_threshold_constant),
364 adaptive_threshold_gradient_threshold: self
365 .adaptive_threshold_gradient_threshold
366 .unwrap_or(d.adaptive_threshold_gradient_threshold),
367 quad_min_area: self.quad_min_area.unwrap_or(d.quad_min_area),
368 quad_max_aspect_ratio: self
369 .quad_max_aspect_ratio
370 .unwrap_or(d.quad_max_aspect_ratio),
371 quad_min_fill_ratio: self.quad_min_fill_ratio.unwrap_or(d.quad_min_fill_ratio),
372 quad_max_fill_ratio: self.quad_max_fill_ratio.unwrap_or(d.quad_max_fill_ratio),
373 quad_min_edge_length: self.quad_min_edge_length.unwrap_or(d.quad_min_edge_length),
374 quad_min_edge_score: self.quad_min_edge_score.unwrap_or(d.quad_min_edge_score),
375 subpixel_refinement_sigma: self
376 .subpixel_refinement_sigma
377 .unwrap_or(d.subpixel_refinement_sigma),
378 segmentation_margin: self.segmentation_margin.unwrap_or(d.segmentation_margin),
379 segmentation_connectivity: self
380 .segmentation_connectivity
381 .unwrap_or(d.segmentation_connectivity),
382 upscale_factor: self.upscale_factor.unwrap_or(d.upscale_factor),
383 decoder_min_contrast: self.decoder_min_contrast.unwrap_or(d.decoder_min_contrast),
384 refinement_mode: self.refinement_mode.unwrap_or(d.refinement_mode),
385 decode_mode: self.decode_mode.unwrap_or(d.decode_mode),
386 }
387 }
388
389 #[must_use]
391 pub fn segmentation_connectivity(mut self, connectivity: SegmentationConnectivity) -> Self {
392 self.segmentation_connectivity = Some(connectivity);
393 self
394 }
395
396 #[must_use]
398 pub fn segmentation_margin(mut self, margin: i16) -> Self {
399 self.segmentation_margin = Some(margin);
400 self
401 }
402
403 #[must_use]
405 pub fn upscale_factor(mut self, factor: usize) -> Self {
406 self.upscale_factor = Some(factor);
407 self
408 }
409
410 #[must_use]
413 pub fn decoder_min_contrast(mut self, contrast: f64) -> Self {
414 self.decoder_min_contrast = Some(contrast);
415 self
416 }
417
418 #[must_use]
420 pub fn refinement_mode(mut self, mode: CornerRefinementMode) -> Self {
421 self.refinement_mode = Some(mode);
422 self
423 }
424
425 #[must_use]
427 pub fn decode_mode(mut self, mode: DecodeMode) -> Self {
428 self.decode_mode = Some(mode);
429 self
430 }
431}
432
433#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
439#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
440pub enum TagFamily {
441 AprilTag36h11,
443 AprilTag16h5,
445 ArUco4x4_50,
447 ArUco4x4_100,
449}
450
451impl TagFamily {
452 #[must_use]
454 pub const fn all() -> &'static [TagFamily] {
455 &[
456 TagFamily::AprilTag36h11,
457 TagFamily::AprilTag16h5,
458 TagFamily::ArUco4x4_50,
459 TagFamily::ArUco4x4_100,
460 ]
461 }
462}
463
464#[derive(Clone, Debug, PartialEq)]
483#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
484pub struct DetectOptions {
485 pub families: Vec<TagFamily>,
487 pub intrinsics: Option<crate::pose::CameraIntrinsics>,
489 pub tag_size: Option<f64>,
491 pub decimation: usize,
494 pub pose_estimation_mode: PoseEstimationMode,
496}
497
498impl Default for DetectOptions {
499 fn default() -> Self {
500 Self {
501 families: Vec::new(),
502 intrinsics: None,
503 tag_size: None,
504 decimation: 1,
505 pose_estimation_mode: PoseEstimationMode::Fast,
506 }
507 }
508}
509
510impl DetectOptions {
511 #[must_use]
513 pub fn builder() -> DetectOptionsBuilder {
514 DetectOptionsBuilder::default()
515 }
516 #[must_use]
518 pub fn with_families(families: &[TagFamily]) -> Self {
519 Self {
520 families: families.to_vec(),
521 intrinsics: None,
522 tag_size: None,
523 decimation: 1,
524 pose_estimation_mode: PoseEstimationMode::Fast,
525 }
526 }
527
528 #[must_use]
530 pub fn all_families() -> Self {
531 Self {
532 families: TagFamily::all().to_vec(),
533 intrinsics: None,
534 tag_size: None,
535 decimation: 1,
536 pose_estimation_mode: PoseEstimationMode::Fast,
537 }
538 }
539}
540
541pub struct DetectOptionsBuilder {
543 families: Vec<TagFamily>,
544 intrinsics: Option<crate::pose::CameraIntrinsics>,
545 tag_size: Option<f64>,
546 decimation: usize,
547 pose_estimation_mode: PoseEstimationMode,
548}
549
550impl Default for DetectOptionsBuilder {
551 fn default() -> Self {
552 Self {
553 families: Vec::new(),
554 intrinsics: None,
555 tag_size: None,
556 decimation: 1,
557 pose_estimation_mode: PoseEstimationMode::Fast,
558 }
559 }
560}
561
562impl DetectOptionsBuilder {
563 #[must_use]
565 pub fn families(mut self, families: &[TagFamily]) -> Self {
566 self.families = families.to_vec();
567 self
568 }
569
570 #[must_use]
572 pub fn intrinsics(mut self, fx: f64, fy: f64, cx: f64, cy: f64) -> Self {
573 self.intrinsics = Some(crate::pose::CameraIntrinsics::new(fx, fy, cx, cy));
574 self
575 }
576
577 #[must_use]
579 pub fn tag_size(mut self, size: f64) -> Self {
580 self.tag_size = Some(size);
581 self
582 }
583
584 #[must_use]
586 pub fn decimation(mut self, decimation: usize) -> Self {
587 self.decimation = decimation.max(1);
588 self
589 }
590
591 #[must_use]
593 pub fn pose_estimation_mode(mut self, mode: PoseEstimationMode) -> Self {
594 self.pose_estimation_mode = mode;
595 self
596 }
597
598 #[must_use]
600 pub fn build(self) -> DetectOptions {
601 DetectOptions {
602 families: self.families,
603 intrinsics: self.intrinsics,
604 tag_size: self.tag_size,
605 decimation: self.decimation,
606 pose_estimation_mode: self.pose_estimation_mode,
607 }
608 }
609}
610
611#[cfg(test)]
612mod tests {
613 use super::*;
614
615 #[test]
616 fn test_detector_config_builder() {
617 let config = DetectorConfig::builder()
618 .threshold_tile_size(16)
619 .quad_min_area(1000)
620 .build();
621 assert_eq!(config.threshold_tile_size, 16);
622 assert_eq!(config.quad_min_area, 1000);
623 assert_eq!(config.threshold_min_range, 10);
625 }
626
627 #[test]
628 fn test_detector_config_defaults() {
629 let config = DetectorConfig::default();
630 assert_eq!(config.threshold_tile_size, 8);
631 assert_eq!(config.quad_min_area, 16);
632 }
633
634 #[test]
635 fn test_detect_options_families() {
636 let opt = DetectOptions::with_families(&[TagFamily::AprilTag36h11]);
637 assert_eq!(opt.families.len(), 1);
638 assert_eq!(opt.families[0], TagFamily::AprilTag36h11);
639 }
640
641 #[test]
642 fn test_detect_options_default_empty() {
643 let opt = DetectOptions::default();
644 assert!(opt.families.is_empty());
645 }
646
647 #[test]
648 fn test_all_families() {
649 let opt = DetectOptions::all_families();
650 assert_eq!(opt.families.len(), 4);
651 }
652}