nuscenes_data_nalgebra/
lib.rs

1use nalgebra as na;
2use nuscenes_data::serializable::{CalibratedSensor, EgoPose, SampleAnnotation};
3
4pub use nalgebra;
5
6pub mod prelude {
7    pub use super::{CalibratedSensorNalgebraExt, EgoPoseNalgebraExt, SampleAnnotationNalgebraExt};
8}
9
10pub trait CalibratedSensorNalgebraExt {
11    fn na_camera_intrinsic_matrix(&self) -> Option<na::Matrix3<f64>>;
12    fn na_translation(&self) -> na::Translation3<f64>;
13}
14
15impl CalibratedSensorNalgebraExt for CalibratedSensor {
16    fn na_camera_intrinsic_matrix(&self) -> Option<na::Matrix3<f64>> {
17        let iter = self.camera_intrinsic.as_ref()?.iter().flatten().cloned();
18        Some(na::Matrix3::from_iterator(iter))
19    }
20
21    fn na_translation(&self) -> na::Translation3<f64> {
22        self.translation.into()
23    }
24}
25
26pub trait EgoPoseNalgebraExt {
27    fn na_rotation(&self) -> na::UnitQuaternion<f64>;
28    fn na_translation(&self) -> na::Translation3<f64>;
29    fn na_transofrm(&self) -> na::Isometry3<f64>;
30}
31
32impl EgoPoseNalgebraExt for EgoPose {
33    fn na_rotation(&self) -> na::UnitQuaternion<f64> {
34        let quat: na::Quaternion<f64> = self.rotation.into();
35        na::Unit::new_normalize(quat)
36    }
37
38    fn na_translation(&self) -> na::Translation3<f64> {
39        self.translation.into()
40    }
41
42    fn na_transofrm(&self) -> na::Isometry3<f64> {
43        na::Isometry3::from_parts(self.na_translation(), self.na_rotation())
44    }
45}
46
47pub trait SampleAnnotationNalgebraExt {
48    fn na_size(&self) -> na::Vector3<f64>;
49    fn na_rotation(&self) -> na::UnitQuaternion<f64>;
50    fn na_translation(&self) -> na::Translation3<f64>;
51    fn na_transofrm(&self) -> na::Isometry3<f64>;
52}
53
54impl SampleAnnotationNalgebraExt for SampleAnnotation {
55    fn na_rotation(&self) -> na::UnitQuaternion<f64> {
56        let quat: na::Quaternion<f64> = self.rotation.into();
57        na::Unit::new_normalize(quat)
58    }
59
60    fn na_translation(&self) -> na::Translation3<f64> {
61        self.translation.into()
62    }
63
64    fn na_transofrm(&self) -> na::Isometry3<f64> {
65        na::Isometry3::from_parts(self.na_translation(), self.na_rotation())
66    }
67
68    fn na_size(&self) -> na::Vector3<f64> {
69        self.size.into()
70    }
71}
nuscenes_data_nalgebra/lib.rs

nuscenes_data_nalgebra/
lib.rs
