nuscenes-data-nalgebra 0.1.0

Extension crate to nuscenes-data adding `nalgebra` integration
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

use nalgebra as na;
use nuscenes_data::serializable::{CalibratedSensor, EgoPose, SampleAnnotation};

pub use nalgebra;

pub mod prelude {
    pub use super::{CalibratedSensorNalgebraExt, EgoPoseNalgebraExt, SampleAnnotationNalgebraExt};
}

pub trait CalibratedSensorNalgebraExt {
    fn na_camera_intrinsic_matrix(&self) -> Option<na::Matrix3<f64>>;
    fn na_translation(&self) -> na::Translation3<f64>;
}

impl CalibratedSensorNalgebraExt for CalibratedSensor {
    fn na_camera_intrinsic_matrix(&self) -> Option<na::Matrix3<f64>> {
        let iter = self.camera_intrinsic.as_ref()?.iter().flatten().cloned();
        Some(na::Matrix3::from_iterator(iter))
    }

    fn na_translation(&self) -> na::Translation3<f64> {
        self.translation.into()
    }
}

pub trait EgoPoseNalgebraExt {
    fn na_rotation(&self) -> na::UnitQuaternion<f64>;
    fn na_translation(&self) -> na::Translation3<f64>;
    fn na_transofrm(&self) -> na::Isometry3<f64>;
}

impl EgoPoseNalgebraExt for EgoPose {
    fn na_rotation(&self) -> na::UnitQuaternion<f64> {
        let quat: na::Quaternion<f64> = self.rotation.into();
        na::Unit::new_normalize(quat)
    }

    fn na_translation(&self) -> na::Translation3<f64> {
        self.translation.into()
    }

    fn na_transofrm(&self) -> na::Isometry3<f64> {
        na::Isometry3::from_parts(self.na_translation(), self.na_rotation())
    }
}

pub trait SampleAnnotationNalgebraExt {
    fn na_size(&self) -> na::Vector3<f64>;
    fn na_rotation(&self) -> na::UnitQuaternion<f64>;
    fn na_translation(&self) -> na::Translation3<f64>;
    fn na_transofrm(&self) -> na::Isometry3<f64>;
}

impl SampleAnnotationNalgebraExt for SampleAnnotation {
    fn na_rotation(&self) -> na::UnitQuaternion<f64> {
        let quat: na::Quaternion<f64> = self.rotation.into();
        na::Unit::new_normalize(quat)
    }

    fn na_translation(&self) -> na::Translation3<f64> {
        self.translation.into()
    }

    fn na_transofrm(&self) -> na::Isometry3<f64> {
        na::Isometry3::from_parts(self.na_translation(), self.na_rotation())
    }

    fn na_size(&self) -> na::Vector3<f64> {
        self.size.into()
    }
}