camera_intrinsic_calibration/
board.rs

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
use glam;
use serde::{Deserialize, Serialize};
use std::{collections::HashMap, io::Write};

#[derive(Debug, Serialize, Deserialize)]
pub struct BoardConfig {
    tag_size_meter: f32,
    tag_spacing: f32,
    tag_rows: usize,
    tag_cols: usize,
    first_id: u32,
}

pub fn board_config_to_json(output_path: &str, board_config: &BoardConfig) {
    let j = serde_json::to_string_pretty(board_config).unwrap();
    let mut file = std::fs::File::create(output_path).unwrap();
    file.write_all(j.as_bytes()).unwrap();
}

pub fn board_config_from_json(file_path: &str) -> BoardConfig {
    let contents =
        std::fs::read_to_string(file_path).expect("Should have been able to read the file");
    serde_json::from_str(&contents).unwrap()
}

impl Default for BoardConfig {
    fn default() -> Self {
        Self {
            tag_size_meter: 0.088,
            tag_spacing: 0.3,
            tag_rows: 6,
            tag_cols: 6,
            first_id: 0,
        }
    }
}

pub struct Board {
    pub id_to_3d: HashMap<u32, glam::Vec3>,
}

impl Board {
    pub fn from_config(board_config: &BoardConfig) -> Board {
        Self::init_aprilgrid(
            board_config.tag_size_meter,
            board_config.tag_spacing,
            board_config.tag_rows,
            board_config.tag_cols,
            board_config.first_id,
        )
    }
    pub fn init_aprilgrid(
        tag_size_meter: f32,
        tag_spacing: f32,
        tag_rows: usize,
        tag_cols: usize,
        first_id: u32,
    ) -> Board {
        let mut id_to_3d = HashMap::new();
        let mut count_id = first_id * 4;
        for r in 0..tag_rows {
            for c in 0..tag_cols {
                let start_x = (c as f32) * tag_size_meter * (1.0 + tag_spacing);
                let start_y = -1.0 * (r as f32) * tag_size_meter * (1.0 + tag_spacing);
                id_to_3d.insert(
                    count_id,
                    glam::Vec3 {
                        x: start_x,
                        y: start_y,
                        z: 0.0,
                    },
                );
                id_to_3d.insert(
                    count_id + 1,
                    glam::Vec3 {
                        x: start_x + tag_size_meter,
                        y: start_y,
                        z: 0.0,
                    },
                );
                id_to_3d.insert(
                    count_id + 2,
                    glam::Vec3 {
                        x: start_x + tag_size_meter,
                        y: start_y - tag_size_meter,
                        z: 0.0,
                    },
                );
                id_to_3d.insert(
                    count_id + 3,
                    glam::Vec3 {
                        x: start_x,
                        y: start_y - tag_size_meter,
                        z: 0.0,
                    },
                );
                count_id += 4;
            }
        }
        Board { id_to_3d }
    }
}

pub fn create_default_6x6_board() -> Board {
    Board::init_aprilgrid(0.088, 0.3, 6, 6, 0)
}