Skip to main content

qualia_vision/biosense/rppg/
spectral_hr_peak.rs

1//! Peak frequency in BPM band via naive DFT power.
2
3use crate::cv::error::CvError;
4
5#[derive(Debug, Clone, Copy)]
6pub struct HrEstimate {
7    pub bpm: f32,
8    pub snr: f32,
9    pub confidence: f32,
10}
11
12/// Estimate HR from pulse trace. `fps` sample rate. Band 40–180 BPM.
13pub fn spectral_hr_peak(trace: &[f32], fps: f32) -> Result<HrEstimate, CvError> {
14    let n = trace.len();
15    if n < 16 || fps <= 1.0 {
16        return Err(CvError::InvalidParameter);
17    }
18    // Demean
19    let mean = trace.iter().sum::<f32>() / n as f32;
20    let mut best_f = 1.0f32;
21    let mut best_p = 0.0f32;
22    let mut total = 0.0f32;
23    let f_lo = 40.0 / 60.0;
24    let f_hi = 180.0 / 60.0;
25    let steps = 64usize;
26    for k in 0..steps {
27        let f = f_lo + (f_hi - f_lo) * (k as f32 / steps as f32);
28        let mut re = 0.0f32;
29        let mut im = 0.0f32;
30        for (i, &v) in trace.iter().enumerate() {
31            let t = i as f32 / fps;
32            let ang = core::f32::consts::TAU * f * t;
33            re += (v - mean) * ang.cos();
34            im += (v - mean) * ang.sin();
35        }
36        let p = re * re + im * im;
37        total += p;
38        if p > best_p {
39            best_p = p;
40            best_f = f;
41        }
42    }
43    let snr = if total > 1e-6 {
44        best_p / (total / steps as f32).max(1e-6)
45    } else {
46        0.0
47    };
48    let conf = (snr / 20.0).clamp(0.0, 1.0);
49    Ok(HrEstimate {
50        bpm: best_f * 60.0,
51        snr,
52        confidence: conf,
53    })
54}
55
56#[cfg(test)]
57mod tests {
58    use super::*;
59    #[test]
60    fn finds_60bpm() {
61        let fps = 30.0f32;
62        let n = 150;
63        let mut t = vec![0.0f32; n];
64        for i in 0..n {
65            t[i] = (core::f32::consts::TAU * 1.0 * i as f32 / fps).sin();
66        }
67        let e = spectral_hr_peak(&t, fps).unwrap();
68        assert!((e.bpm - 60.0).abs() < 8.0, "bpm={}", e.bpm);
69    }
70}