From 10684972d7280f446dce148f8c8e6897a80e5ed4 Mon Sep 17 00:00:00 2001 From: Akhilesh Arora Date: Sun, 17 May 2026 23:02:53 +0200 Subject: [PATCH] fix(vital_signs): use circular variance for wrapped phases (#595) process_frame computed arithmetic mean + variance on phase values from atan2(), which are wrapped to (-pi, pi]. Phases close across the +/-pi discontinuity produced ~pi^2 variance instead of ~1e-6, feeding wrap noise into the heart-rate FFT buffer. Replace inline math with a standard circular variance helper (1 - mean resultant length). Add 4 unit tests, one through the production path of process_frame. Closes #593 --- .../src/vital_signs.rs | 116 +++++++++++++++++- 1 file changed, 110 insertions(+), 6 deletions(-) diff --git a/v2/crates/wifi-densepose-sensing-server/src/vital_signs.rs b/v2/crates/wifi-densepose-sensing-server/src/vital_signs.rs index f5f2fb71..04ef6f77 100644 --- a/v2/crates/wifi-densepose-sensing-server/src/vital_signs.rs +++ b/v2/crates/wifi-densepose-sensing-server/src/vital_signs.rs @@ -139,13 +139,15 @@ impl VitalSignDetector { // Cardiac-induced body surface displacement is < 0.5 mm, producing // tiny phase changes. Cross-subcarrier phase variance captures this // more sensitively than amplitude alone. + // + // Phases come from atan2() and are wrapped to (-pi, pi]. Linear mean + // and variance on wrapped values is wrong: two phases close across + // the +/-pi discontinuity (e.g. pi-eps and -pi+eps) are physically + // ~2*eps rad apart but produce arithmetic variance ~pi^2. Use the + // standard circular variance (1 - mean resultant length), which is + // stable across the wrap. let phase_var = if phase.len() > 1 { - let mean_phase: f64 = phase.iter().sum::() / phase.len() as f64; - phase - .iter() - .map(|p| (p - mean_phase).powi(2)) - .sum::() - / phase.len() as f64 + phase_circular_variance(phase) } else { // Fallback: use amplitude high-pass residual when phase is unavailable let half = amplitude.len() / 2; @@ -367,6 +369,25 @@ impl VitalSignDetector { /// Constructs a bandpass by subtracting two lowpass filters (LPF_high - LPF_low) /// with a Hamming window. This is a zero-external-dependency implementation /// suitable for the buffer sizes we encounter (up to ~600 samples). +/// Circular variance of wrapped phase samples in radians. +/// +/// Returns `1 - R` where `R` is the mean resultant length of the unit-circle +/// representation of the input. `0.0` means all samples point in the same +/// direction; `1.0` means they are uniformly spread. Stable across the +/-pi +/// discontinuity of `atan2`-derived phases. Returns `0.0` for inputs shorter +/// than 2 samples. +pub(crate) fn phase_circular_variance(phase: &[f64]) -> f64 { + if phase.len() < 2 { + return 0.0; + } + let (sin_sum, cos_sum) = phase + .iter() + .fold((0.0_f64, 0.0_f64), |(s, c), &p| (s + p.sin(), c + p.cos())); + let n = phase.len() as f64; + let r = (sin_sum * sin_sum + cos_sum * cos_sum).sqrt() / n; + (1.0 - r).clamp(0.0, 1.0) +} + pub fn bandpass_filter(data: &[f64], low_hz: f64, high_hz: f64, sample_rate: f64) -> Vec { if data.len() < 3 || sample_rate < f64::EPSILON { return data.to_vec(); @@ -605,6 +626,89 @@ pub fn run_benchmark(n_frames: usize) -> (std::time::Duration, std::time::Durati mod tests { use super::*; + /// Regression test for the linear-vs-circular phase variance bug. + /// + /// Two CSI subcarriers whose phases land just either side of the +/-pi + /// wrap are physically ~2*eps rad apart, but the previous code computed + /// arithmetic mean + arithmetic variance on the wrapped values, treating + /// them as ~2*pi apart and producing variance ~pi^2 ~= 9.87. The corrected + /// circular variance returns ~1e-6 for the same input. + #[test] + fn test_phase_variance_handles_wraparound() { + // Two phases ~0.002 rad apart, straddling the +/-pi discontinuity. + let phases = [PI - 0.001, -PI + 0.001]; + + let v = phase_circular_variance(&phases); + assert!( + v < 0.01, + "circular variance of nearly-identical wrapped phases must be tiny, got {v}" + ); + + // For reference, the *old* (buggy) linear formula on the same input + // produced ~9.87. Document that gap here so the assertion above + // explicitly fails on any regression to the linear computation. + let mean_linear: f64 = phases.iter().sum::() / phases.len() as f64; + let v_linear_buggy: f64 = phases + .iter() + .map(|p| (p - mean_linear).powi(2)) + .sum::() + / phases.len() as f64; + assert!( + v_linear_buggy > 9.0, + "sanity: linear formula on wrapped input should be ~pi^2, got {v_linear_buggy}" + ); + } + + /// End-to-end: `process_frame` must not blow up `heartbeat_buffer` when + /// the input phases happen to straddle the +/-pi wrap. Anyone can run this + /// against `main` (with the inline linear-formula bug) and observe the + /// stored value of ~9.86; with the fix it is ~1e-6. + #[test] + fn test_process_frame_handles_wrapped_phases() { + let mut detector = VitalSignDetector::new(20.0); + let amp = vec![1.0_f64; 8]; + // 8 subcarriers, all physically ~aligned at ~+/-pi, alternating sign. + let phase = vec![ + PI - 0.001, -PI + 0.001, PI - 0.001, -PI + 0.001, + PI - 0.001, -PI + 0.001, PI - 0.001, -PI + 0.001, + ]; + + detector.process_frame(&, &phase); + + let stored = *detector + .heartbeat_buffer + .back() + .expect("process_frame should push exactly one phase_var"); + + assert!( + stored < 0.1, + "phase_var pushed into heartbeat_buffer should be near 0 for \ + physically-aligned wrapped phases, got {stored}. The linear \ + (buggy) formula produces ~pi^2 = 9.87 here; the circular \ + (fixed) formula produces ~1e-6." + ); + } + + /// Diametrically opposite phases must yield maximum circular variance. + #[test] + fn test_phase_variance_opposite_phases() { + let v = phase_circular_variance(&[0.0, PI]); + assert!( + v > 0.99, + "two opposite phases must give circular variance near 1.0, got {v}" + ); + } + + /// Identical phases must yield zero circular variance, even far from zero. + #[test] + fn test_phase_variance_identical_phases() { + let v = phase_circular_variance(&[2.5, 2.5, 2.5, 2.5]); + assert!( + v < 1e-9, + "identical phases must give circular variance ~0, got {v}" + ); + } + #[test] fn test_fft_magnitude_dc() { let signal = vec![1.0; 8];