diff --git a/google-tests/test2.cpp b/google-tests/test2.cpp index 5399cf8..b23acfc 100644 --- a/google-tests/test2.cpp +++ b/google-tests/test2.cpp @@ -79,7 +79,7 @@ TEST(HelloTest, Filter_Delta_U) { // NOTE: later SSF must be fed -u, not u TEST(HelloTest, Filter_SSF) { - SsfFilter f_ssf(3); + SsfFilter f_ssf(FPS * 3/4); // target upslope_width = 3 std::vector x { 1.0, 3.0, 2.0, 5.0, 1.0, 1.5 }; // du { 1.0, 2.0, -1.0, 3.0, -4.0, 0.5 } // duc { 1.0, 2.0, 0.0, 3.0, 0.0, 0.5 } @@ -116,8 +116,7 @@ TEST(HelloTest, Zong_SSF_Stage2) { auto y_neg = apply_filter(f_neg, y); // Stage 2: sum slope function - const size_t upslope_width = 4; - SsfFilter f_ssf(upslope_width); + SsfFilter f_ssf(FPS); auto ssf = apply_filter(f_ssf, y_neg); npy_save("test2/ssf_t2_ssf.npy", ssf); @@ -148,22 +147,20 @@ TEST(HelloTest, Zong_SSF_Stage3) { //std::cerr << "before stage 2" << std::endl; // Stage 2: sum slope function - const size_t upslope_width = 4; - SsfFilter f_ssf(upslope_width); + SsfFilter f_ssf(FPS); 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); + DebugSsfStepDetectorThreshold f_ssd_thr(FPS); 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); + SsfStepDetector f_ssd(FPS); auto steps = apply_filter(f_ssd, ssf); //std::cerr << "before writing results 2" << std::endl; diff --git a/google-tests/test3.cpp b/google-tests/test3.cpp index 487b882..c1dfa4c 100644 --- a/google-tests/test3.cpp +++ b/google-tests/test3.cpp @@ -174,22 +174,20 @@ TEST(SignalTest, RunningQuality_t2) { //std::cerr << "before stage 2" << std::endl; // Stage 2: sum slope function - const size_t upslope_width = 4; - SsfFilter f_ssf(upslope_width); + SsfFilter f_ssf(FPS); 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); + DebugSsfStepDetectorThreshold f_ssd_thr(FPS); 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_t3_ssf_threshold.npy", ssf_threshold); - SsfStepDetector f_ssd(len_refr); + SsfStepDetector f_ssd(FPS); auto steps = apply_filter(f_ssd, ssf); //std::cerr << "before writing results 2" << std::endl; diff --git a/google-tests/test4.cpp b/google-tests/test4.cpp index 8297fe2..668de29 100644 --- a/google-tests/test4.cpp +++ b/google-tests/test4.cpp @@ -13,12 +13,12 @@ TEST(StepDetector, t1_sub_sample_resolution) { std::vector signal = fetch_y_axis(s); const size_t N = signal.size(); - const size_t N_INIT = SsfStepDetector::initial_samples(); + const size_t N_INIT = SsfStepDetector::initial_samples(FPS); - StepDetector det(nullptr, true); + StepDetector det(FPS, nullptr, true); // initialize: feed for priming the filters - det.primeFilters(signal); + det.primeFilters(FPS, signal); // feed for actual test for (size_t i = 0; i < N; i++) { diff --git a/google-tests/test5.cpp b/google-tests/test5.cpp index a75e703..d07f095 100644 --- a/google-tests/test5.cpp +++ b/google-tests/test5.cpp @@ -40,15 +40,13 @@ TEST(HelloTest, Zong_SSF_Test5_a1) { //std::cerr << "before stage 2" << std::endl; // Stage 2: sum slope function - const size_t upslope_width = 4; - SsfFilter f_ssf(upslope_width); + SsfFilter f_ssf(FPS); 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); + DebugSsfStepDetectorThreshold f_ssd_thr(FPS); auto ssf_threshold = apply_filter(f_ssd_thr, ssf); //std::cerr << "before writing results 1 and doing step detection" << std::endl; @@ -57,7 +55,7 @@ TEST(HelloTest, Zong_SSF_Test5_a1) { npy_save("test5/ssf_a1_ssf.npy", ssf); npy_save("test5/ssf_a1_ssf_threshold.npy", ssf_threshold); - SsfStepDetector f_ssd(len_refr); + SsfStepDetector f_ssd(FPS); auto steps = apply_filter(f_ssd, ssf); //std::cerr << "before writing results 2" << std::endl; diff --git a/google-tests/test_helpers.cpp b/google-tests/test_helpers.cpp index 153125f..e87f3da 100644 --- a/google-tests/test_helpers.cpp +++ b/google-tests/test_helpers.cpp @@ -46,7 +46,7 @@ std::vector fetch_y_axis(npy::npy_data& acc, int dim) { return signal; } -DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {} +DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(double fps) : SsfStepDetector(fps) {} double DebugSsfStepDetectorThreshold::filter(double val) { this->SsfStepDetector::filter(val); return peek_threshold(); diff --git a/google-tests/test_helpers.h b/google-tests/test_helpers.h index 3fec1c4..43d43a2 100644 --- a/google-tests/test_helpers.h +++ b/google-tests/test_helpers.h @@ -27,7 +27,7 @@ std::vector fetch_y_axis(npy::npy_data& acc, int dim = 1); /** Returns the ssf_threshold as the filter output for debugging. */ class DebugSsfStepDetectorThreshold : public SsfStepDetector { public: - DebugSsfStepDetectorThreshold(size_t len_refr); + DebugSsfStepDetectorThreshold(double fps); double filter(double val); }; diff --git a/pasada-lib/include/ssf_filter.h b/pasada-lib/include/ssf_filter.h index 27e7c9f..284fccb 100644 --- a/pasada-lib/include/ssf_filter.h +++ b/pasada-lib/include/ssf_filter.h @@ -18,11 +18,10 @@ */ class SsfFilter { protected: - size_t sw; Filt f_delta_u; Filt f_window; public: - SsfFilter(size_t upslope_width); + SsfFilter(double fps); double filter(double val); }; @@ -44,16 +43,17 @@ protected: size_t n_refr; bool is_refr; double ssf_nm1; + size_t ssf_usw2; Filt f_ssf_mean; public: /** * @param len_refr duration of refractory period, in samples */ - SsfStepDetector(size_t len_refr); + SsfStepDetector(double fps); double filter(double val); double peek_threshold(); - static size_t initial_samples(); + static size_t initial_samples(double fps); }; /** @@ -115,7 +115,7 @@ protected: std::vector ssf_buf; double sqi; public: - RunningQualityFilter(size_t upslope_width); + RunningQualityFilter(double fps); 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 fc0b0bb..af6dcc3 100644 --- a/pasada-lib/include/step_detector.h +++ b/pasada-lib/include/step_detector.h @@ -36,7 +36,7 @@ protected: std::vector buf_out; public: - StepDetector(StepListener *listener, bool debug = false); + StepDetector(double fps, StepListener *listener, bool debug = false); void filter(std::vector values); std::vector getBufSsd(); std::vector getBufSqi(); @@ -46,7 +46,7 @@ public: * Prime the filters using the given input signal. * Used for debugging (non-realtime processing) to align the signal. */ - void primeFilters(std::vector sig); + void primeFilters(double fps, std::vector sig); }; #endif //PASADASUPERPROJECT_STEP_DETECTOR_H \ No newline at end of file diff --git a/pasada-lib/ssf_filter.cpp b/pasada-lib/ssf_filter.cpp index 0d43b06..3b78b38 100644 --- a/pasada-lib/ssf_filter.cpp +++ b/pasada-lib/ssf_filter.cpp @@ -27,11 +27,18 @@ static std::vector make_ones(size_t sw) { return ones; } -SsfFilter::SsfFilter(size_t upslope_width) : - sw(upslope_width), +static int get_upslope_width(double fps) { + // was 4 at 60 fps = 66.7 ms + if (fps == 4.0) throw std::invalid_argument("check SsfFilter ctor call, must pass fps now"); // nice-to remove + int usw = static_cast(std::round(0.0667 * fps)); + if (usw == 0) throw std::invalid_argument("upslope_width = 0 computed - low fps?"); + return usw; +} + +SsfFilter::SsfFilter(double fps) : // Filt(N, shift, offset, taps) f_delta_u(2, 0, 0, std::vector {1.0, -1.0}), - f_window(upslope_width, 0, 0, make_ones(upslope_width)) + f_window(get_upslope_width(fps), 0, 0, make_ones(get_upslope_width(fps))) {} double SsfFilter::filter(double val) { double du = f_delta_u.filter(val); @@ -40,18 +47,41 @@ double SsfFilter::filter(double val) { return ssf; } -size_t SsfStepDetector::initial_samples() { return (size_t) (3.0 * FPS); } -SsfStepDetector::SsfStepDetector(size_t len_refr) : +static size_t get_initial_samples(double fps) { + if (fps == 12.0) throw std::invalid_argument("check SsfStepDetector ctor call, must pass fps now"); // nice-to remove + int init_samp = static_cast(std::round(3.0 * fps)); + if (init_samp == 0) throw std::invalid_argument("init_samp = 0 computed - low fps?"); + return init_samp; +} + +size_t SsfStepDetector::initial_samples(double fps) { return get_initial_samples(fps); } + +static size_t get_len_refr(double fps) { + if (fps == 12.0) throw std::invalid_argument("check SsfStepDetector ctor call, must pass fps now"); // nice-to remove + size_t len_refr = static_cast(std::round(fps / (MAX_BPM / 60))); + if (len_refr == 0) throw std::invalid_argument("len_refr = 0 computed - low fps?"); + return len_refr; +} + +static int get_len_ssf_th_smoothing(double fps) { + // was 6 at 60 fps = 100 ms + if (fps == 12.0) throw std::invalid_argument("check SsfStepDetector ctor call, must pass fps now"); // nice-to remove + int sts = static_cast(std::round(0.100 * fps)); + if (sts == 0) throw std::invalid_argument("len_ssf_th_smoothing = 0 computed - low fps?"); + return sts; +} +SsfStepDetector::SsfStepDetector(double fps) : // note: also change above, in initial_samples() - LEN_INIT((size_t) (3.0 * FPS)), // initial window length for ssf_threshold - LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold + LEN_INIT(get_initial_samples(fps)), // initial window length for ssf_threshold + LEN_TH_WIN(get_initial_samples(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), + f_ssf_threshold_smoothing(get_len_ssf_th_smoothing(fps), 0, 0, make_ones(get_len_ssf_th_smoothing(fps))), + len_refr(get_len_refr(fps)), n_refr(0), is_refr(false), ssf_nm1(0.0), + ssf_usw2(get_upslope_width(fps)/2), 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"); @@ -89,12 +119,10 @@ double SsfStepDetector::filter(double ssf) { } else if (num_samples > LEN_TH_WIN) { //DEBUG_PRINT(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) { + // the ssf peak comes SsfFilter.upslope_width/2 = 3 samples (half-window + 1 sample) after the crossing + if (num_samples == n_refr + ssf_usw2 + 1) { //DEBUG_PRINT(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; } } @@ -193,7 +221,7 @@ bool RunningQuality::append(std::vector &rawBeat, std::vector &r } -RunningQualityFilter::RunningQualityFilter(size_t upslope_width) : sqi(0.0) {} +RunningQualityFilter::RunningQualityFilter(double fps) : sqi(0.0) {} 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 6a5d6c5..1909054 100644 --- a/pasada-lib/step_detector.cpp +++ b/pasada-lib/step_detector.cpp @@ -19,13 +19,13 @@ static std::vector hpf_taps_a {1. , -4.83056552, 9.33652742, -9. static size_t upslope_width = 4; const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60)); -StepDetector::StepDetector(StepListener *listener, bool debug) : +StepDetector::StepDetector(double fps, StepListener *listener, bool debug) : listener(listener), f_highpass(hpf_taps_b, hpf_taps_a), f_neg(1, 0, 0, std::vector {-1.0}), - f_ssf(upslope_width), - f_ssd(len_refr), - f_sqi(upslope_width), + f_ssf(fps), + f_ssd(fps), + f_sqi(fps), debug(debug) {} @@ -56,8 +56,8 @@ std::vector StepDetector::getBufSsd() { return buf_ssd; } std::vector StepDetector::getBufSqi() { return buf_sqi; } std::vector StepDetector::getBufOut() { return buf_out; } -void StepDetector::primeFilters(std::vector sig) { - const size_t N_INIT = SsfStepDetector::initial_samples(); +void StepDetector::primeFilters(double fps, std::vector sig) { + const size_t N_INIT = SsfStepDetector::initial_samples(fps); // initialize: feed for priming the filters for (size_t i = 0; i < N_INIT; i++) { const auto a_i = static_cast(sig[i]);