Skip to main content

qualia_vision/biosense/respiration/
respiration_rate_from_motion_trace.rs

1//! Respiratory rate from a motion energy / vertical displacement trace.
2//!
3//! Pure spectral peak in the breath band (~0.1–0.5 Hz): demean → dense DFT
4//! power scan → residual-band SNR → optional parabolic refine → fail closed.
5//! No learned weights / training.
6
7use super::rr_estimate::{
8    snr_to_confidence, RrEstimate, RR_F_HI_HZ, RR_F_LO_HZ, RR_MIN_SAMPLES, RR_MIN_SNR_DEFAULT,
9    RR_SPECTRAL_STEPS,
10};
11use crate::cv::error::CvError;
12
13/// Core spectral peak search in the respiratory band. Fail closed on low SNR.
14///
15/// Shared by motion RR and rPPG low-frequency harmonic paths.
16pub fn spectral_rr_peak(trace: &[f32], fps: f32, min_snr: f32) -> Result<RrEstimate, CvError> {
17    let n = trace.len();
18    if n < RR_MIN_SAMPLES || fps <= 1.0 || !fps.is_finite() || min_snr < 0.0 {
19        return Err(CvError::InvalidParameter);
20    }
21    // At least ~1.5 cycles of the lowest band edge inside the window.
22    let duration = n as f32 / fps;
23    if duration < 1.5 / RR_F_LO_HZ {
24        return Err(CvError::InvalidParameter);
25    }
26
27    let mean = trace.iter().sum::<f32>() / n as f32;
28    let steps = RR_SPECTRAL_STEPS;
29    let mut powers = [0.0f32; RR_SPECTRAL_STEPS];
30    let mut best_k = 0usize;
31    let mut best_p = 0.0f32;
32    let mut total_p = 0.0f32;
33    let denom = (steps as f32 - 1.0).max(1.0);
34    let band = RR_F_HI_HZ - RR_F_LO_HZ;
35
36    for k in 0..steps {
37        let f = RR_F_LO_HZ + band * (k as f32 / denom);
38        let mut re = 0.0f32;
39        let mut im = 0.0f32;
40        for (i, &v) in trace.iter().enumerate() {
41            let t = i as f32 / fps;
42            let ang = core::f32::consts::TAU * f * t;
43            let x = v - mean;
44            re += x * ang.cos();
45            im += x * ang.sin();
46        }
47        let p = re * re + im * im;
48        powers[k] = p;
49        total_p += p;
50        if p > best_p {
51            best_p = p;
52            best_k = k;
53        }
54    }
55
56    if total_p < 1e-12 || best_p < 1e-12 {
57        return Err(CvError::InvalidParameter);
58    }
59
60    // Residual-band SNR: peak / mean of non-peak bins (honest vs peak/mean-all inflation).
61    let noise = ((total_p - best_p) / (steps as f32 - 1.0).max(1.0)).max(1e-12);
62    let snr = best_p / noise;
63    if snr < min_snr {
64        return Err(CvError::InvalidParameter);
65    }
66
67    let f_best = refined_freq_hz(best_k, &powers, steps, denom, band);
68    let conf = snr_to_confidence(snr)
69        .max(snr / (snr + 8.0))
70        .clamp(0.05, 0.95);
71
72    Ok(RrEstimate {
73        breaths_per_min: f_best * 60.0,
74        snr,
75        confidence: conf,
76    })
77}
78
79fn refined_freq_hz(
80    best_k: usize,
81    powers: &[f32; RR_SPECTRAL_STEPS],
82    steps: usize,
83    denom: f32,
84    band: f32,
85) -> f32 {
86    let f_of = |k: usize| RR_F_LO_HZ + band * (k as f32 / denom);
87    if best_k == 0 || best_k + 1 >= steps {
88        return f_of(best_k);
89    }
90    let p_l = powers[best_k - 1];
91    let p_0 = powers[best_k];
92    let p_r = powers[best_k + 1];
93    let den = p_l - 2.0 * p_0 + p_r;
94    if den.abs() < 1e-12 {
95        return f_of(best_k);
96    }
97    let delta = (0.5 * (p_l - p_r) / den).clamp(-1.0, 1.0);
98    let df = band / denom;
99    (f_of(best_k) + delta * df).clamp(RR_F_LO_HZ, RR_F_HI_HZ)
100}
101
102/// Estimate respiration (breaths/min) from per-frame scalar motion
103/// (chest ROI mean, vertical optical-flow energy, …).
104///
105/// `min_snr`: gate; pass `0.0` to use [`RR_MIN_SNR_DEFAULT`].
106pub fn respiration_rate_from_motion_trace(
107    trace: &[f32],
108    fps: f32,
109    min_snr: f32,
110) -> Result<RrEstimate, CvError> {
111    let gate = if min_snr > 0.0 {
112        min_snr
113    } else {
114        RR_MIN_SNR_DEFAULT
115    };
116    spectral_rr_peak(trace, fps, gate)
117}
118
119#[cfg(test)]
120mod tests {
121    use super::*;
122
123    fn synth_sinusoid(bpm: f32, fps: f32, n: usize, amp: f32, noise: f32) -> Vec<f32> {
124        let f = bpm / 60.0;
125        let mut t = vec![0.0f32; n];
126        for i in 0..n {
127            let pure = amp * (core::f32::consts::TAU * f * i as f32 / fps).sin();
128            let nbit = ((i.wrapping_mul(1103515245).wrapping_add(12345) >> 16) & 0x7fff) as f32
129                / 32768.0
130                - 0.5;
131            t[i] = pure + noise * nbit;
132        }
133        t
134    }
135
136    #[test]
137    fn finds_twelve_bpm() {
138        let fps = 30.0;
139        let t = synth_sinusoid(12.0, fps, 450, 1.0, 0.02);
140        let e = respiration_rate_from_motion_trace(&t, fps, 2.0).unwrap();
141        assert!(
142            (e.breaths_per_min - 12.0).abs() < 1.5,
143            "bpm={}",
144            e.breaths_per_min
145        );
146        assert!(e.confidence > 0.2);
147        assert!(e.snr > 2.0);
148    }
149
150    #[test]
151    fn finds_fifteen_bpm() {
152        let fps = 30.0f32;
153        let t = synth_sinusoid(15.0, fps, 450, 1.0, 0.02);
154        let e = respiration_rate_from_motion_trace(&t, fps, 2.0).unwrap();
155        assert!(
156            (e.breaths_per_min - 15.0).abs() < 1.5,
157            "bpm={}",
158            e.breaths_per_min
159        );
160        assert!(e.confidence > 0.2);
161    }
162
163    #[test]
164    fn finds_eighteen_and_twenty_bpm() {
165        let fps = 30.0;
166        for target in [18.0f32, 20.0] {
167            let t = synth_sinusoid(target, fps, 450, 1.0, 0.02);
168            let e = respiration_rate_from_motion_trace(&t, fps, 2.0).unwrap();
169            assert!(
170                (e.breaths_per_min - target).abs() < 1.8,
171                "target={} got={}",
172                target,
173                e.breaths_per_min
174            );
175        }
176    }
177
178    #[test]
179    fn noise_abstains() {
180        let tr = vec![0.01f32; 128];
181        let r = respiration_rate_from_motion_trace(&tr, 30.0, 50.0);
182        assert!(r.is_err());
183    }
184
185    #[test]
186    fn pure_noise_high_gate_fails_closed() {
187        let fps = 30.0;
188        let n = 450;
189        let mut t = vec![0.0f32; n];
190        for i in 0..n {
191            let nbit = ((i.wrapping_mul(1664525).wrapping_add(1013904223) >> 16) & 0x7fff) as f32
192                / 32768.0
193                - 0.5;
194            t[i] = nbit;
195        }
196        assert!(respiration_rate_from_motion_trace(&t, fps, 8.0).is_err());
197    }
198
199    #[test]
200    fn rejects_short_trace() {
201        let t = [0.1f32; 16];
202        assert!(respiration_rate_from_motion_trace(&t, 30.0, 0.0).is_err());
203    }
204
205    #[test]
206    fn rejects_bad_fps() {
207        let t = synth_sinusoid(15.0, 30.0, 200, 1.0, 0.0);
208        assert!(respiration_rate_from_motion_trace(&t, 0.5, 2.0).is_err());
209    }
210}