realsense_rust/frame/
pose.rs

1//! Type for representing a pose frame taken from an IMU or pose-sensor.
2//!
3//! A Pose frame holds information describing the motion and position of a device
4//! at a point in time. See the member and function declarations for how these values are stored
5//! and retrieved.
6
7use super::prelude::{CouldNotGetFrameSensorError, FrameCategory, FrameConstructionError, FrameEx};
8use crate::{
9    check_rs2_error,
10    kind::{Rs2Extension, Rs2FrameMetadata, Rs2StreamKind, Rs2TimestampDomain},
11    sensor::Sensor,
12    stream_profile::StreamProfile,
13};
14use anyhow::Result;
15use num_traits::FromPrimitive;
16use realsense_sys as sys;
17use std::{
18    convert::{TryFrom, TryInto},
19    mem::MaybeUninit,
20    ptr::{self, NonNull},
21};
22
23/// Holds information describing the motion and position of a device at a point in time.
24#[derive(Debug)]
25pub struct PoseFrame {
26    /// The raw data pointer from the original rs2 frame.
27    frame_ptr: NonNull<sys::rs2_frame>,
28    /// The timestamp of the frame.
29    timestamp: f64,
30    /// The RealSense time domain from which the timestamp is derived.
31    timestamp_domain: Rs2TimestampDomain,
32    /// The frame number.
33    frame_number: u64,
34    /// The Stream Profile that created the frame.
35    frame_stream_profile: StreamProfile,
36    /// The rs2 Pose data
37    data: sys::rs2_pose,
38    /// A boolean used during `Drop` calls. This allows for proper handling of the pointer
39    /// during ownership transfer.
40    should_drop: bool,
41}
42
43/// Used by the tracker and mapper to estimate the certainty in this pose.
44pub enum Confidence {
45    /// The tracker/mapper has failed. This information is probably not reliable.
46    Failed,
47    /// The tracker/mapper confidence is low.
48    Low,
49    /// The tracker/mapper confidence is marginal.
50    Medium,
51    /// The tracker/mapper confidence is high.
52    High,
53}
54
55impl PoseFrame {
56    /// X, Y, Z values of translation, in meters (relative to initial position)
57    pub fn translation(&self) -> [f32; 3] {
58        let sys::rs2_vector { x, y, z } = self.data.translation;
59        [x, y, z]
60    }
61
62    /// X, Y, Z values of velocity, in meters/sec
63    pub fn velocity(&self) -> [f32; 3] {
64        let sys::rs2_vector { x, y, z } = self.data.velocity;
65        [x, y, z]
66    }
67
68    /// X, Y, Z values of acceleration, in meters/sec^2
69    pub fn acceleration(&self) -> [f32; 3] {
70        let sys::rs2_vector { x, y, z } = self.data.acceleration;
71        [x, y, z]
72    }
73
74    /// Qi, Qj, Qk, Qr components of rotation as represented in quaternion rotation (relative to initial position)
75    pub fn rotation(&self) -> [f32; 4] {
76        let sys::rs2_quaternion { x, y, z, w } = self.data.rotation;
77        [x, y, z, w]
78    }
79
80    /// X, Y, Z values of angular velocity, in radians/sec
81    pub fn angular_velocity(&self) -> [f32; 3] {
82        let sys::rs2_vector { x, y, z } = self.data.angular_velocity;
83        [x, y, z]
84    }
85
86    /// X, Y, Z values of angular acceleration, in radians/sec^2
87    pub fn angular_acceleration(&self) -> [f32; 3] {
88        let sys::rs2_vector { x, y, z } = self.data.angular_acceleration;
89        [x, y, z]
90    }
91
92    /// Pose confidence from [`Confidence::Failed`] to [`Confidence::High`]
93    pub fn tracker_confidence(&self) -> Confidence {
94        match self.data.tracker_confidence {
95            0x0 => Confidence::Failed,
96            0x1 => Confidence::Low,
97            0x2 => Confidence::Medium,
98            0x3 => Confidence::High,
99            _ => panic!("Unknown confidence, please report a bug!"),
100        }
101    }
102
103    /// Pose map confidence from [`Confidence::Failed`] to [`Confidence::High`]
104    pub fn mapper_confidence(&self) -> Confidence {
105        match self.data.tracker_confidence {
106            0x0 => Confidence::Failed,
107            0x1 => Confidence::Low,
108            0x2 => Confidence::Medium,
109            0x3 => Confidence::High,
110            _ => panic!("Unknown confidence, please report a bug!"),
111        }
112    }
113}
114
115impl Drop for PoseFrame {
116    /// Drop the raw pointer stored with this struct whenever it goes out of scope.
117    fn drop(&mut self) {
118        unsafe {
119            if self.should_drop {
120                sys::rs2_release_frame(self.frame_ptr.as_ptr());
121            }
122        }
123    }
124}
125
126unsafe impl Send for PoseFrame {}
127
128impl FrameCategory for PoseFrame {
129    fn extension() -> Rs2Extension {
130        Rs2Extension::PoseFrame
131    }
132
133    fn kind() -> Rs2StreamKind {
134        Rs2StreamKind::Pose
135    }
136
137    fn has_correct_kind(&self) -> bool {
138        self.frame_stream_profile.kind() == Self::kind()
139    }
140}
141
142impl TryFrom<NonNull<sys::rs2_frame>> for PoseFrame {
143    type Error = anyhow::Error;
144
145    /// Attempt to construct a PoseFrame from the raw pointer to `rs2_frame`
146    ///
147    /// All members of the `PoseFrame` struct are validated and populated during this call.
148    ///
149    /// # Errors
150    ///
151    /// There are a number of errors that may occur if the data in the `rs2_frame` is not valid, all
152    /// of type [`FrameConstructionError`].
153    ///
154    /// - [`CouldNotGetTimestamp`](FrameConstructionError::CouldNotGetTimestamp)
155    /// - [`CouldNotGetTimestampDomain`](FrameConstructionError::CouldNotGetTimestampDomain)
156    /// - [`CouldNotGetFrameStreamProfile`](FrameConstructionError::CouldNotGetFrameStreamProfile)
157    /// - [`CouldNotGetData`](FrameConstructionError::CouldNotGetData)
158    ///
159    /// See [`FrameConstructionError`] documentation for more details.
160    ///
161    fn try_from(frame_ptr: NonNull<sys::rs2_frame>) -> Result<Self, Self::Error> {
162        unsafe {
163            let mut err = ptr::null_mut::<sys::rs2_error>();
164
165            let timestamp = sys::rs2_get_frame_timestamp(frame_ptr.as_ptr(), &mut err);
166            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestamp)?;
167
168            let timestamp_domain =
169                sys::rs2_get_frame_timestamp_domain(frame_ptr.as_ptr(), &mut err);
170            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestampDomain)?;
171
172            let frame_number = sys::rs2_get_frame_number(frame_ptr.as_ptr(), &mut err);
173            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameNumber)?;
174
175            let profile_ptr = sys::rs2_get_frame_stream_profile(frame_ptr.as_ptr(), &mut err);
176            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameStreamProfile)?;
177
178            let nonnull_profile_ptr =
179                NonNull::new(profile_ptr as *mut sys::rs2_stream_profile).unwrap();
180            let profile = StreamProfile::try_from(nonnull_profile_ptr)?;
181
182            let mut pose_data = MaybeUninit::uninit();
183            sys::rs2_pose_frame_get_pose_data(frame_ptr.as_ptr(), pose_data.as_mut_ptr(), &mut err);
184            check_rs2_error!(err, FrameConstructionError::CouldNotGetData)?;
185
186            Ok(PoseFrame {
187                frame_ptr,
188                timestamp,
189                timestamp_domain: Rs2TimestampDomain::from_i32(timestamp_domain as i32).unwrap(),
190                frame_number,
191                frame_stream_profile: profile,
192                data: pose_data.assume_init(),
193                should_drop: true,
194            })
195        }
196    }
197}
198
199impl FrameEx for PoseFrame {
200    fn stream_profile(&self) -> &StreamProfile {
201        &self.frame_stream_profile
202    }
203
204    fn sensor(&self) -> Result<Sensor> {
205        unsafe {
206            let mut err = std::ptr::null_mut::<sys::rs2_error>();
207            let sensor_ptr = sys::rs2_get_frame_sensor(self.frame_ptr.as_ptr(), &mut err);
208            check_rs2_error!(err, CouldNotGetFrameSensorError)?;
209
210            Ok(Sensor::from(NonNull::new(sensor_ptr).unwrap()))
211        }
212    }
213
214    fn timestamp(&self) -> f64 {
215        self.timestamp
216    }
217
218    fn timestamp_domain(&self) -> Rs2TimestampDomain {
219        self.timestamp_domain
220    }
221
222    fn frame_number(&self) -> u64 {
223        self.frame_number
224    }
225
226    fn metadata(&self, metadata_kind: Rs2FrameMetadata) -> Option<std::os::raw::c_longlong> {
227        if !self.supports_metadata(metadata_kind) {
228            return None;
229        }
230
231        unsafe {
232            let mut err = std::ptr::null_mut::<sys::rs2_error>();
233
234            let val = sys::rs2_get_frame_metadata(
235                self.frame_ptr.as_ptr(),
236                #[allow(clippy::useless_conversion)]
237                (metadata_kind as i32).try_into().unwrap(),
238                &mut err,
239            );
240
241            if err.as_ref().is_none() {
242                Some(val)
243            } else {
244                sys::rs2_free_error(err);
245                None
246            }
247        }
248    }
249
250    fn supports_metadata(&self, metadata_kind: Rs2FrameMetadata) -> bool {
251        unsafe {
252            let mut err = std::ptr::null_mut::<sys::rs2_error>();
253
254            let supports_metadata = sys::rs2_supports_frame_metadata(
255                self.frame_ptr.as_ptr(),
256                #[allow(clippy::useless_conversion)]
257                (metadata_kind as i32).try_into().unwrap(),
258                &mut err,
259            );
260
261            if err.as_ref().is_none() {
262                supports_metadata != 0
263            } else {
264                sys::rs2_free_error(err);
265                false
266            }
267        }
268    }
269
270    unsafe fn get_owned_raw(mut self) -> NonNull<sys::rs2_frame> {
271        self.should_drop = false;
272
273        self.frame_ptr
274    }
275}
276
277#[cfg(test)]
278mod tests {
279    use super::*;
280
281    #[test]
282    fn frame_has_correct_kind() {
283        assert_eq!(PoseFrame::kind(), Rs2StreamKind::Pose);
284    }
285}
realsense_rust/frame/pose.rs

realsense_rust/frame/
pose.rs
