apriltag 0.4.0

High level Rust bindings to AprilTag library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

use anyhow::{ensure, Context, Error, Result};
use apriltag::{Detector, Family, Image, TagParams};
use clap::Parser;
use std::str::FromStr;

/// Simple AprilTag detector.
#[derive(Debug, Clone, Parser)]
struct Opts {
    #[clap(long = "family", default_value = "tag16h5")]
    /// family name.
    pub family_name: String,

    #[clap(long)]
    /// optional tag parameters in format "tagsize,fx,fy,cx,cy".
    pub tag_params: Option<TagParamsArg>,

    /// a list of input PNM image files.
    pub input_files: Vec<String>,
}

fn main() -> Result<()> {
    let Opts {
        family_name,
        tag_params,
        input_files,
    } = Opts::parse();

    ensure!(!input_files.is_empty(), "no input files");

    let family: Family = family_name.parse()?;
    let tag_params: Option<TagParams> = tag_params.map(|params| params.into());
    let mut detector = Detector::builder().add_family_bits(family, 1).build()?;

    for path in input_files {
        let image = Image::from_pnm_file(&path)?;
        let detections = detector.detect(&image);

        println!("= image {path}");

        detections.into_iter().enumerate().for_each(|(index, det)| {
            println!("  - detection {index}: {det:#?}");
            if let Some(tag_params) = &tag_params {
                let pose = det.estimate_tag_pose(tag_params);
                println!("  - pose {index}: {pose:#?}");
                println!("  - isometry {index}: {pose:#?}");
            }
        });
    }
    Ok(())
}

#[derive(Debug, Clone)]
struct TagParamsArg {
    pub tagsize: f64,
    pub fx: f64,
    pub fy: f64,
    pub cx: f64,
    pub cy: f64,
}

impl From<TagParamsArg> for TagParams {
    fn from(arg: TagParamsArg) -> Self {
        let TagParamsArg {
            tagsize,
            fx,
            fy,
            cx,
            cy,
        } = arg;

        Self {
            tagsize,
            fx,
            fy,
            cx,
            cy,
        }
    }
}

impl FromStr for TagParamsArg {
    type Err = Error;

    fn from_str(text: &str) -> Result<Self, Self::Err> {
        let tokens: Vec<_> = text.split(',').collect();
        ensure!(
            tokens.len() == 5,
            r#"tag parameters must be in format "tagsize,fx,fy,cx,cy""#
        );

        let values = tokens
            .into_iter()
            .map(|token| -> Result<_> {
                let value: f64 = token.parse()?;
                Ok(value)
            })
            .collect::<Result<Vec<_>>>()
            .with_context(|| format!("failed to parse tag parameters {}", text))?;

        Ok(Self {
            tagsize: values[0],
            fx: values[1],
            fy: values[2],
            cx: values[3],
            cy: values[4],
        })
    }
}