chalkydri/pose/
field_layout.rs

1use std::collections::HashMap;
2
3use nalgebra as na;
4use sophus_autodiff::linalg::VecF64;
5use sophus_lie::{Isometry3F64, Rotation3F64};
6
7use crate::error::Error;
8
9use super::PoseEstimator;
10
11#[derive(Default, Debug, Clone, PartialEq, Serialize, Deserialize)]
12#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
13#[serde(rename_all = "camelCase")]
14pub struct AprilTagFieldLayout {
15    pub tags: Vec<LayoutTag>,
16    pub field: Field,
17}
18impl AprilTagFieldLayout {
19    pub async fn load(
20        &self,
21        pose_est: &mut PoseEstimator,
22    ) -> Result<HashMap<usize, Isometry3F64>, Error> {
23        let mut tags: HashMap<usize, Isometry3F64> = HashMap::new();
24        for LayoutTag {
25            id,
26            pose:
27                LayoutPose {
28                    translation,
29                    rotation: LayoutRotation { quaternion },
30                },
31        } in self.tags.clone()
32        {
33            // Turn the field layout values into Rust datatypes
34            let translation = VecF64::<3>::new(translation.x, translation.y, translation.z);
35            let rotation = na::UnitQuaternion::from_quaternion(na::Quaternion::new(
36                quaternion.x,
37                quaternion.y,
38                quaternion.z,
39                quaternion.w,
40            ))
41            .to_rotation_matrix();
42            let rotation = Rotation3F64::try_from_mat(rotation.matrix()).unwrap();
43
44            let isometry = Isometry3F64::from_translation_and_rotation(translation, rotation);
45
46            tags.insert(id as usize, isometry);
47        }
48
49        Ok(tags)
50    }
51}
52
53#[derive(Default, Debug, Clone, PartialEq, Serialize, Deserialize)]
54#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
55#[serde(rename_all = "camelCase")]
56pub struct LayoutTag {
57    #[serde(rename = "ID")]
58    pub id: i64,
59    pub pose: LayoutPose,
60}
61
62#[derive(Default, Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
63#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
64#[serde(rename_all = "camelCase")]
65pub struct LayoutPose {
66    pub translation: LayoutTranslation,
67    pub rotation: LayoutRotation,
68}
69
70#[derive(Default, Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
71#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
72#[serde(rename_all = "camelCase")]
73pub struct LayoutTranslation {
74    pub x: f64,
75    pub y: f64,
76    pub z: f64,
77}
78
79#[derive(Default, Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
80#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
81#[serde(rename_all = "camelCase")]
82pub struct LayoutRotation {
83    pub quaternion: LayoutQuaternion,
84}
85
86#[derive(Default, Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
87#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
88#[serde(rename_all = "camelCase")]
89pub struct LayoutQuaternion {
90    #[serde(rename = "W")]
91    pub w: f64,
92    #[serde(rename = "X")]
93    pub x: f64,
94    #[serde(rename = "Y")]
95    pub y: f64,
96    #[serde(rename = "Z")]
97    pub z: f64,
98}
99
100#[derive(Default, Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
101#[cfg_attr(feature = "web", derive(utopia::ToSchema))]
102#[serde(rename_all = "camelCase")]
103pub struct Field {
104    pub length: f64,
105    pub width: f64,
106}
chalkydri/pose/field_layout.rs

chalkydri/pose/
field_layout.rs
