realsense_rust/frame/
points.rs

1//! Type for representing a Points frame.
2//!
3//! A Points frame is a RealSense point cloud storage class.
4
5use super::prelude::{CouldNotGetFrameSensorError, FrameCategory, FrameConstructionError, FrameEx};
6use crate::{
7    check_rs2_error,
8    kind::{Rs2Extension, Rs2FrameMetadata, Rs2StreamKind, Rs2TimestampDomain},
9    sensor::Sensor,
10    stream_profile::StreamProfile,
11};
12use anyhow::Result;
13use num_traits::FromPrimitive;
14use realsense_sys as sys;
15use std::{
16    convert::TryInto,
17    ptr::{self, NonNull},
18    slice,
19};
20
21/// Holds the raw data pointer and derived data for an RS2 Points frame.
22///
23/// All fields in this struct are initialized during struct creation (via `try_from`).
24/// Everything called from here during runtime should be valid as long as the
25/// Frame is in scope... like normal Rust.
26#[derive(Debug)]
27pub struct PointsFrame {
28    /// The raw data pointer from the original rs2 frame.
29    frame_ptr: NonNull<sys::rs2_frame>,
30    /// The timestamp of the frame.
31    timestamp: f64,
32    /// The RealSense time domain from which the timestamp is derived.
33    timestamp_domain: Rs2TimestampDomain,
34    /// The frame number.
35    frame_number: u64,
36    /// The Stream Profile that created the frame.
37    frame_stream_profile: StreamProfile,
38    /// The number of points represented in the Points frame.
39    num_points: usize,
40    /// The raw pointer to the vertex data.
41    vertices_data_ptr: NonNull<sys::rs2_vertex>,
42    /// The raw pointer to the texture data.
43    texture_data_ptr: NonNull<sys::rs2_pixel>,
44    /// A boolean used during `Drop` calls. This allows for proper handling of the pointer
45    /// during ownership transfer.
46    should_drop: bool,
47}
48
49impl FrameCategory for PointsFrame {
50    fn extension() -> Rs2Extension {
51        Rs2Extension::Points
52    }
53
54    fn kind() -> Rs2StreamKind {
55        Rs2StreamKind::Any
56    }
57
58    fn has_correct_kind(&self) -> bool {
59        self.frame_stream_profile.kind() == Self::kind()
60    }
61}
62
63impl FrameEx for PointsFrame {
64    fn stream_profile(&self) -> &StreamProfile {
65        &self.frame_stream_profile
66    }
67
68    fn sensor(&self) -> Result<Sensor> {
69        unsafe {
70            let mut err = std::ptr::null_mut::<sys::rs2_error>();
71            let sensor_ptr = sys::rs2_get_frame_sensor(self.frame_ptr.as_ptr(), &mut err);
72            check_rs2_error!(err, CouldNotGetFrameSensorError)?;
73
74            Ok(Sensor::from(NonNull::new(sensor_ptr).unwrap()))
75        }
76    }
77
78    fn timestamp(&self) -> f64 {
79        self.timestamp
80    }
81
82    fn timestamp_domain(&self) -> Rs2TimestampDomain {
83        self.timestamp_domain
84    }
85
86    fn frame_number(&self) -> u64 {
87        self.frame_number
88    }
89
90    fn metadata(&self, metadata_kind: Rs2FrameMetadata) -> Option<std::os::raw::c_longlong> {
91        if !self.supports_metadata(metadata_kind) {
92            return None;
93        }
94
95        unsafe {
96            let mut err = std::ptr::null_mut::<sys::rs2_error>();
97
98            let val = sys::rs2_get_frame_metadata(
99                self.frame_ptr.as_ptr(),
100                #[allow(clippy::useless_conversion)]
101                (metadata_kind as i32).try_into().unwrap(),
102                &mut err,
103            );
104
105            if err.as_ref().is_none() {
106                Some(val)
107            } else {
108                sys::rs2_free_error(err);
109                None
110            }
111        }
112    }
113
114    fn supports_metadata(&self, metadata_kind: Rs2FrameMetadata) -> bool {
115        unsafe {
116            let mut err = std::ptr::null_mut::<sys::rs2_error>();
117
118            let supports_metadata = sys::rs2_supports_frame_metadata(
119                self.frame_ptr.as_ptr(),
120                #[allow(clippy::useless_conversion)]
121                (metadata_kind as i32).try_into().unwrap(),
122                &mut err,
123            );
124
125            if err.as_ref().is_none() {
126                supports_metadata != 0
127            } else {
128                sys::rs2_free_error(err);
129                false
130            }
131        }
132    }
133
134    unsafe fn get_owned_raw(mut self) -> NonNull<sys::rs2_frame> {
135        self.should_drop = false;
136
137        self.frame_ptr
138    }
139}
140
141impl Drop for PointsFrame {
142    /// Drop the raw pointer stored with this struct whenever it goes out of scope.
143    fn drop(&mut self) {
144        unsafe {
145            if self.should_drop {
146                // Note: Vertices and Texture pointer lifetimes are managed by the
147                // frame itself, so dropping the frame should suffice.
148                sys::rs2_release_frame(self.frame_ptr.as_ptr());
149            }
150        }
151    }
152}
153
154unsafe impl Send for PointsFrame {}
155
156impl std::convert::TryFrom<NonNull<sys::rs2_frame>> for PointsFrame {
157    type Error = anyhow::Error;
158
159    /// Attempt to construct a points frame from a raw pointer to `rs2_frame`
160    ///
161    /// All members of the `PointsFrame` struct are validated and populated during this call.
162    ///
163    /// # Errors
164    ///
165    /// There are a number of errors that may occur if the data in the `rs2_frame` is not valid, all
166    /// of type [`FrameConstructionError`].
167    ///
168    /// - [`CouldNotGetTimestamp`](FrameConstructionError::CouldNotGetTimestamp)
169    /// - [`CouldNotGetTimestampDomain`](FrameConstructionError::CouldNotGetTimestampDomain)
170    /// - [`CouldNotGetFrameStreamProfile`](FrameConstructionError::CouldNotGetFrameStreamProfile)
171    /// - [`CouldNotGetPointCount`](FrameConstructionError::CouldNotGetPointCount)
172    /// - [`CouldNotGetData`](FrameConstructionError::CouldNotGetData)
173    ///
174    /// See [`FrameConstructionError`] documentation for more details.
175    ///
176    fn try_from(frame_ptr: NonNull<sys::rs2_frame>) -> Result<Self, Self::Error> {
177        unsafe {
178            let mut err = ptr::null_mut::<sys::rs2_error>();
179
180            let timestamp = sys::rs2_get_frame_timestamp(frame_ptr.as_ptr(), &mut err);
181            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestamp)?;
182
183            let timestamp_domain =
184                sys::rs2_get_frame_timestamp_domain(frame_ptr.as_ptr(), &mut err);
185            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestampDomain)?;
186
187            let frame_number = sys::rs2_get_frame_number(frame_ptr.as_ptr(), &mut err);
188            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameNumber)?;
189
190            let profile_ptr = sys::rs2_get_frame_stream_profile(frame_ptr.as_ptr(), &mut err);
191            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameStreamProfile)?;
192
193            let nonnull_profile_ptr =
194                NonNull::new(profile_ptr as *mut sys::rs2_stream_profile).unwrap();
195            let profile = StreamProfile::try_from(nonnull_profile_ptr)?;
196
197            let num_points = sys::rs2_get_frame_points_count(frame_ptr.as_ptr(), &mut err);
198            check_rs2_error!(err, FrameConstructionError::CouldNotGetPointCount)?;
199
200            let vertices_ptr = sys::rs2_get_frame_vertices(frame_ptr.as_ptr(), &mut err);
201            check_rs2_error!(err, FrameConstructionError::CouldNotGetData)?;
202
203            let texture_ptr = sys::rs2_get_frame_texture_coordinates(frame_ptr.as_ptr(), &mut err);
204            check_rs2_error!(err, FrameConstructionError::CouldNotGetData)?;
205
206            Ok(PointsFrame {
207                frame_ptr,
208                timestamp,
209                timestamp_domain: Rs2TimestampDomain::from_i32(timestamp_domain as i32).unwrap(),
210                frame_number,
211                frame_stream_profile: profile,
212                num_points: num_points as usize,
213                vertices_data_ptr: NonNull::new(vertices_ptr).unwrap(),
214                texture_data_ptr: NonNull::new(texture_ptr).unwrap(),
215                should_drop: true,
216            })
217        }
218    }
219}
220
221impl PointsFrame {
222    /// Gets vertices of the point cloud.
223    pub fn vertices(&self) -> &[sys::rs2_vertex] {
224        unsafe {
225            slice::from_raw_parts::<sys::rs2_vertex>(
226                self.vertices_data_ptr.as_ptr(),
227                self.num_points,
228            )
229        }
230    }
231
232    /// Retrieve the texture coordinates (uv map) for the point cloud.
233    ///
234    /// # Safety
235    ///
236    /// The librealsense2 C++ API directly casts the `rs2_pixel*` returned from
237    /// `rs2_get_frame_texture_coordinates()` into a `texture_coordinate*`, thereby re-interpreting
238    /// `[[c_int; 2]; N]` as `[[c_float; 2]; N]` values.  Note that C does not generally guarantee
239    /// that `sizeof(int) == sizeof(float)`.
240    ///
241    pub fn texture_coordinates(&self) -> &[[f32; 2]] {
242        unsafe {
243            slice::from_raw_parts::<[f32; 2]>(
244                self.texture_data_ptr.as_ptr().cast::<[f32; 2]>(),
245                self.num_points,
246            )
247        }
248    }
249
250    /// Gets number of points in the point cloud.
251    pub fn points_count(&self) -> usize {
252        self.num_points
253    }
254}
255
256#[cfg(test)]
257mod tests {
258    use super::*;
259
260    #[test]
261    fn frame_has_correct_kind() {
262        assert_eq!(PointsFrame::kind(), Rs2StreamKind::Any);
263    }
264}
realsense_rust/frame/points.rs

realsense_rust/frame/
points.rs
