realsense_rust/processing_blocks/
decimation.rs

1//! Processing block that decimates the depth stream
2//!
3//! Based on an example here:
4//! <https://github.com/IntelRealSense/librealsense/blob/4673a37d981164af8eeb8e296e430fc1427e008d/doc/post-processing-filters.md?plain=1#L111>
5
6use crate::{
7    check_rs2_error,
8    frame::{DepthFrame, FrameEx},
9    processing_blocks::{
10        errors::{ProcessFrameError, ProcessingBlockConstructionError},
11        options::{
12            get_processing_block_option, get_processing_block_option_range,
13            set_processing_block_option, DecimationOptions, OptionsExt, ProcessingBlockOptionError,
14        },
15    },
16};
17use anyhow::Result;
18use realsense_sys as sys;
19use std::{convert::TryFrom, ptr::NonNull, task::Poll, time::Duration};
20
21/// Applies decimation filter and plots modified params. Note that due to the modified frame size,
22/// the decimated frame replaces the original one.
23#[derive(Debug, Clone)]
24pub struct Decimation {
25    /// The processing block for the "Decimation" method
26    processing_block: NonNull<sys::rs2_processing_block>,
27    /// The frame queue upon which the processing block will deposit decimated frames. We check this
28    /// for completed block operations.
29    processing_queue: NonNull<sys::rs2_frame_queue>,
30}
31
32impl Drop for Decimation {
33    fn drop(&mut self) {
34        unsafe {
35            sys::rs2_delete_frame_queue(self.processing_queue.as_ptr());
36            sys::rs2_delete_processing_block(self.processing_block.as_ptr());
37        }
38    }
39}
40
41impl Decimation {
42    /// Create a new Decimation object
43    pub fn new(processing_queue_size: i32) -> Result<Self, ProcessingBlockConstructionError> {
44        let (processing_block, processing_queue) = unsafe {
45            let mut err = std::ptr::null_mut::<sys::rs2_error>();
46
47            let ptr = sys::rs2_create_decimation_filter_block(&mut err);
48            check_rs2_error!(
49                err,
50                ProcessingBlockConstructionError::CouldNotCreateProcessingBlock
51            )?;
52
53            let queue_ptr = sys::rs2_create_frame_queue(processing_queue_size, &mut err);
54            check_rs2_error!(
55                err,
56                ProcessingBlockConstructionError::CouldNotCreateProcessingQueue
57            )?;
58
59            sys::rs2_start_processing_queue(ptr, queue_ptr, &mut err);
60            check_rs2_error!(
61                err,
62                ProcessingBlockConstructionError::CouldNotStartProcessingQueue
63            )?;
64            (NonNull::new(ptr).unwrap(), NonNull::new(queue_ptr).unwrap())
65        };
66
67        Ok(Self {
68            processing_block,
69            processing_queue,
70        })
71    }
72
73    /// Own and process the composite frame and return the decimated frames.
74    pub fn queue(&mut self, frame: DepthFrame) -> Result<(), ProcessFrameError> {
75        unsafe {
76            let mut err = std::ptr::null_mut::<sys::rs2_error>();
77            sys::rs2_process_frame(
78                self.processing_block.as_ptr(),
79                frame.get_owned_raw().as_ptr(),
80                &mut err,
81            );
82            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
83            Ok(())
84        }
85    }
86
87    /// Wait to receive the results of the processing block
88    pub fn wait(&mut self, timeout: Duration) -> Result<DepthFrame, ProcessFrameError> {
89        unsafe {
90            let mut err = std::ptr::null_mut::<sys::rs2_error>();
91            let timeout_millis = u32::try_from(timeout.as_millis()).unwrap_or(u32::MAX);
92            let decimated_frame =
93                sys::rs2_wait_for_frame(self.processing_queue.as_ptr(), timeout_millis, &mut err);
94            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
95            Ok(DepthFrame::try_from(NonNull::new(decimated_frame).unwrap()).unwrap())
96        }
97    }
98
99    /// Poll to receive the results of the processing block
100    pub fn poll(&mut self) -> Result<Poll<DepthFrame>, ProcessFrameError> {
101        unsafe {
102            let mut err = std::ptr::null_mut::<sys::rs2_error>();
103            let mut frame = std::ptr::null_mut::<sys::rs2_frame>();
104            let is_ready =
105                sys::rs2_poll_for_frame(self.processing_queue.as_ptr(), &mut frame, &mut err);
106
107            // Check for errors
108            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
109
110            // Check for queue readiness
111            if is_ready == 0 {
112                Ok(Poll::Pending)
113            } else {
114                Ok(Poll::Ready(
115                    DepthFrame::try_from(NonNull::new(frame).unwrap()).unwrap(),
116                ))
117            }
118        }
119    }
120
121    /// Apply options to configure the decimation filter
122    pub fn apply_options(
123        &mut self,
124        options: &DecimationOptions,
125    ) -> Result<(), ProcessingBlockOptionError> {
126        if let Some(filter_magnitude) = options.filter_magnitude {
127            self.set_option(
128                sys::rs2_option_RS2_OPTION_FILTER_MAGNITUDE,
129                filter_magnitude,
130            )?;
131        }
132        Ok(())
133    }
134}
135
136impl OptionsExt for Decimation {
137    fn set_option(
138        &mut self,
139        option: sys::rs2_option,
140        value: f32,
141    ) -> Result<(), ProcessingBlockOptionError> {
142        set_processing_block_option(self.processing_block, option, value)
143    }
144
145    fn get_option(&self, option: sys::rs2_option) -> Result<f32, ProcessingBlockOptionError> {
146        get_processing_block_option(self.processing_block, option)
147    }
148
149    fn supports_option(&self, option: sys::rs2_option) -> bool {
150        unsafe {
151            let mut err = std::ptr::null_mut::<sys::rs2_error>();
152            let is_supported = sys::rs2_supports_option(
153                self.processing_block.as_ptr() as *const sys::rs2_options,
154                option,
155                &mut err,
156            );
157            err.is_null() && is_supported != 0
158        }
159    }
160
161    fn get_option_range(
162        &self,
163        option: sys::rs2_option,
164    ) -> Result<(f32, f32, f32, f32), ProcessingBlockOptionError> {
165        get_processing_block_option_range(self.processing_block, option)
166    }
167}
realsense_rust/processing_blocks/decimation.rs

realsense_rust/processing_blocks/
decimation.rs
