Skip to main content

qualia_vision/biosense/magnification/
temporal_bandpass.rs

1//! Temporal band-pass (difference of two 1st-order IIR low-pass filters).
2//!
3//! Explicit `fps`, `f_lo_hz`, `f_hi_hz` (e.g. 0.7–4 Hz for resting HR band).
4//! Zero heap: state is either stack (scalar series) or caller-supplied per-sample state.
5
6use crate::cv::error::CvError;
7
8/// Coefficients for a first-order low-pass: y += α (x − y).
9#[derive(Debug, Clone, Copy)]
10pub struct LowpassAlpha {
11    pub alpha: f32,
12}
13
14/// Band-pass as high_lp − low_lp (Wu-style IIR band).
15#[derive(Debug, Clone, Copy)]
16pub struct BandpassIir {
17    pub lo: LowpassAlpha,
18    pub hi: LowpassAlpha,
19    pub f_lo_hz: f32,
20    pub f_hi_hz: f32,
21    pub fps: f32,
22}
23
24/// Design IIR band-pass from sample rate and cut-offs.
25///
26/// Requires `0 < f_lo_hz < f_hi_hz < fps/2` (Nyquist).
27pub fn design_bandpass_iir(fps: f32, f_lo_hz: f32, f_hi_hz: f32) -> Result<BandpassIir, CvError> {
28    if !(fps > 1.0) || !(f_lo_hz > 0.0) || !(f_hi_hz > f_lo_hz) {
29        return Err(CvError::InvalidParameter);
30    }
31    let nyq = fps * 0.5;
32    if f_hi_hz >= nyq {
33        return Err(CvError::InvalidParameter);
34    }
35    // α ≈ 1 − exp(−2π f_c / fps) for a mild 1-pole LP
36    let alpha_for = |fc: f32| -> f32 {
37        let a = 1.0 - (-core::f32::consts::TAU * fc / fps).exp();
38        a.clamp(1e-6, 1.0)
39    };
40    Ok(BandpassIir {
41        lo: LowpassAlpha {
42            alpha: alpha_for(f_lo_hz),
43        },
44        hi: LowpassAlpha {
45            alpha: alpha_for(f_hi_hz),
46        },
47        f_lo_hz,
48        f_hi_hz,
49        fps,
50    })
51}
52
53/// Filter a scalar series: `out[i] = lp_hi[i] − lp_lo[i]`.
54/// DC and frequencies below `f_lo` / above `f_hi` are attenuated.
55pub fn temporal_bandpass_series(
56    samples: &[f32],
57    bp: &BandpassIir,
58    out: &mut [f32],
59) -> Result<(), CvError> {
60    if samples.is_empty() {
61        return Err(CvError::EmptyInput);
62    }
63    if out.len() < samples.len() {
64        return Err(CvError::BufferTooSmall);
65    }
66    let mut y_lo = samples[0];
67    let mut y_hi = samples[0];
68    for (i, &x) in samples.iter().enumerate() {
69        y_lo += bp.lo.alpha * (x - y_lo);
70        y_hi += bp.hi.alpha * (x - y_hi);
71        out[i] = y_hi - y_lo;
72    }
73    Ok(())
74}
75
76/// One sample step for a multi-pixel path. Caller holds `state_lo` / `state_hi`.
77#[inline]
78pub fn bandpass_step(x: f32, bp: &BandpassIir, state_lo: &mut f32, state_hi: &mut f32) -> f32 {
79    *state_lo += bp.lo.alpha * (x - *state_lo);
80    *state_hi += bp.hi.alpha * (x - *state_hi);
81    *state_hi - *state_lo
82}
83
84/// Band-pass every temporal series along frame axis for a packed buffer.
85///
86/// Layout: `frames` length ≥ `n_frames * plane_elems`, frame-major.
87/// `state_lo` / `state_hi` each length ≥ `plane_elems` (initialized to first frame).
88pub fn temporal_bandpass_planes(
89    frames: &[f32],
90    n_frames: usize,
91    plane_elems: usize,
92    bp: &BandpassIir,
93    state_lo: &mut [f32],
94    state_hi: &mut [f32],
95    out: &mut [f32],
96) -> Result<(), CvError> {
97    if n_frames == 0 || plane_elems == 0 {
98        return Err(CvError::InvalidParameter);
99    }
100    let need = n_frames * plane_elems;
101    if frames.len() < need
102        || out.len() < need
103        || state_lo.len() < plane_elems
104        || state_hi.len() < plane_elems
105    {
106        return Err(CvError::BufferTooSmall);
107    }
108    // Init states from frame 0
109    state_lo[..plane_elems].copy_from_slice(&frames[..plane_elems]);
110    state_hi[..plane_elems].copy_from_slice(&frames[..plane_elems]);
111    for t in 0..n_frames {
112        let base = t * plane_elems;
113        for p in 0..plane_elems {
114            let x = frames[base + p];
115            out[base + p] = bandpass_step(x, bp, &mut state_lo[p], &mut state_hi[p]);
116        }
117    }
118    Ok(())
119}
120
121#[cfg(test)]
122mod tests {
123    use super::*;
124
125    #[test]
126    fn bandpass_passes_sinusoid_rejects_dc() {
127        let fps = 30.0f32;
128        let f_sig = 1.5f32; // inside 0.7–4
129        let bp = design_bandpass_iir(fps, 0.7, 4.0).unwrap();
130        let n = 180;
131        let mut sig = vec![0.0f32; n];
132        for i in 0..n {
133            // DC + in-band + high-freq noise
134            let t = i as f32 / fps;
135            sig[i] = 50.0
136                + 3.0 * (core::f32::consts::TAU * f_sig * t).sin()
137                + 2.0 * (core::f32::consts::TAU * 12.0 * t).sin();
138        }
139        let mut out = vec![0.0f32; n];
140        temporal_bandpass_series(&sig, &bp, &mut out).unwrap();
141        // Skip IIR transient
142        let body = &out[60..];
143        let mean: f32 = body.iter().sum::<f32>() / body.len() as f32;
144        let rms: f32 =
145            (body.iter().map(|v| (v - mean) * (v - mean)).sum::<f32>() / body.len() as f32).sqrt();
146        // Band energy should be non-trivial (sinusoid preserved)
147        assert!(rms > 0.5, "rms={rms}");
148        // Mean of band-passed should be near 0 (DC rejected)
149        assert!(mean.abs() < 0.5, "mean={mean}");
150    }
151
152    #[test]
153    fn design_rejects_bad_band() {
154        assert!(design_bandpass_iir(30.0, 4.0, 0.7).is_err());
155        assert!(design_bandpass_iir(30.0, 0.7, 20.0).is_err()); // above Nyquist
156    }
157}