diff --git a/google-tests/CMakeLists.txt b/google-tests/CMakeLists.txt index d8e648f..68f45da 100644 --- a/google-tests/CMakeLists.txt +++ b/google-tests/CMakeLists.txt @@ -12,6 +12,7 @@ add_executable(Google_Tests_run test1.cpp test2.cpp test3.cpp + test_helpers.cpp ) file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) @@ -23,6 +24,8 @@ file(COPY test1/iir_t1_y.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) file(COPY test2/ssf_t2_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2) file(COPY test2/ssf_t2_y_ref.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2) +file(COPY test3/ssf_t3_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test3) + target_link_libraries(Google_Tests_run pasada) #target_include_directories(Google_Tests_run PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pasada-lib/include") diff --git a/google-tests/test2.cpp b/google-tests/test2.cpp index 64a3de5..5399cf8 100644 --- a/google-tests/test2.cpp +++ b/google-tests/test2.cpp @@ -6,56 +6,13 @@ #include #include "iir_filter.h" #include "ssf_filter.h" +#include "test_helpers.h" #include #include #define FPS 60 #define MAX_BPM 300 -template static std::vector apply_filter(T& filter, std::vector& x) { - std::vector y; - y.resize(x.size()); - for (int i = 0; i < x.size(); i++) { - y[i] = filter.filter(x[i]); - } - return y; -} - -static void npy_save(std::string path, std::vector& x) { - npy::npy_data_ptr d; - d.data_ptr = x.data(); - d.shape = {(unsigned long) x.size()}; - npy::write_npy(path, d); -} - -static std::vector fetch_y_axis(npy::npy_data& acc) { - // TODO: later on, we should use a vector projection towards gravity - std::vector signal; - const size_t rows_real = acc.shape[0]; -#if DEBUG_IIR == 1 - const size_t rows = 5; -#else - const size_t rows = acc.shape[0]; -#endif - int stride = 3; - int offset = 1; // [x,y,z] per row - fetch y - signal.resize(rows); - if (acc.fortran_order) { - stride = 1; - offset = (int) rows_real; - } - /* - std::cout << "is_fortran=" << acc.fortran_order << std::endl; - for (size_t i = 0; i < 10; i++) { - std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl; - } - */ - for (int i = 0; i < rows; i++) { - signal[i] = acc.data[i * stride + offset]; - } - return signal; -} - TEST(HelloTest, Zong_SSF_Stage1) { npy::npy_data acc = npy::read_npy("test2/ssf_t2_acc.npy"); npy::npy_data y_ref = npy::read_npy("test2/ssf_t2_y_ref.npy"); @@ -166,16 +123,6 @@ TEST(HelloTest, Zong_SSF_Stage2) { npy_save("test2/ssf_t2_ssf.npy", ssf); } -/** Returns the ssf_threshold as the filter output for debugging. */ -class DebugSsfStepDetectorThreshold : public SsfStepDetector { -public: - DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {} - double filter(double val) { - this->SsfStepDetector::filter(val); - return peek_threshold(); - } -}; - TEST(HelloTest, Zong_SSF_Stage3) { npy::npy_data acc = npy::read_npy("test2/ssf_t2_acc.npy"); diff --git a/google-tests/test3.cpp b/google-tests/test3.cpp index 2577076..49bc59b 100644 --- a/google-tests/test3.cpp +++ b/google-tests/test3.cpp @@ -2,9 +2,12 @@ // Created by david on 04.03.2026. // #include +#include "npy.hpp" //#include #include #include "pd_signal.h" +#include "ssf_filter.h" +#include "test_helpers.h" using namespace pd_signal; @@ -38,123 +41,6 @@ TEST(SignalTest, ranges) { ASSERT_NEAR(2.0, i[2], abs_error); } -/** - * Running signal quality indicator. - */ -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.9; - /** threshold for accepting subsequent beats */ - 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::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.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.emplace_back(x); - // essentially just a copy - pd_signal::mean(beatTemplate, beatTemplates); - } - - virtual void dispatchLocked() { /* implement me, add Listener etc. */ } - 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), 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, 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(ssf, beatTemplate); - posCorr = pd_signal::clip(corr, 0.0, 1.0); - double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1; - goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf); - } - - if (beatTemplates.size() == 0) { - // cannot correlate the first beat, no template yet - std::cerr << "(0) first beat -> addTemplate()" << std::endl; - addTemplate(beat); - justLocked = false; - } else if (beatTemplates.size() <= 2) { - // restart if there is no clear correlation between beats - if (goodBeat) { - std::cerr << "(2) good initial beat -> addTemplate()" << std::endl; - addTemplate(beat); - if (beatTemplates.size() > 2) - justLocked = true; // TODO why not set? wrong compiler optimization? (is it unaware of member change?) - //std::cerr << " (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl; - } else { - std::cerr << "(2) bad initial beat -> replaceTemplate()" << std::endl; - replaceTemplate(beat); - //badBeatRanges.clear(); - justLocked = false; - } - } else { - // running mode: collect bad beats, but may be OK not to restart immediately - - std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl; - if (goodBeat) { - addTemplate(beat); - } else { - // badBeatRanges.add(s, e) - // numNoisy++ - } - // runningCorrs.add(posCorr) - if (justLocked) { dispatchLocked(); justLocked = false; } - dispatchBeat(idx, goodBeat, posCorr); - } - idx++; - } -}; - class DebugRunningQuality : public RunningQuality { protected: virtual void dispatchLocked() { locked = true; } @@ -212,4 +98,79 @@ TEST(SignalTest, RunningQuality_t1) { //ASSERT_NEAR(0.3, sqi.getBeatTemplate()[1], 1e-6); //ASSERT_NEAR(0.7, sqi.getBeatTemplate()[4], 1e-6); // nb. resampled! ASSERT_NEAR(num/norm, sqi.getCorrs()[0], 1e-3); -} \ No newline at end of file +} + +TEST(SignalTest, RunningQuality_t2) { + npy::npy_data acc = npy::read_npy("test3/ssf_t3_acc.npy"); + + std::vector signal = fetch_y_axis(acc); + +#if (FPS != 60) +#error "FPS must currently be 60, as highpass taps are pre-computed for that value" +#endif + + // TODO: SQI: cehck input file + // TODO: SQI: print debug values corr,idx, checkedSsf + + // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass' + std::vector b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845}; + 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_t3_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_t3_steps.npy", steps); + + // Debug SQI + + DebugRunningQuality sqi; + + std::vector beat_buf; + std::vector ssf_buf; + // y, ssf + size_t N = y.size(); + for (size_t i = 0; i < N; i++) { + if (steps[i] == 1.0) { + sqi.append(beat_buf, ssf_buf); + beat_buf.clear(); + ssf_buf.clear(); + } + beat_buf.push_back(y[i]); + ssf_buf.push_back(ssf[i]); + } + + EXPECT_TRUE(sqi.isLocked()); + EXPECT_TRUE(sqi.getCorrs().size() > 50); + + std::vector corrs(sqi.getCorrs()); + npy_save("test3/ssf_t3_sqi_corrs.npy", corrs); +} diff --git a/google-tests/test3/ssf_t3_acc.npy b/google-tests/test3/ssf_t3_acc.npy new file mode 100644 index 0000000..e018df0 Binary files /dev/null and b/google-tests/test3/ssf_t3_acc.npy differ diff --git a/google-tests/test_helpers.cpp b/google-tests/test_helpers.cpp new file mode 100644 index 0000000..bef2d96 --- /dev/null +++ b/google-tests/test_helpers.cpp @@ -0,0 +1,46 @@ +// +// Created by david on 11.03.2026. +// + +#include "test_helpers.h" + +void npy_save(std::string path, std::vector& x) { + npy::npy_data_ptr d; + d.data_ptr = x.data(); + d.shape = {(unsigned long) x.size()}; + npy::write_npy(path, d); +} + +std::vector fetch_y_axis(npy::npy_data& acc) { + // TODO: later on, we should use a vector projection towards gravity + std::vector signal; + const size_t rows_real = acc.shape[0]; +#if DEBUG_IIR == 1 + const size_t rows = 5; +#else + const size_t rows = acc.shape[0]; +#endif + int stride = 3; + int offset = 1; // [x,y,z] per row - fetch y + signal.resize(rows); + if (acc.fortran_order) { + stride = 1; + offset = (int) rows_real; + } + /* + std::cout << "is_fortran=" << acc.fortran_order << std::endl; + for (size_t i = 0; i < 10; i++) { + std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl; + } + */ + for (int i = 0; i < rows; i++) { + signal[i] = acc.data[i * stride + offset]; + } + return signal; +} + +DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {} +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 new file mode 100644 index 0000000..9b23828 --- /dev/null +++ b/google-tests/test_helpers.h @@ -0,0 +1,33 @@ +// +// Created by david on 11.03.2026. +// + +#ifndef PASADASUPERPROJECT_TEST_HELPERS_H +#define PASADASUPERPROJECT_TEST_HELPERS_H + +#include "npy.hpp" +#include "ssf_filter.h" +#include +#include + +template static std::vector apply_filter(T& filter, std::vector& x) { + std::vector y; + y.resize(x.size()); + for (int i = 0; i < x.size(); i++) { + y[i] = filter.filter(x[i]); + } + return y; +} + +void npy_save(std::string path, std::vector& x); + +std::vector fetch_y_axis(npy::npy_data& acc); + +/** Returns the ssf_threshold as the filter output for debugging. */ +class DebugSsfStepDetectorThreshold : public SsfStepDetector { +public: + DebugSsfStepDetectorThreshold(size_t len_refr); + double filter(double val); +}; + +#endif //PASADASUPERPROJECT_TEST_HELPERS_H \ No newline at end of file diff --git a/pasada-lib/include/ssf_filter.h b/pasada-lib/include/ssf_filter.h index 9199a1c..11fd9da 100644 --- a/pasada-lib/include/ssf_filter.h +++ b/pasada-lib/include/ssf_filter.h @@ -6,6 +6,7 @@ #define PASADASUPERPROJECT_SSF_FILTER_H #include "iir_filter.h" +#include #define FPS 60 #define MAX_BPM 300 @@ -53,4 +54,52 @@ public: double peek_threshold(); }; +/** + * Running signal quality indicator. + */ +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.9; + /** threshold for accepting subsequent beats */ + 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::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); + + void replaceTemplate(std::vector& x); + + virtual void dispatchLocked(); + virtual void dispatchBeat(int idx, bool good, double posCorr); + +public: + RunningQuality(); + explicit RunningQuality(bool 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, std::vector &rawSsf); +}; + #endif //PASADASUPERPROJECT_SSF_FILTER_H \ No newline at end of file diff --git a/pasada-lib/ssf_filter.cpp b/pasada-lib/ssf_filter.cpp index 5b281ee..88556f0 100644 --- a/pasada-lib/ssf_filter.cpp +++ b/pasada-lib/ssf_filter.cpp @@ -2,9 +2,9 @@ // Created by david on 03.03.2026. // -#include "include/ssf_filter.h" +#include "ssf_filter.h" +#include "pd_signal.h" #include -#include #include #include @@ -86,3 +86,87 @@ double SsfStepDetector::filter(double ssf) { double SsfStepDetector::peek_threshold() { return ssf_threshold; } + + +void RunningQuality::addTemplate(std::vector& 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 RunningQuality::replaceTemplate(std::vector& x) { + beatTemplates.clear(); + beatTemplates.emplace_back(x); + // essentially just a copy + pd_signal::mean(beatTemplate, beatTemplates); +} + +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) {} +RunningQuality::~RunningQuality() {} + +// note: arg should be an iterator really, but can do later +void RunningQuality::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; + pd_signal::resample(beat, rawBeat, BEAT_LEN); + pd_signal::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(ssf, beatTemplate); + posCorr = pd_signal::clip(corr, 0.0, 1.0); + double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1; + goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf); + } + + if (beatTemplates.size() == 0) { + // cannot correlate the first beat, no template yet + std::cerr << "(0) first beat -> addTemplate()" << std::endl; + addTemplate(beat); + justLocked = false; + } else if (beatTemplates.size() <= 2) { + // restart if there is no clear correlation between beats + if (goodBeat) { + std::cerr << "(2) good initial beat -> addTemplate()" << std::endl; + addTemplate(beat); + if (beatTemplates.size() > 2) + justLocked = true; // TODO why not set? wrong compiler optimization? (is it unaware of member change?) + //std::cerr << " (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl; + } else { + std::cerr << "(2) bad initial beat -> replaceTemplate()" << std::endl; + replaceTemplate(beat); + //badBeatRanges.clear(); + justLocked = false; + } + } else { + // running mode: collect bad beats, but may be OK not to restart immediately + + std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl; + if (goodBeat) { + addTemplate(beat); + } else { + // badBeatRanges.add(s, e) + // numNoisy++ + } + // runningCorrs.add(posCorr) + if (justLocked) { dispatchLocked(); justLocked = false; } + dispatchBeat(idx, goodBeat, posCorr); + } + idx++; +}