qualia_vision/biosense/rppg/
spectral_hr_peak.rs1use 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
12pub 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 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}