qualia_vision/biosense/magnification/
temporal_bandpass_iir.rs1use crate::cv::error::CvError;
4
5#[derive(Debug, Clone, Copy)]
7pub struct BandpassState {
8 pub x1: f32,
9 pub x2: f32,
10 pub y1: f32,
11 pub y2: f32,
12 pub b0: f32,
13 pub b1: f32,
14 pub b2: f32,
15 pub a1: f32,
16 pub a2: f32,
17}
18
19impl BandpassState {
20 pub fn design(fps: f32, f_lo: f32, f_hi: f32) -> Result<Self, CvError> {
22 if fps <= 1.0 || f_lo <= 0.0 || f_hi <= f_lo || f_hi >= fps * 0.45 {
23 return Err(CvError::InvalidParameter);
24 }
25 let f0 = (f_lo * f_hi).sqrt();
26 let bw = f_hi - f_lo;
27 let q = (f0 / bw).clamp(0.3, 10.0);
28 let w0 = core::f32::consts::TAU * f0 / fps;
29 let alpha = w0.sin() / (2.0 * q);
30 let cosw = w0.cos();
31 let b0 = alpha;
32 let b1 = 0.0;
33 let b2 = -alpha;
34 let a0 = 1.0 + alpha;
35 let a1 = -2.0 * cosw;
36 let a2 = 1.0 - alpha;
37 Ok(Self {
38 x1: 0.0,
39 x2: 0.0,
40 y1: 0.0,
41 y2: 0.0,
42 b0: b0 / a0,
43 b1: b1 / a0,
44 b2: b2 / a0,
45 a1: a1 / a0,
46 a2: a2 / a0,
47 })
48 }
49
50 #[inline]
51 pub fn process(&mut self, x: f32) -> f32 {
53 let y = self.b0 * x + self.b1 * self.x1 + self.b2 * self.x2
54 - self.a1 * self.y1
55 - self.a2 * self.y2;
56 self.x2 = self.x1;
57 self.x1 = x;
58 self.y2 = self.y1;
59 self.y1 = y;
60 y
61 }
62}
63
64pub fn temporal_bandpass_iir(
66 input: &[f32],
67 fps: f32,
68 f_lo: f32,
69 f_hi: f32,
70 out: &mut [f32],
71) -> Result<(), CvError> {
72 if input.len() != out.len() || input.len() < 8 {
73 return Err(CvError::BufferTooSmall);
74 }
75 let mut st = BandpassState::design(fps, f_lo, f_hi)?;
76 for i in 0..input.len() {
77 out[i] = st.process(input[i]);
78 }
79 Ok(())
80}
81
82#[cfg(test)]
83mod tests {
84 use super::*;
85
86 #[test]
87 fn passes_in_band_sinusoid() {
88 let fps = 30.0f32;
89 let n = 256;
90 let mut x = vec![0.0f32; n];
91 let f = 1.2f32; for i in 0..n {
93 x[i] = (core::f32::consts::TAU * f * (i as f32 / fps)).sin();
94 }
95 let mut y = vec![0.0f32; n];
96 temporal_bandpass_iir(&x, fps, 0.7, 4.0, &mut y).unwrap();
97 let power: f32 = y.iter().map(|v| v * v).sum();
98 assert!(power > 1.0, "power={}", power);
99 }
100}