Skip to main content

qualia_vision/biosense/magnification/
temporal_bandpass_iir.rs

1//! Second-order IIR band-pass on a scalar time series (cold/Tier-2 OK for Vec-free caller buffers).
2
3use crate::cv::error::CvError;
4
5/// One sample of a biquad band-pass (Direct Form I, simplified RBJ-style coefficients).
6#[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    /// Design band-pass around geometric centre of [f_lo, f_hi] at `fps`.
21    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    /// Processes.
52    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
64/// Filter entire trace into `out` (same length).
65pub 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; // in 0.7–4
92        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}