realsense_rust/processing_blocks/
threshold.rs

1//! Processing block for thresholding depth data
2//!
3//! By controlling min and max options on the block, one could filter out depth values
4//! that are either too large or too small, as a software post-processing step
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, OptionsExt, ProcessingBlockOptionError, ThresholdOptions,
14        },
15    },
16};
17use anyhow::Result;
18use realsense_sys as sys;
19use std::{convert::TryFrom, ptr::NonNull, task::Poll, time::Duration};
20
21/// Creates depth thresholding processing block
22/// By controlling min and max options on the block, one could filter out depth values
23/// that are either too large or too small, as a software post-processing step
24#[derive(Debug, Clone)]
25pub struct ThresholdFilter {
26    /// The processing block for depth thresholding
27    processing_block: NonNull<sys::rs2_processing_block>,
28    /// The frame queue upon which the processing block will deposit filtered frames
29    processing_queue: NonNull<sys::rs2_frame_queue>,
30}
31
32impl Drop for ThresholdFilter {
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 ThresholdFilter {
42    /// Create a new ThresholdFilter processing block
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_threshold(&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    /// Process a depth frame with thresholding
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 thresholded results
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
93            let filtered_frame =
94                sys::rs2_wait_for_frame(self.processing_queue.as_ptr(), timeout_millis, &mut err);
95            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
96            Ok(DepthFrame::try_from(NonNull::new(filtered_frame).unwrap()).unwrap())
97        }
98    }
99
100    /// Poll to receive the thresholded results
101    pub fn poll(&mut self) -> Result<Poll<DepthFrame>, ProcessFrameError> {
102        unsafe {
103            let mut err = std::ptr::null_mut::<sys::rs2_error>();
104            let mut frame = std::ptr::null_mut::<sys::rs2_frame>();
105            let is_ready =
106                sys::rs2_poll_for_frame(self.processing_queue.as_ptr(), &mut frame, &mut err);
107
108            // Check for errors
109            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
110
111            // Check for queue readiness
112            if is_ready == 0 {
113                Ok(Poll::Pending)
114            } else {
115                Ok(Poll::Ready(
116                    DepthFrame::try_from(NonNull::new(frame).unwrap()).unwrap(),
117                ))
118            }
119        }
120    }
121
122    /// Apply options to configure the threshold filter
123    pub fn apply_options(
124        &mut self,
125        options: &ThresholdOptions,
126    ) -> Result<(), ProcessingBlockOptionError> {
127        if let Some(min_distance) = options.min_distance {
128            self.set_option(sys::rs2_option_RS2_OPTION_MIN_DISTANCE, min_distance)?;
129        }
130        if let Some(max_distance) = options.max_distance {
131            self.set_option(sys::rs2_option_RS2_OPTION_MAX_DISTANCE, max_distance)?;
132        }
133        Ok(())
134    }
135}
136
137impl OptionsExt for ThresholdFilter {
138    fn set_option(
139        &mut self,
140        option: sys::rs2_option,
141        value: f32,
142    ) -> Result<(), ProcessingBlockOptionError> {
143        set_processing_block_option(self.processing_block, option, value)
144    }
145
146    fn get_option(&self, option: sys::rs2_option) -> Result<f32, ProcessingBlockOptionError> {
147        get_processing_block_option(self.processing_block, option)
148    }
149
150    fn supports_option(&self, option: sys::rs2_option) -> bool {
151        unsafe {
152            let mut err = std::ptr::null_mut::<sys::rs2_error>();
153            let is_supported = sys::rs2_supports_option(
154                self.processing_block.as_ptr() as *const sys::rs2_options,
155                option,
156                &mut err,
157            );
158            err.is_null() && is_supported != 0
159        }
160    }
161
162    fn get_option_range(
163        &self,
164        option: sys::rs2_option,
165    ) -> Result<(f32, f32, f32, f32), ProcessingBlockOptionError> {
166        get_processing_block_option_range(self.processing_block, option)
167    }
168}
realsense_rust/processing_blocks/threshold.rs

realsense_rust/processing_blocks/
threshold.rs
