camera_intrinsic_calibration/
types.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
use camera_intrinsic_model::GenericModel;
use nalgebra as na;
use serde::{Deserialize, Serialize};

pub struct CalibParams {
    pub fixed_focal: Option<f64>,
    pub disabled_distortion_num: usize,
    pub one_focal: bool,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RvecTvec {
    rvec: (f64, f64, f64),
    tvec: (f64, f64, f64),
}

impl RvecTvec {
    pub fn new(rvec: &na::DVector<f64>, tvec: &na::DVector<f64>) -> RvecTvec {
        RvecTvec {
            rvec: (rvec[0], rvec[1], rvec[2]),
            tvec: (tvec[0], tvec[1], tvec[2]),
        }
    }
    pub fn to_na_isometry3(&self) -> na::Isometry3<f64> {
        na::Isometry3::new(self.na_tvec().to_vec3(), self.na_rvec().to_vec3())
    }
    pub fn na_rvec(&self) -> na::DVector<f64> {
        na::dvector![self.rvec.0, self.rvec.1, self.rvec.2]
    }
    pub fn na_tvec(&self) -> na::DVector<f64> {
        na::dvector![self.tvec.0, self.tvec.1, self.tvec.2]
    }
}

pub type Intrinsics = Vec<GenericModel<f64>>;

#[derive(Debug, Serialize, Deserialize)]
pub struct Extrinsics {
    rtvecs: Vec<RvecTvec>,
}

impl Extrinsics {
    pub fn new(rtvecs: &[RvecTvec]) -> Extrinsics {
        Extrinsics {
            rtvecs: rtvecs.to_vec(),
        }
    }
}

pub trait ToRvecTvec {
    fn to_rvec_tvec(&self) -> RvecTvec;
}
impl ToRvecTvec for na::Isometry3<f64> {
    fn to_rvec_tvec(&self) -> RvecTvec {
        let rvec = self.rotation.scaled_axis().to_dvec();
        let tvec = na::dvector![self.translation.x, self.translation.y, self.translation.z,];
        RvecTvec::new(&rvec, &tvec)
    }
}

pub trait Vec3DVec<T: Clone> {
    fn to_dvec(&self) -> na::DVector<T>;
}
pub trait DVecVec3<T: Clone> {
    fn to_vec3(&self) -> nalgebra::Vector3<T>;
}
impl<T: Clone> DVecVec3<T> for na::DVector<T> {
    fn to_vec3(&self) -> nalgebra::Vector3<T> {
        na::Vector3::new(self[0].clone(), self[1].clone(), self[2].clone())
    }
}
impl<T: Clone> Vec3DVec<T> for na::Vector3<T> {
    fn to_dvec(&self) -> na::DVector<T> {
        na::dvector![self[0].clone(), self[1].clone(), self[2].clone()]
    }
}