From 60a5b109bb294c607d335331c0257414658ca413 Mon Sep 17 00:00:00 2001 From: David Madl Date: Wed, 20 May 2026 00:57:49 +0200 Subject: [PATCH] feat: StepDetector update FPS, more FPS fixes --- google-tests/test3.cpp | 9 +++++---- google-tests/test4.cpp | 2 +- pasada-lib/include/ssf_filter.h | 21 ++++++++++++--------- pasada-lib/include/step_detector.h | 1 + pasada-lib/ssf_filter.cpp | 13 ++++++++++--- pasada-lib/step_detector.cpp | 13 +++++++++++-- 6 files changed, 40 insertions(+), 19 deletions(-) diff --git a/google-tests/test3.cpp b/google-tests/test3.cpp index 021db0e..dd481a3 100644 --- a/google-tests/test3.cpp +++ b/google-tests/test3.cpp @@ -94,8 +94,8 @@ protected: std::vector goods; public: - DebugRunningQuality(): lockedAt(-1), locked(false) {} - explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {} + DebugRunningQuality(double fps): RunningQuality(fps), lockedAt(-1), locked(false) {} + explicit DebugRunningQuality(double fps, bool disableSsf): RunningQuality(fps, disableSsf), locked(false) {} virtual ~DebugRunningQuality() {} bool isLocked() { return locked; } std::vector getCorrs() { return corrs; } @@ -126,7 +126,8 @@ TEST(SignalTest, resample_same_len) { */ TEST(SignalTest, RunningQuality_t1) { - DebugRunningQuality sqi(true); + double fps = 60.0; + DebugRunningQuality sqi(fps, 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}; @@ -194,7 +195,7 @@ TEST(SignalTest, RunningQuality_t2) { // Debug SQI - DebugRunningQuality sqi; + DebugRunningQuality sqi(fps); std::vector beat_buf; std::vector ssf_buf; diff --git a/google-tests/test4.cpp b/google-tests/test4.cpp index eb2500c..f89bb6f 100644 --- a/google-tests/test4.cpp +++ b/google-tests/test4.cpp @@ -25,7 +25,7 @@ TEST(StepDetector, t1_sub_sample_resolution) { // feed for actual test for (size_t i = 0; i < N; i++) { const auto a_i = static_cast(signal[i]); - det.filter(ts, std::vector {0.0f, a_i, 0.0f}); + det.filter(ts * 1e9, std::vector {0.0f, a_i, 0.0f}); ts += 1.0 / fps; } diff --git a/pasada-lib/include/ssf_filter.h b/pasada-lib/include/ssf_filter.h index f2fd46d..2caa6eb 100644 --- a/pasada-lib/include/ssf_filter.h +++ b/pasada-lib/include/ssf_filter.h @@ -33,8 +33,8 @@ public: */ class SsfStepDetector { protected: - const size_t LEN_INIT; - const size_t LEN_TH_WIN; + size_t LEN_INIT; + size_t LEN_TH_WIN; size_t num_samples; double ssf_threshold; double ssf_threshold_nm1; @@ -50,6 +50,7 @@ public: * @param len_refr duration of refractory period, in samples */ SsfStepDetector(double fps); + ~SsfStepDetector(); double filter(double val); double peek_threshold(); @@ -62,18 +63,19 @@ public: class RunningQuality { protected: // TODO: make it a filter (output proper samples) + // TODO: use fps info /** template beat is resampled to this #samples */ - const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */; + int BEAT_LEN; /** threshold for accepting initial beats */ - const double BEAT_CORR_THR_1 = 0.9; + double BEAT_CORR_THR_1 = 0.9; /** threshold for accepting subsequent beats */ - const double BEAT_CORR_THR_2 = 0.8; + double BEAT_CORR_THR_2 = 0.8; /** absolute SSF threshold for accepting any beat */ - const double SSF_THRESHOLD = 5.0; + 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; + int NUM_BEATS = 4; std::deque > beatTemplates; std::vector beatTemplate; @@ -93,8 +95,8 @@ protected: virtual void dispatchBeat(int idx, bool good, double posCorr); public: - RunningQuality(); - explicit RunningQuality(bool disableSsf); + RunningQuality(double fps); + explicit RunningQuality(double fps, bool disableSsf); virtual ~RunningQuality(); // note: arg should be an iterator really, but can do later @@ -116,6 +118,7 @@ protected: double sqi; public: RunningQualityFilter(double fps); + ~RunningQualityFilter(); double filter(double y, double ssf, double step); }; diff --git a/pasada-lib/include/step_detector.h b/pasada-lib/include/step_detector.h index 2067ecd..d27c23e 100644 --- a/pasada-lib/include/step_detector.h +++ b/pasada-lib/include/step_detector.h @@ -51,6 +51,7 @@ protected: Resampler res_x; Resampler res_y; Resampler res_z; + double fps; public: StepDetector(double fps, StepListener *listener, bool debug = false); diff --git a/pasada-lib/ssf_filter.cpp b/pasada-lib/ssf_filter.cpp index 3b78b38..9606c74 100644 --- a/pasada-lib/ssf_filter.cpp +++ b/pasada-lib/ssf_filter.cpp @@ -86,6 +86,7 @@ SsfStepDetector::SsfStepDetector(double fps) : { assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold"); } +SsfStepDetector::~SsfStepDetector() {} double SsfStepDetector::filter(double ssf) { double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN); double rv = 0.0; @@ -154,8 +155,13 @@ void RunningQuality::replaceTemplate(std::vector& x) { void RunningQuality::dispatchLocked() { /* implement me, add Listener etc. */ } void RunningQuality::dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ } -RunningQuality::RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {} -RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {} +static int get_beat_len(double fps) { + /* 2*FPS for 30 bpm lower end */ + return 2.0 * fps; +} + +RunningQuality::RunningQuality(double fps): BEAT_LEN(get_beat_len(fps)), beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {} +RunningQuality::RunningQuality(double fps, bool disableSsf): BEAT_LEN(get_beat_len(fps)), beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {} RunningQuality::~RunningQuality() {} // note: arg should be an iterator really, but can do later @@ -221,7 +227,8 @@ bool RunningQuality::append(std::vector &rawBeat, std::vector &r } -RunningQualityFilter::RunningQualityFilter(double fps) : sqi(0.0) {} +RunningQualityFilter::RunningQualityFilter(double fps) : f_sqi(fps), sqi(0.0) {} +RunningQualityFilter::~RunningQualityFilter() {} double RunningQualityFilter::filter(double y, double ssf, double step) { if (step == 1.0) { diff --git a/pasada-lib/step_detector.cpp b/pasada-lib/step_detector.cpp index 8681e67..dd4480a 100644 --- a/pasada-lib/step_detector.cpp +++ b/pasada-lib/step_detector.cpp @@ -12,7 +12,8 @@ StepDetector::StepDetector(double fps, StepListener *listener, bool debug) : f_ssf(fps), f_ssd(fps), f_sqi(fps), - debug(debug) + debug(debug), + fps(0.0) {} static int gravity_num_taps(double fps) { @@ -50,6 +51,14 @@ void StepDetector::filter(double ts, std::vector values) { res_x.push(ts, values[0]); res_y.push(ts, values[1]); res_z.push(ts, values[2]); + // as soon as there is 'fps' information, re-init the classes requiring fps info + if (fps == 0.0 && res_x.peek()) { + fps = res_x.get_fs(); + f_grav = GravityFilter(fps); + f_ssf = SsfFilter(fps); + f_ssd = SsfStepDetector(fps); + f_sqi = RunningQualityFilter(fps); + } while (res_x.peek()) { double x = res_x.get(), y = res_y.get(), z = res_z.get(); std::vector samp { x, y, z }; @@ -85,7 +94,7 @@ double StepDetector::primeFilters(double fps, std::vector sig) { double ts = 0; for (size_t i = 0; i < N_INIT; i++) { const auto a_i = static_cast(sig[i]); - filter(ts, std::vector {0.0f, a_i, 0.0f}); + filter(ts * 1e9, std::vector {0.0f, a_i, 0.0f}); ts += 1.0 / fps; } // clear debug buffers