vzense_rust/util/
touch_detector.rs

1//! A simple touch detector based on depth data.
2
3use std::iter::zip;
4
5use super::new_fixed_vec;
6
7/// To allow invocation of generic devices from different APIs.
8pub trait Data {
9    fn get_frame_p_frame_data(&self) -> *mut u8;
10    fn get_frame_data_len(&self) -> usize;
11    fn get_min_depth_mm(&self) -> u16;
12    fn get_max_depth_mm(&self) -> u16;
13    fn current_frame_is_depth(&self) -> bool;
14}
15
16/**
17The touch detector uses depth data to calculate the difference between the current depth and an initially recorded baseline depth. If this difference is between `min_touch` and `max_touch` a touch is assumed.
18
19* `min_depth` and `max_depth` are the measuring ranges of the depth camera in mm.
20* `min_touch` is the minimum height in mm above the surface considered to be a touch. If this parameter is too small, noise will lead to a lot of false detections.
21* `max_touch` is the maximum height in mm above the surface considered to be a touch.
22
23First, an average baseline depth is computed using the first `baseline_sample_size` frames. The current depth is estimated by a moving average of `sample_size` frames.
24*/
25pub struct TouchDetector {
26    min_depth: u16,
27    max_depth: u16,
28    min_touch: f32,
29    max_touch: f32,
30    pixel_count: usize,
31    baseline_sample_size: usize,
32    sample_size: usize,
33    baseline_sample: usize,
34    sample: usize,
35    baseline_depth_sum: Vec<u32>,
36    depth_sum: Vec<u32>,
37    ring_buffer: Vec<u16>,
38}
39impl TouchDetector {
40    /// Creates a new instance with the specified parameters. All length parameters are in mm.
41    pub fn new<Device: Data>(
42        device: &Device,
43        min_touch: f32,
44        max_touch: f32,
45        baseline_sample_size: usize,
46        sample_size: usize,
47        pixel_count: usize,
48    ) -> Self {
49        Self {
50            min_depth: device.get_min_depth_mm(),
51            max_depth: device.get_max_depth_mm(),
52            min_touch,
53            max_touch,
54            pixel_count,
55            baseline_sample_size,
56            sample_size,
57            baseline_sample: 0,
58            sample: 0,
59            baseline_depth_sum: new_fixed_vec(pixel_count, 0u32),
60            depth_sum: new_fixed_vec(pixel_count, 0u32),
61            ring_buffer: new_fixed_vec(sample_size * pixel_count, 0u16),
62        }
63    }
64
65    /// Processes a depth frame resulting in a `touch_signal` (255 for "touch", 0 otherwise) and a `distance` from the initially measured depth in mm.
66    ///
67    /// **Note**: This function does nothing if the current frame in device is not a depth frame. Call `get_depth_mm_u16_frame()` or `get_depth_scaled_u8_frame()` before calling `process()`.
68    pub fn process<Device: Data>(
69        &mut self,
70        device: &Device,
71        touch_signal: &mut [u8],
72        distance: &mut [f32],
73    ) {
74        // check if current frame holds a depth frame
75        if device.current_frame_is_depth() {
76            unsafe {
77                let p = match std::ptr::slice_from_raw_parts(
78                    device.get_frame_p_frame_data(),
79                    device.get_frame_data_len(),
80                )
81                .as_ref()
82                {
83                    Some(ptr) => ptr,
84                    None => return,
85                };
86
87                for (i, pi) in p.chunks_exact(2).enumerate() {
88                    // create one u16 from two consecutive u8 and clamp to measuring range
89                    let depth_mm =
90                        u16::from_le_bytes([pi[0], pi[1]]).clamp(self.min_depth, self.max_depth);
91
92                    // create baseline by averaging over first baseline_sample_size frames
93                    if self.baseline_sample < self.baseline_sample_size {
94                        self.baseline_depth_sum[i] += depth_mm as u32;
95                    }
96
97                    // pixel index of current sample in ring buffer
98                    let j = self.pixel_count * self.sample + i;
99
100                    // subtract old depth value in ring buffer from depth sum
101                    self.depth_sum[i] -= self.ring_buffer[j] as u32;
102
103                    // set ring buffer to new depth value and add it to depth sum
104                    self.ring_buffer[j] = depth_mm;
105                    self.depth_sum[i] += depth_mm as u32;
106
107                    let diff = self.baseline_depth_sum[i] as f32 / self.baseline_sample_size as f32
108                        - self.depth_sum[i] as f32 / self.sample_size as f32;
109
110                    touch_signal[i] = if self.min_touch < diff && diff < self.max_touch {
111                        255
112                    } else {
113                        0
114                    };
115
116                    distance[i] = diff;
117                }
118            }
119            self.sample = (self.sample + 1) % self.sample_size;
120            if self.baseline_sample < self.baseline_sample_size {
121                self.baseline_sample += 1;
122            }
123        }
124    }
125
126    /// A by-product, returning only the normalized moving average of the depth.
127    pub fn get_normalized_average_depth(&self, average_depth: &mut [u8]) {
128        for (adi, dsi) in zip(average_depth, self.depth_sum.as_slice()) {
129            let d = *dsi as f32 / self.sample_size as f32;
130
131            *adi = ((d - self.min_depth as f32) * 255.0 / (self.max_depth - self.min_depth) as f32)
132                .floor() as u8;
133        }
134    }
135
136    /// The baseline depth as the average of the first `baseline_sample_size` frames.
137    pub fn get_baseline(&self) -> Vec<f32> {
138        let mut base_line = new_fixed_vec(self.baseline_depth_sum.len(), 0.0);
139        for (a, b) in zip(self.baseline_depth_sum.as_slice(), base_line.as_mut_slice()) {
140            *b = *a as f32 / self.baseline_sample_size as f32;
141        }
142        base_line
143    }
144}
vzense_rust/util/touch_detector.rs

vzense_rust/util/
touch_detector.rs
