qualia_vision/biosense/magnification/
temporal_bandpass.rs1use crate::cv::error::CvError;
7
8#[derive(Debug, Clone, Copy)]
10pub struct LowpassAlpha {
11 pub alpha: f32,
12}
13
14#[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
24pub 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 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
53pub 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#[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
84pub 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 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; 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 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 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 assert!(rms > 0.5, "rms={rms}");
148 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()); }
157}