From 90f89439302779f24077ecb13e7489fb5e40a419 Mon Sep 17 00:00:00 2001 From: David Madl Date: Wed, 11 Mar 2026 20:47:53 +0100 Subject: [PATCH] feat: iterate on SsfStepDetector * use SSF signal instead of accelerometer signal * use higher BEAT_CORR_THR_{12} for SSF signal * add absolute SSF_THRESHOLD to ignore small accelero bumps * compute ssf_threshold according to detected SSF peaks, not the mean (more robust vs. noise) --- google-tests/test2.cpp | 10 +++++++ google-tests/test3.cpp | 50 ++++++++++++++++++++++++--------- pasada-lib/iir_filter.cpp | 14 ++++++--- pasada-lib/include/iir_filter.h | 7 +++-- pasada-lib/include/pd_signal.h | 3 ++ pasada-lib/include/ssf_filter.h | 4 ++- pasada-lib/pd_signal.cpp | 9 +++++- pasada-lib/ssf_filter.cpp | 33 ++++++++++++++++++---- 8 files changed, 103 insertions(+), 27 deletions(-) diff --git a/google-tests/test2.cpp b/google-tests/test2.cpp index 6aa49df..64a3de5 100644 --- a/google-tests/test2.cpp +++ b/google-tests/test2.cpp @@ -190,26 +190,36 @@ TEST(HelloTest, Zong_SSF_Stage3) { std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171}; IirFilter filter(b, a); + //std::cerr << "before stage 1" << std::endl; + // Stage 1: high-pass auto y = apply_filter(filter, signal); Filt f_neg(1, 0, 0, std::vector {-1.0}); auto y_neg = apply_filter(f_neg, y); + //std::cerr << "before stage 2" << std::endl; + // Stage 2: sum slope function const size_t upslope_width = 4; SsfFilter f_ssf(upslope_width); auto ssf = apply_filter(f_ssf, y_neg); + //std::cerr << "before stage 3" << std::endl; + // Stage 3: threshold detection const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60)); DebugSsfStepDetectorThreshold f_ssd_thr(len_refr); auto ssf_threshold = apply_filter(f_ssd_thr, ssf); + //std::cerr << "before writing results 1 and doing step detection" << std::endl; + npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold); SsfStepDetector f_ssd(len_refr); auto steps = apply_filter(f_ssd, ssf); + //std::cerr << "before writing results 2" << std::endl; + npy_save("test2/ssf_t2_steps.npy", steps); } diff --git a/google-tests/test3.cpp b/google-tests/test3.cpp index 433bdbc..2577076 100644 --- a/google-tests/test3.cpp +++ b/google-tests/test3.cpp @@ -3,6 +3,7 @@ // #include //#include +#include #include "pd_signal.h" using namespace pd_signal; @@ -42,29 +43,42 @@ TEST(SignalTest, ranges) { */ class RunningQuality { protected: + // TODO: make it a filter (output proper samples) + /** template beat is resampled to this #samples */ const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */; /** threshold for accepting initial beats */ - const double BEAT_CORR_THR_1 = 0.8; + const double BEAT_CORR_THR_1 = 0.9; /** threshold for accepting subsequent beats */ - const double BEAT_CORR_THR_2 = 0.6; + const double BEAT_CORR_THR_2 = 0.8; + /** absolute SSF threshold for accepting any beat */ + const double SSF_THRESHOLD = 5.0; + /** number of recent beats to use for beat template. must be even (alternating feet have different patterns; make it symmetric) */ + const int NUM_BEATS = 4; - std::vector > beatTemplates; + std::deque > beatTemplates; std::vector beatTemplate; //std::vector > badBeatRanges; double beatCorrThr2; bool justLocked; int idx; + /** for debugging only - disable SSF_THRESHOLD */ + bool disableSsf; + void addTemplate(std::vector& x) { - beatTemplates.push_back(x); + beatTemplates.emplace_back(x); + while (beatTemplates.size() > NUM_BEATS) { + // sliding window on 'beat_templates', do not use all history + beatTemplates.pop_front(); + } pd_signal::mean(beatTemplate, beatTemplates); } void replaceTemplate(std::vector& x) { beatTemplates.clear(); - beatTemplates.push_back(x); + beatTemplates.emplace_back(x); // essentially just a copy pd_signal::mean(beatTemplate, beatTemplates); } @@ -73,28 +87,35 @@ protected: virtual void dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ } public: - RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0) {} + RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {} + explicit RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {} virtual ~RunningQuality() {} // note: arg should be an iterator really, but can do later /** * @param beat individual beat accelero signal */ - void append(std::vector &rawBeat) { + void append(std::vector &rawBeat, std::vector &rawSsf) { // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector) std::vector beat; + std::vector ssf; resample(beat, rawBeat, BEAT_LEN); + resample(ssf, rawSsf, BEAT_LEN); //std::ranges::copy(rawBeat, std::back_inserter(beat)); + // check ssf at sample 2 (mid-slope of 4 window of ssf) + // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding + double checkedSsf = ssf[(int) (2*((double)beat.size())/((double)rawBeat.size()))]; + double corr = std::numeric_limits::quiet_NaN(); double posCorr = std::numeric_limits::quiet_NaN(); bool goodBeat = false; if (beatTemplates.size() > 0) { - corr = pd_signal::crossCorr(beat, beatTemplate); + corr = pd_signal::crossCorr(ssf, beatTemplate); posCorr = pd_signal::clip(corr, 0.0, 1.0); double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1; - goodBeat = (corr > corrThreshold); + goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf); } if (beatTemplates.size() == 0) { @@ -144,6 +165,7 @@ protected: public: DebugRunningQuality(): locked(false) {} + explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {} virtual ~DebugRunningQuality() {} bool isLocked() { return locked; } std::vector getCorrs() { return corrs; } @@ -172,16 +194,16 @@ TEST(SignalTest, resample_same_len) { */ TEST(SignalTest, RunningQuality_t1) { - DebugRunningQuality sqi; + DebugRunningQuality sqi(true); std::vector a {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1}; std::vector b {0.0, 0.3, 0.9, 1.0, 0.5, 0.5, 0.1}; std::vector c {0.0, 0.3, 0.9, 1.0, 0.9, 0.5, 0.1}; std::vector d {0.0, 0.3, 0.9, 1.0, 0.7, 0.4, 0.1}; - sqi.append(a); - sqi.append(b); - sqi.append(c); + sqi.append(a, a); + sqi.append(b, b); + sqi.append(c, c); EXPECT_FALSE(sqi.isLocked()); - sqi.append(d); + sqi.append(d, d); EXPECT_TRUE(sqi.isLocked()); ASSERT_EQ(1, sqi.getCorrs().size()); double norm = sqrt((0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.5*0.5 + 0.1*0.1) // \sum x_i^2 diff --git a/pasada-lib/iir_filter.cpp b/pasada-lib/iir_filter.cpp index fd050b2..9b19cfd 100644 --- a/pasada-lib/iir_filter.cpp +++ b/pasada-lib/iir_filter.cpp @@ -13,13 +13,13 @@ #define DEBUG_PRINT(expr) while(0) { expr; } #endif -Buf::Buf(size_t N): size(N), n(0) { +Buf::Buf(size_t N): N(N), n(0) { data.resize(N); data.assign(N, 0.0); } void Buf::push(double val) { data[n] = val; - n = (n+1) % size; + n = (n+1) % N; } Filt::Filt(size_t N, size_t shift, size_t offset, std::vector taps): Buf(N), shift(shift), offset(offset), taps(taps) { @@ -31,9 +31,9 @@ double Filt::filter(double val) { } double Filt::peek() { double sum = 0; - for (size_t i = offset; i < this->size; i++) { + for (size_t i = offset; i < this->N; i++) { //size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative - size_t n = (this->size + this->n - i + shift - 1) % this->size; + size_t n = (this->N + this->n - i + shift - 1) % this->N; DEBUG_PRINT(std::cout << " t[" << i << "] * v[" << n << "]" << std::endl); sum += this->data[n] * this->taps[i]; } @@ -42,6 +42,12 @@ double Filt::peek() { void Filt::push(double val) { Buf::push(val); } +void Filt::prime(double val) { + data.assign(this->N, val); +} +size_t Filt::size() { + return this->N; +} IirFilter::IirFilter(std::vector b, std::vector a) : x(b.size(), 0, 0, b), y(a.size(), 1, 1, a) { if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()"); diff --git a/pasada-lib/include/iir_filter.h b/pasada-lib/include/iir_filter.h index b3fca5f..6d13d86 100644 --- a/pasada-lib/include/iir_filter.h +++ b/pasada-lib/include/iir_filter.h @@ -13,7 +13,7 @@ class Buf { protected: std::vector data; - size_t size; + size_t N; size_t n; public: Buf(size_t N); @@ -21,7 +21,7 @@ public: }; /** Running filter base. */ -class Filt : Buf { +class Filt : public Buf { protected: std::vector taps; size_t shift; @@ -31,6 +31,9 @@ public: double filter(double val); double peek(); void push(double val); + /** prime the filter by overwriting the entire buffer with 'val' */ + void prime(double val); + size_t size(); }; /** Running IIR filter. */ diff --git a/pasada-lib/include/pd_signal.h b/pasada-lib/include/pd_signal.h index 65c085d..ff34d89 100644 --- a/pasada-lib/include/pd_signal.h +++ b/pasada-lib/include/pd_signal.h @@ -6,6 +6,7 @@ #define PASADASUPERPROJECT_SIGNAL_H #include +#include namespace pd_signal { /** `num` evenly spaced numbers over interval [start,stop] */ @@ -33,6 +34,8 @@ namespace pd_signal { /** two-dimensional mean of a collection of signals */ void mean(std::vector &out, std::vector >& m); + /** two-dimensional mean of a collection of signals */ + void mean(std::vector &out, std::deque >& m); } diff --git a/pasada-lib/include/ssf_filter.h b/pasada-lib/include/ssf_filter.h index 0197132..9199a1c 100644 --- a/pasada-lib/include/ssf_filter.h +++ b/pasada-lib/include/ssf_filter.h @@ -37,10 +37,12 @@ protected: const size_t LEN_TH_WIN; size_t num_samples; double ssf_threshold; + double ssf_threshold_nm1; + Filt f_ssf_threshold_smoothing; size_t len_refr; size_t n_refr; bool is_refr; - double nm1_ssf; + double ssf_nm1; Filt f_ssf_mean; public: /** diff --git a/pasada-lib/pd_signal.cpp b/pasada-lib/pd_signal.cpp index 84ca03e..0b6ea18 100644 --- a/pasada-lib/pd_signal.cpp +++ b/pasada-lib/pd_signal.cpp @@ -113,7 +113,7 @@ double clip(double val, double a_min, double a_max) { } // two-dimensional mean of a collection of signals -void mean(std::vector &out, std::vector >& m) { +template void mean_tpl(std::vector &out, T& m) { if (m.empty()) { out.resize(0); return; @@ -132,4 +132,11 @@ void mean(std::vector &out, std::vector >& m) { } } +void mean(std::vector &out, std::vector >& m) { + mean_tpl(out, m); +} +void mean(std::vector &out, std::deque >& m) { + mean_tpl(out, m); +} + } diff --git a/pasada-lib/ssf_filter.cpp b/pasada-lib/ssf_filter.cpp index 65c01b8..5b281ee 100644 --- a/pasada-lib/ssf_filter.cpp +++ b/pasada-lib/ssf_filter.cpp @@ -6,6 +6,7 @@ #include #include #include +#include static std::vector make_ones(size_t sw) { std::vector ones; @@ -33,21 +34,26 @@ SsfStepDetector::SsfStepDetector(size_t len_refr) : LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold num_samples(0), ssf_threshold(std::numeric_limits::infinity()), + ssf_threshold_nm1(std::numeric_limits::infinity()), + f_ssf_threshold_smoothing(6, 0, 0, make_ones(6)), len_refr(len_refr), n_refr(0), is_refr(false), - nm1_ssf(0.0), + ssf_nm1(0.0), f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN)) { assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold"); } -double SsfStepDetector::filter(double val) { - double ssf_mean = f_ssf_mean.filter(val) / ((double) LEN_TH_WIN); +double SsfStepDetector::filter(double ssf) { + double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN); double rv = 0.0; if (num_samples >= LEN_INIT) { // initial and subsequent threshold setting. ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers } // threshold crossing detection - bool is_txing = nm1_ssf < ssf_threshold && val >= ssf_threshold; + // 'is_prev_lower' fixes a glitch where a falling threshold leads to undetected crossings + bool is_prev_lower = ssf_nm1 < ssf_threshold || ssf_nm1 < ssf_threshold_nm1; + bool is_cur_higher = ssf >= ssf_threshold; + bool is_txing = is_prev_lower && is_cur_higher; // refractory period reset if (num_samples - n_refr >= len_refr) is_refr = false; // transition and not in refractory period? detected a step. @@ -56,7 +62,24 @@ double SsfStepDetector::filter(double val) { is_refr = true; n_refr = num_samples; } - nm1_ssf = val; + if (num_samples == LEN_INIT) { + // initial threshold setting + ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers + //std::cerr << "before prime()" << std::endl; + f_ssf_threshold_smoothing.prime(ssf_threshold); + } else if (num_samples > LEN_TH_WIN) { + //std::cerr << "adaptive threshold setting" << std::endl; + // adaptive threshold setting + // +2 is half the window size + // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding -- also f_ssf_threshold_smoothing(), nb. should be even number + if (num_samples == n_refr + 2) { + //std::cerr << "setting adaptive threshold setting" << std::endl; + ssf_threshold_nm1 = ssf_threshold; + // the ssf peak comes 3 samples (half-window + 1 sample) after the crossing + ssf_threshold = f_ssf_threshold_smoothing.filter(ssf) / ((double) f_ssf_threshold_smoothing.size()) * 0.6; + } + } + ssf_nm1 = ssf; num_samples++; return rv; }