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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137

use super::*;

pub struct Calibration<'a> {
    factory: &'a Factory,
    pub(crate) calibration: k4a_calibration_t,
}

impl Calibration<'_> {
    pub(crate) fn from_handle(factory: &Factory, calibration: k4a_calibration_t) -> Calibration {
        Calibration {
            factory: factory,
            calibration: calibration,
        }
    }

    pub fn from_raw<'a>(
        factory: &'a Factory,
        raw_calibration: &Vec<u8>,
        target_depth_mode: k4a_depth_mode_t,
        target_color_resolution: k4a_color_resolution_t,
    ) -> Result<Calibration<'a>, Error> {
        let mut calibration = k4a_calibration_t::default();
        Error::from((factory.k4a_calibration_get_from_raw)(
            raw_calibration.as_ptr() as *mut i8,
            raw_calibration.len(),
            target_depth_mode,
            target_color_resolution,
            &mut calibration,
        ))
        .to_result_fn(&|| Calibration::from_handle(factory, calibration))
    }

    /// Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system.
    pub fn convert_3d_to_3d(
        &self,
        source_point3d: &k4a_float3_t,
        source_camera: k4a_calibration_type_t,
        target_camera: k4a_calibration_type_t,
    ) -> Result<k4a_float3_t, Error> {
        let mut target_point3d = k4a_float3_t::default();
        Error::from((self.factory.k4a_calibration_3d_to_3d)(
            &self.calibration,
            source_point3d,
            source_camera,
            target_camera,
            &mut target_point3d,
        ))
        .to_result(target_point3d)
    }

    /// Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
    /// Returns false if the point is invalid in the target coordinate system (and therefore target_point3d should not be used)
    pub fn convert_2d_to_3d(
        &self,
        source_point2d: &k4a_float2_t,
        source_depth: f32,
        source_camera: k4a_calibration_type_t,
        target_camera: k4a_calibration_type_t,
    ) -> Result<(k4a_float3_t, bool), Error> {
        let mut target_point3d = k4a_float3_t::default();
        let mut valid: i32 = 0;
        Error::from((self.factory.k4a_calibration_2d_to_3d)(
            &self.calibration,
            source_point2d,
            source_depth,
            source_camera,
            target_camera,
            &mut target_point3d,
            &mut valid,
        ))
        .to_result((target_point3d, valid != 0))
    }

    /// Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera.
    /// Returns false if the point is invalid in the target coordinate system (and therefore target_point2d should not be used)
    pub fn convert_3d_to_2d(
        &self,
        source_point3d: &k4a_float3_t,
        source_camera: k4a_calibration_type_t,
        target_camera: k4a_calibration_type_t,
    ) -> Result<(k4a_float2_t, bool), Error> {
        let mut target_point2d = k4a_float2_t::default();
        let mut valid: i32 = 0;
        Error::from((self.factory.k4a_calibration_3d_to_2d)(
            &self.calibration,
            source_point3d,
            source_camera,
            target_camera,
            &mut target_point2d,
            &mut valid,
        ))
        .to_result((target_point2d, valid != 0))
    }

    /// Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera
    /// Returns false if the point is invalid in the target coordinate system (and therefore target_point2d should not be used)
    pub fn convert_2d_to_2d(
        &self,
        source_point2d: &k4a_float2_t,
        source_depth: f32,
        source_camera: k4a_calibration_type_t,
        target_camera: k4a_calibration_type_t,
    ) -> Result<(k4a_float2_t, bool), Error> {
        let mut target_point2d = k4a_float2_t::default();
        let mut valid: i32 = 0;
        Error::from((self.factory.k4a_calibration_2d_to_2d)(
            &self.calibration,
            source_point2d,
            source_depth,
            source_camera,
            target_camera,
            &mut target_point2d,
            &mut valid,
        ))
        .to_result((target_point2d, valid != 0))
    }

    /// Transform a 2D pixel coordinate from color camera into a 2D pixel coordinate of the depth camera. This function
    /// searches along an epipolar line in the depth image to find the corresponding depth pixel.
    /// Returns false if the point is invalid in the target coordinate system (and therefore target_point2d should not be used)
    pub fn convert_color_2d_to_depth_2d(
        &self,
        source_point2d: &k4a_float2_t,
        depth_image: &Image,
    ) -> Result<(k4a_float2_t, bool), Error> {
        let mut target_point2d = k4a_float2_t::default();
        let mut valid: i32 = 0;
        Error::from((self.factory.k4a_calibration_color_2d_to_depth_2d)(
            &self.calibration,
            source_point2d,
            depth_image.handle,
            &mut target_point2d,
            &mut valid,
        ))
        .to_result((target_point2d, valid != 0))
    }
}