From 13015c9d368d9e892a909d23b5737ffa55976f19 Mon Sep 17 00:00:00 2001 From: rUv Date: Mon, 27 Jul 2026 08:46:58 -0700 Subject: [PATCH] fix(vitals): HeartRateExtractor weights=[] silent None + BreathingExtractor stale-lock recovery (#1422, #1423) (#1449) Fixes #1422, Fixes #1423. HeartRateExtractor no longer truncates subcarrier count on empty phases; BreathingExtractor resets immediately on first out-of-band rejection instead of passively draining a stale window (recovery 28.6s -> 6.0s). 64 tests passing, clippy clean, zero regressions workspace-wide. --- python/src/bindings/vitals.rs | 13 +- python/tests/test_vitals.py | 32 +++ .../wifi-densepose-vitals/src/breathing.rs | 259 +++++++++++++++++- .../wifi-densepose-vitals/src/heartrate.rs | 136 ++++++++- 4 files changed, 420 insertions(+), 20 deletions(-) diff --git a/python/src/bindings/vitals.rs b/python/src/bindings/vitals.rs index c30844a0..2d840922 100644 --- a/python/src/bindings/vitals.rs +++ b/python/src/bindings/vitals.rs @@ -252,8 +252,9 @@ impl PyBreathingExtractor { /// hr = HeartRateExtractor.esp32_default() # 56 subcarriers, 100 Hz, 15s window /// /// # Feed residuals and matching unwrapped phases from your preprocessor. -/// # Unlike BreathingExtractor weights, phases=[] is invalid for heart-rate -/// # extraction because the Rust core requires phase data for each subcarrier. +/// # Like BreathingExtractor's weights, phases=[] means "no per-subcarrier +/// # coherence information available" and falls back to equal weighting +/// # across all subcarriers -- it does NOT silently drop every frame. /// est = hr.extract(residuals=[0.01, -0.02, …], phases=[0.0, 0.01, …]) /// if est is not None: /// print(est.value_bpm, est.confidence) @@ -281,9 +282,11 @@ impl PyHeartRateExtractor { } /// Extract heart rate from per-subcarrier residuals and matching - /// per-subcarrier unwrapped phases (radians). Empty phases are invalid - /// and return `None` because the Rust extractor requires phase data. - /// GIL released during DSP. + /// per-subcarrier unwrapped phases (radians). A short or empty `phases` + /// slice falls back to equal weighting for any subcarrier missing phase + /// data (issue #1423) -- it does not truncate the number of subcarriers + /// fused, and does not silently return `None` for every frame. GIL + /// released during DSP. fn extract( &mut self, py: Python<'_>, diff --git a/python/tests/test_vitals.py b/python/tests/test_vitals.py index 942d2960..d4ddac4e 100644 --- a/python/tests/test_vitals.py +++ b/python/tests/test_vitals.py @@ -223,6 +223,38 @@ def test_heart_rate_extract_with_synthetic_signal_and_phases() -> None: ) +def test_heart_rate_extract_with_empty_phases_produces_estimates() -> None: + """Issue #1423 regression: `phases=[]` must fall back to equal + weighting (mirroring `BreathingExtractor`'s `weights=[]`), not silently + return `None` for every frame. + + Reproduces the GH-issue repro: a noiseless 1.2 Hz (72 BPM) sine + identical across all 56 subcarriers, fed frame-by-frame with an empty + `phases` list. Before the fix this produced 0/4000 estimates; the same + signal with `phases=[1.0] * 56` already produced thousands. + """ + hr = HeartRateExtractor.esp32_default() + sample_rate = 100.0 + target_freq = 1.2 # 72 BPM + n_samples = 4000 + + produced = 0 + for i in range(n_samples): + t = i / sample_rate + base = math.sin(2.0 * math.pi * target_freq * t) + residuals = [base] * 56 + est = hr.extract(residuals=residuals, phases=[]) + if est is not None: + produced += 1 + assert math.isfinite(est.value_bpm) + assert 0.0 <= est.confidence <= 1.0 + + assert produced > 0, ( + "HeartRateExtractor.extract(residuals=..., phases=[]) must not silently " + "return None for every frame of a clean 72 BPM signal (issue #1423)" + ) + + # ─── Build feature flag ────────────────────────────────────────────── diff --git a/v2/crates/wifi-densepose-vitals/src/breathing.rs b/v2/crates/wifi-densepose-vitals/src/breathing.rs index 1bfd7cd4..8906307a 100644 --- a/v2/crates/wifi-densepose-vitals/src/breathing.rs +++ b/v2/crates/wifi-densepose-vitals/src/breathing.rs @@ -47,8 +47,29 @@ pub struct BreathingExtractor { freq_high: f64, /// IIR filter state. filter_state: IirState, + /// Count of consecutive `extract()` calls that rejected the estimated + /// frequency as out of the breathing band (i.e. "no periodic signal + /// detected"), since the last accepted estimate or reset. + /// + /// Without this, a subject leaving mid-lock only clears via the + /// `filtered_history` ring buffer passively flushing over the full + /// `window_secs` (up to 30s) — during which a stale, decreasingly + /// accurate estimate keeps being emitted, and any *new* subject + /// arriving mid-flush gets fused with leftover stale samples instead + /// of starting from a clean window (issue #1422). Once + /// `consecutive_rejections` reaches `STALE_RESET_REJECTIONS`, `reset()` + /// is called so the next accepted estimate is built from fresh data + /// only, instead of waiting out the old window. + consecutive_rejections: usize, } +/// Number of consecutive out-of-band rejections after which the sliding +/// window and filter state are cleared (ADR-157-adjacent fix, issue #1422). +/// One rejection is enough: once the estimated frequency has left the +/// breathing band, the same rejected estimate must never be echoed again as +/// a stale "lock" while the window slowly flushes over up to `window_secs`. +const STALE_RESET_REJECTIONS: usize = 1; + impl BreathingExtractor { /// Create a new breathing extractor. /// @@ -67,6 +88,7 @@ impl BreathingExtractor { freq_low: 0.1, freq_high: 0.5, filter_state: IirState::default(), + consecutive_rejections: 0, } } @@ -127,10 +149,29 @@ impl BreathingExtractor { let duration_s = history.len() as f64 / self.sample_rate; let frequency_hz = crossings as f64 / (2.0 * duration_s); - // Validate frequency is within the breathing band + // Validate frequency is within the breathing band. An out-of-band + // estimate means no periodic breathing signal was found in the + // current window (e.g. the subject left, or noise dominates). + // + // Without an active reset here, the stale `filtered_history` window + // only clears by passively flushing over the full `window_secs` + // (up to 30s) as new samples evict old ones. During that flush a + // transiently *accepted* estimate can keep climbing toward + // `freq_high` before the frequency finally leaves the band (issue + // #1422) — and once rejected, any real signal that resumes would + // otherwise have to wait out the rest of that stale window before + // it can dominate a fresh, accurate estimate again. Resetting on + // rejection makes both directions fast: reject-and-forget instead + // of reject-then-linger, and reacquire-from-scratch instead of + // reacquire-diluted-by-ghosts. if frequency_hz < self.freq_low || frequency_hz > self.freq_high { + self.consecutive_rejections += 1; + if self.consecutive_rejections >= STALE_RESET_REJECTIONS { + self.reset(); + } return None; } + self.consecutive_rejections = 0; let bpm = frequency_hz * 60.0; let confidence = compute_confidence(history); @@ -200,6 +241,7 @@ impl BreathingExtractor { pub fn reset(&mut self) { self.filtered_history.clear(); self.filter_state = IirState::default(); + self.consecutive_rejections = 0; } /// Current number of samples in the history buffer. @@ -479,6 +521,197 @@ mod tests { ); } + /// Deterministic small PRNG (LCG) for reproducible synthetic-signal + /// tests -- mirrors the style already used in + /// `heartrate::tests::pure_noise_is_never_reported_valid`. Returns a + /// value in roughly `[-1, 1)`. + fn lcg_next(seed: &mut u64) -> f64 { + *seed = seed + .wrapping_mul(6_364_136_223_846_793_005) + .wrapping_add(1_442_695_040_888_963_407); + ((*seed >> 33) as f64 / (1u64 << 31) as f64) - 1.0 + } + + /// Issue #1422 bug-catching test. + /// + /// Reproduces the reported scenario at ESP32 defaults (56 subcarriers, + /// 100 Hz, 30s window): 60s of a real 0.25 Hz (15 BPM) signal with + /// per-subcarrier gain/phase (lock-on), then broadband noise-floor-only + /// residuals (the "empty room"). + /// + /// `extract()` already returned `None` once the estimated frequency left + /// the breathing band (that part was not silently broken) -- the actual + /// defect was that the 30s `filtered_history` window only cleared by + /// *passively* flushing sample-by-sample, so a locked-on extractor kept + /// re-fusing whatever noise trickled in with a shrinking-but-still-large + /// fraction of stale real signal, letting a decreasingly-accurate + /// estimate keep being accepted right up to the range ceiling (30 BPM -- + /// the exact value from the GH issue) before it finally rejected. Once + /// rejected, the *next* real subject would then have to wait out the + /// rest of that same stale window before a fresh, undiluted estimate + /// could dominate again (see `issue_1422_recovery_after_dropout_is_fast` + /// for that half of the regression). + /// + /// This test pins the fix at its source: the very first out-of-band + /// rejection after a real signal disappears must actively clear + /// `filtered_history` (not wait for it to passively drain), so no + /// stale majority-real-signal window can ever linger and re-validate a + /// ghost estimate near the ceiling. + #[test] + fn issue_1422_stale_lock_does_not_persist_after_subject_leaves() { + let n = 56usize; + let fs = 100.0; + let weights = vec![1.0f64; n]; + let mut seed: u64 = 0x1422_1422; + let gain: Vec = (0..n).map(|_| 0.4 + 0.6 * lcg_next(&mut seed).abs()).collect(); + let phase: Vec = (0..n) + .map(|_| lcg_next(&mut seed).abs() * 2.0 * std::f64::consts::PI) + .collect(); + let mut ext = BreathingExtractor::esp32_default(); + + // 60s of a strong, real 15 BPM (0.25 Hz) breathing signal -- lock on. + let mut got_valid_lock = false; + for i in 0..6000usize { + let t = i as f64 / fs; + let residuals: Vec = (0..n) + .map(|c| { + 0.6 * gain[c] * (2.0 * std::f64::consts::PI * 0.25 * t + phase[c]).sin() + + lcg_next(&mut seed) * 0.05 + }) + .collect(); + if let Some(est) = ext.extract(&residuals, &weights) { + assert!( + (est.value_bpm - 15.0).abs() < 5.0, + "should track ~15 BPM while the subject is present, got {}", + est.value_bpm + ); + got_valid_lock = true; + } + } + assert!(got_valid_lock, "extractor must lock onto the real breathing signal first"); + assert!( + ext.history_len() > 0, + "a locked-on extractor must carry a non-empty window into the empty-room phase" + ); + + // Empty room: feed noise-floor-only residuals one at a time until the + // *first* rejection (the first `None` here can only come from the + // frequency-band check, never "insufficient history", since the + // window is already far past `min_samples` from the lock-on phase). + let mut first_reject_at: Option = None; + let mut history_len_at_reject: Option = None; + for i in 0..12000usize { + let residuals: Vec = (0..n).map(|_| lcg_next(&mut seed) * 0.05).collect(); + let outcome = ext.extract(&residuals, &weights); + if outcome.is_none() { + first_reject_at = Some(i); + history_len_at_reject = Some(ext.history_len()); + break; + } + } + let first_reject_at = + first_reject_at.expect("pure noise must eventually be rejected as out of band"); + + // The fix: the window must be actively cleared in the *same* call + // that rejected the frequency -- not left to drain passively over + // the remaining ~30s - first_reject_at samples. Before the fix, + // `history_len()` here was still the full ~3000-sample stale window. + assert_eq!( + history_len_at_reject, + Some(0), + "the first out-of-band rejection (at sample {first_reject_at}) must reset the \ + history window immediately, not leave the stale lock-on window in place \ + (issue #1422)", + ); + + // And the window must not be allowed to silently regrow back into a + // large, majority-noise "lock" while noise keeps arriving: each + // rebuild-to-`min_samples` cycle must itself reject and reset, so + // `history_len()` never creeps back up toward the full window. + let min_samples = (fs * 10.0) as usize; + let mut max_history_len_after_reject = 0usize; + for _ in 0..(12000 - first_reject_at - 1) { + let residuals: Vec = (0..n).map(|_| lcg_next(&mut seed) * 0.05).collect(); + ext.extract(&residuals, &weights); + max_history_len_after_reject = max_history_len_after_reject.max(ext.history_len()); + } + assert!( + max_history_len_after_reject <= min_samples, + "history window regrew to {max_history_len_after_reject} samples while fed pure \ + noise -- a stale majority-noise window should never be allowed to accumulate \ + past the minimum warm-up size without being rejected and reset (issue #1422)", + ); + + // Finally: the extractor must be silent at the very end of the long + // empty-room period, not just momentarily quiet. + let final_residuals: Vec = (0..n).map(|_| lcg_next(&mut seed) * 0.05).collect(); + assert!( + ext.extract(&final_residuals, &weights).is_none(), + "BreathingExtractor must report no signal at the end of a long empty-room period", + ); + } + + /// Issue #1422 companion regression: once the subject leaves (and the + /// extractor has rejected/reset), a *returning* subject must be + /// reacquired quickly -- not have to wait out the full stale 30s window + /// passively flushing via FIFO eviction, which is what produced the + /// reported "stayed at 30.0 BPM for roughly another 30s before starting + /// to track again" secondary symptom. + #[test] + fn issue_1422_recovery_after_dropout_is_fast() { + let n = 56usize; + let fs = 100.0; + let weights = vec![1.0f64; n]; + let mut seed: u64 = 0xFEED_1422; + let gain: Vec = (0..n).map(|_| 0.4 + 0.6 * lcg_next(&mut seed).abs()).collect(); + let phase: Vec = (0..n) + .map(|_| lcg_next(&mut seed).abs() * 2.0 * std::f64::consts::PI) + .collect(); + let mut ext = BreathingExtractor::esp32_default(); + + let signal_residuals = |t: f64, seed: &mut u64| -> Vec { + (0..n) + .map(|c| { + 0.6 * gain[c] * (2.0 * std::f64::consts::PI * 0.25 * t + phase[c]).sin() + + lcg_next(seed) * 0.05 + }) + .collect() + }; + let noise_residuals = |seed: &mut u64| -> Vec { + (0..n).map(|_| lcg_next(seed) * 0.05).collect() + }; + + // Lock on. + for i in 0..6000usize { + ext.extract(&signal_residuals(i as f64 / fs, &mut seed), &weights); + } + // Long empty-room period. + for _ in 0..6000usize { + ext.extract(&noise_residuals(&mut seed), &weights); + } + // Subject returns. + let mut recovered_at = None; + for i in 0..6000usize { + let t = (12000 + i) as f64 / fs; + if ext.extract(&signal_residuals(t, &mut seed), &weights).is_some() { + recovered_at = Some(i); + break; + } + } + + let recovered_at = recovered_at.expect("extractor must reacquire the returning subject"); + // A passive-flush-only window (pre-fix) needs on the order of the + // full 30s window to dilute stale noise (measured ~29s); the active + // reset-on-rejection fix reacquires close to the 10s minimum warm-up + // instead (measured ~6s). Generous bound: well under half the window. + assert!( + recovered_at < 1500, + "recovery after a dropout took {recovered_at} samples (~{:.1}s) -- expected fast \ + reacquisition (issue #1422 secondary symptom: slow recovery after a transient)", + recovered_at as f64 / fs, + ); + } + /// ADR-157 §A3 bug-catching test. Divergence needs the pole magnitude /// `|r| >= 1`, i.e. `bw >= 4`. At `fs = 0.5` Hz with the band widened to /// 0.1-0.9 Hz, `bw = 2*pi*(0.9-0.1)/0.5 = 10.05`, so the OLD pole radius @@ -493,12 +726,28 @@ mod tests { ext.freq_high = 0.9; // Feed a unit step for 600 frames — enough for the un-clamped resonator // to overflow to inf. + // + // A constant unit step has essentially no periodic content once the + // resonator settles, so with the issue #1422 reset-on-rejection fix + // this can legitimately cycle `filtered_history` back to empty + // between checks (build up to `min_samples`, get rejected as + // out-of-band, reset, rebuild...). That's an intentional, separate + // behavior change -- this test's actual purpose (ADR-157 §A3) is the + // *filter's* numerical stability under extreme parameters, so it + // tracks the max history length reached and checks finiteness on + // every iteration instead of asserting a nonzero count only at the + // very end. + let mut max_history_len = 0usize; for _ in 0..600 { ext.extract(&[1.0, 1.0, 1.0, 1.0], &[0.25, 0.25, 0.25, 0.25]); + max_history_len = max_history_len.max(ext.history_len()); + for (i, &v) in ext.filtered_history.iter().enumerate() { + assert!(v.is_finite(), "filtered_history[{i}] must be finite, got {v}"); + } } - assert!(ext.history_len() > 0, "history should accumulate"); - for (i, &v) in ext.filtered_history.iter().enumerate() { - assert!(v.is_finite(), "filtered_history[{i}] must be finite, got {v}"); - } + assert!( + max_history_len > 0, + "history should have accumulated samples at some point during the run" + ); } } diff --git a/v2/crates/wifi-densepose-vitals/src/heartrate.rs b/v2/crates/wifi-densepose-vitals/src/heartrate.rs index 924fd2f2..4b2cd350 100644 --- a/v2/crates/wifi-densepose-vitals/src/heartrate.rs +++ b/v2/crates/wifi-densepose-vitals/src/heartrate.rs @@ -98,7 +98,17 @@ impl HeartRateExtractor { /// Returns a `VitalEstimate` with heart rate in BPM, or `None` /// if insufficient data or too few subcarriers. pub fn extract(&mut self, residuals: &[f64], phases: &[f64]) -> Option { - let n = residuals.len().min(self.n_subcarriers).min(phases.len()); + // `n` is driven by `residuals` (and the subcarrier cap) only, NOT by + // `phases.len()`. Before this fix, a missing/short `phases` slice + // (e.g. `phases=[]`, documented as meaning "equal weighting", mirroring + // `BreathingExtractor`'s `weights=[]`) truncated `n` down to + // `phases.len()`, so `phases=[]` forced `n == 0` and `extract()` + // silently returned `None` for every frame (issue #1423). Missing + // phase entries are now treated by `compute_phase_coherence_signal` + // as "no coherence information available" and fused with equal + // weight, exactly like `breathing::fuse_weighted_residuals`'s + // uniform-weight fallback for a missing/partial `weights` slice. + let n = residuals.len().min(self.n_subcarriers); if n == 0 { return None; } @@ -249,29 +259,44 @@ impl HeartRateExtractor { /// Combines amplitude residuals with inter-subcarrier phase coherence /// to enhance the cardiac signal. Subcarriers with similar phase /// derivatives are likely sensing the same body surface. +/// +/// `phases` may be shorter than `n` (including empty). Missing entries mean +/// the caller has no per-subcarrier phase measurement to weight by, so that +/// subcarrier is fused with full coherence (weight 1.0) instead of being +/// excluded -- i.e. `phases=[]` degrades gracefully to equal weighting +/// across all `n` residuals, exactly like `breathing::fuse_weighted_residuals` +/// falling back to a uniform weight for a missing/partial `weights` slice +/// (issue #1423; `phases=[]` is documented as meaning equal weights, the +/// same as `BreathingExtractor`'s `weights=[]`). fn compute_phase_coherence_signal(residuals: &[f64], phases: &[f64], n: usize) -> f64 { if n <= 1 { return residuals.first().copied().unwrap_or(0.0); } + let phase_at = |i: usize| phases.get(i).copied(); + // Compute inter-subcarrier phase differences as coherence weights. // Adjacent subcarriers with small phase differences are more coherent. + // If either phase value needed for a pair is missing, treat the pair as + // fully coherent (weight 1.0) rather than panicking or excluding it. let mut weighted_sum = 0.0; let mut weight_total = 0.0; - for i in 0..n { - let coherence = if i + 1 < n { - let phase_diff = (phases[i + 1] - phases[i]).abs(); - // Higher coherence when phase difference is small - (-phase_diff).exp() + for (i, &r) in residuals.iter().enumerate().take(n) { + let neighbor = if i + 1 < n { + Some(i + 1) } else if i > 0 { - let phase_diff = (phases[i] - phases[i - 1]).abs(); - (-phase_diff).exp() + Some(i - 1) } else { - 1.0 + None }; - weighted_sum += residuals[i] * coherence; + let coherence = match neighbor.and_then(|j| phase_at(i).zip(phase_at(j))) { + Some((a, b)) => (-(b - a).abs()).exp(), + None => 1.0, + }; + + weighted_sum += r * coherence; weight_total += coherence; } @@ -561,4 +586,95 @@ mod tests { assert!(v.is_finite(), "filtered_history[{i}] must be finite, got {v}"); } } + + /// Issue #1423 bug-catching test. + /// + /// Reproduces the exact GH-issue repro: 56 subcarriers @ 100 Hz (ESP32 + /// defaults), a noiseless 1.2 Hz (72 BPM) sine identical across every + /// subcarrier, fed frame-by-frame with an **empty** `phases` slice. + /// + /// Before the fix, `extract()`'s `n` was + /// `residuals.len().min(n_subcarriers).min(phases.len())`, so + /// `phases = []` forced `n == 0` and every single frame returned `None` + /// (0/4000 estimates) even though the identical call with + /// `phases = [1.0; 56]` produced thousands of valid estimates. `phases=[]` + /// must mean "no coherence weighting available", i.e. equal weighting -- + /// the same documented fallback `BreathingExtractor` already honors for + /// `weights=[]` -- not "invalid input, refuse everything". + #[test] + fn issue_1423_empty_phases_yields_equal_weighting_not_silent_none() { + let n = 56usize; + let sample_rate = 100.0; + let heart_freq = 1.2; // 72 BPM + + let mut ext = HeartRateExtractor::esp32_default(); + let mut estimates_with_empty_phases = 0usize; + for i in 0..4000usize { + let t = i as f64 / sample_rate; + let base = (2.0 * std::f64::consts::PI * heart_freq * t).sin(); + let residuals = vec![base; n]; + if ext.extract(&residuals, &[]).is_some() { + estimates_with_empty_phases += 1; + } + } + + assert!( + estimates_with_empty_phases > 0, + "HeartRateExtractor::extract() with phases=[] must not silently return None for \ + every frame of a clean 72 BPM signal (issue #1423); got 0/4000 estimates", + ); + + // Sanity check against the issue's own comparison point: an + // equivalent uniform, non-empty `phases` slice (all subcarriers at + // the same phase, i.e. zero pairwise difference => full coherence, + // exactly what the equal-weighting fallback now produces for the + // empty case too) must yield a comparable number of estimates -- + // the two should behave the same, not `0` vs `thousands`. + let mut ext2 = HeartRateExtractor::esp32_default(); + let uniform_phases = vec![1.0_f64; n]; + let mut estimates_with_uniform_phases = 0usize; + for i in 0..4000usize { + let t = i as f64 / sample_rate; + let base = (2.0 * std::f64::consts::PI * heart_freq * t).sin(); + let residuals = vec![base; n]; + if ext2.extract(&residuals, &uniform_phases).is_some() { + estimates_with_uniform_phases += 1; + } + } + + assert_eq!( + estimates_with_empty_phases, estimates_with_uniform_phases, + "phases=[] (equal-weight fallback) must behave identically to a uniform, \ + non-empty phases slice of the same length -- both represent 'no differential \ + coherence information', got {estimates_with_empty_phases} vs \ + {estimates_with_uniform_phases}", + ); + } + + /// Issue #1423 companion: a `phases` slice *shorter* than `residuals` + /// (partial coverage) must fall back to equal weighting for the missing + /// tail rather than truncating the whole fusion down to the phases that + /// happen to be present -- mirrors + /// `breathing::partial_weights_are_renormalized_not_scale_mixed`. + #[test] + fn partial_phases_do_not_truncate_subcarrier_count() { + // 8 residuals, only 2 phase values supplied. + let residuals = [1.0_f64; 8]; + let phases = [0.0_f64, 0.0]; + let fused = compute_phase_coherence_signal(&residuals, &phases, 8); + + // All residuals are identical (1.0), so regardless of how coherence + // weights are distributed the fused value must still be 1.0 -- but + // this only holds if all 8 residuals are actually used. Before the + // fix, `n` would have been truncated to `phases.len() == 2`, so the + // caller-facing `extract()` never even reached this function with + // `n == 8`; this test pins `compute_phase_coherence_signal` itself + // to handle a short `phases` slice safely and correctly when called + // with the full `n`. + assert!( + (fused - 1.0).abs() < 1e-12, + "fusion with a partial phases slice must still average all {} residuals, got {fused}", + residuals.len(), + ); + } }