qualia_vision/biosense/respiration/
respiration_rate_from_motion_trace.rs1use 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
13pub 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 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 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
102pub 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}