diff --git a/google-tests/CMakeLists.txt b/google-tests/CMakeLists.txt index 68f45da..e609dd2 100644 --- a/google-tests/CMakeLists.txt +++ b/google-tests/CMakeLists.txt @@ -9,10 +9,11 @@ include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} libnpy/inclu # 'Google_Tests_run' is the target name # 'test1.cpp test2.cpp' are source files with tests add_executable(Google_Tests_run + test_helpers.cpp test1.cpp test2.cpp test3.cpp - test_helpers.cpp + test4.cpp ) file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) @@ -26,6 +27,8 @@ 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) +file(COPY test4/step_150a.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test4) + 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/test4.cpp b/google-tests/test4.cpp new file mode 100644 index 0000000..8297fe2 --- /dev/null +++ b/google-tests/test4.cpp @@ -0,0 +1,38 @@ +// +// Created by david on 15.03.2026. +// + +#include + +#include "step_detector.h" +#include "npy.hpp" +#include "test_helpers.h" + +TEST(StepDetector, t1_sub_sample_resolution) { + npy::npy_data s = npy::read_npy("test4/step_150a.npy"); + + std::vector signal = fetch_y_axis(s); + const size_t N = signal.size(); + const size_t N_INIT = SsfStepDetector::initial_samples(); + + StepDetector det(nullptr, true); + + // initialize: feed for priming the filters + det.primeFilters(signal); + + // feed for actual test + for (size_t i = 0; i < N; i++) { + const auto a_i = static_cast(signal[i]); + det.filter(std::vector {0.0f, a_i, 0.0f}); + } + + std::vector ssd = det.getBufSsd(); // raw SsfStepDetector + std::vector sqi = det.getBufSqi(); // SQI - RunningQuality beat correlations + std::vector out = det.getBufOut(); // steps where SQI is OK + + npy_save("test4/t1_ssd.npy", ssd); + npy_save("test4/t1_sqi.npy", sqi); + npy_save("test4/t1_out.npy", out); + + // http://localhost:8888/notebooks/2026-03-10%20step%20interpolate%2F2026-03-15%20synth.ipynb +} diff --git a/google-tests/test4/step_150.npy b/google-tests/test4/step_150.npy new file mode 100644 index 0000000..44f5a0a Binary files /dev/null and b/google-tests/test4/step_150.npy differ diff --git a/google-tests/test4/step_150a.npy b/google-tests/test4/step_150a.npy new file mode 100644 index 0000000..44f5a0a Binary files /dev/null and b/google-tests/test4/step_150a.npy differ diff --git a/pasada-lib/CMakeLists.txt b/pasada-lib/CMakeLists.txt index 4041f29..e0af4f0 100644 --- a/pasada-lib/CMakeLists.txt +++ b/pasada-lib/CMakeLists.txt @@ -5,6 +5,7 @@ SET(PASADA_SRC iir_filter.cpp ssf_filter.cpp pd_signal.cpp + step_detector.cpp ) if(PASADA_BUILD_TESTS) diff --git a/pasada-lib/include/ssf_filter.h b/pasada-lib/include/ssf_filter.h index 0bd2eb9..27e7c9f 100644 --- a/pasada-lib/include/ssf_filter.h +++ b/pasada-lib/include/ssf_filter.h @@ -52,6 +52,8 @@ public: SsfStepDetector(size_t len_refr); double filter(double val); double peek_threshold(); + + static size_t initial_samples(); }; /** diff --git a/pasada-lib/include/step_detector.h b/pasada-lib/include/step_detector.h new file mode 100644 index 0000000..fc0b0bb --- /dev/null +++ b/pasada-lib/include/step_detector.h @@ -0,0 +1,52 @@ +// +// Created by david on 15.03.2026. +// + +#ifndef PASADASUPERPROJECT_STEP_DETECTOR_H +#define PASADASUPERPROJECT_STEP_DETECTOR_H + +#include "iir_filter.h" +#include "ssf_filter.h" +#include + +class StepListener { +public: + virtual ~StepListener() {} + virtual void playBeat() = 0; +}; + +/** + * Step detector from accelerometer signal. + * + * Settling time is 3.0 sec (defined in SsfStepDetector.LEN_INIT), + * no steps are detected before. + */ +class StepDetector { +protected: + StepListener *listener; + IirFilter f_highpass; + Filt f_neg; + SsfFilter f_ssf; + SsfStepDetector f_ssd; + RunningQualityFilter f_sqi; + + bool debug; + std::vector buf_ssd; + std::vector buf_sqi; + std::vector buf_out; + +public: + StepDetector(StepListener *listener, bool debug = false); + void filter(std::vector values); + std::vector getBufSsd(); + std::vector getBufSqi(); + std::vector getBufOut(); + + /** + * Prime the filters using the given input signal. + * Used for debugging (non-realtime processing) to align the signal. + */ + void primeFilters(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 5f38b44..6ec3d4e 100644 --- a/pasada-lib/ssf_filter.cpp +++ b/pasada-lib/ssf_filter.cpp @@ -40,8 +40,10 @@ double SsfFilter::filter(double val) { return ssf; } +size_t SsfStepDetector::initial_samples() { return (size_t) (3.0 * FPS); } SsfStepDetector::SsfStepDetector(size_t len_refr) : + // 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 num_samples(0), diff --git a/pasada-lib/step_detector.cpp b/pasada-lib/step_detector.cpp new file mode 100644 index 0000000..6a5d6c5 --- /dev/null +++ b/pasada-lib/step_detector.cpp @@ -0,0 +1,70 @@ +// +// Created by david on 15.03.2026. +// + +#include "step_detector.h" + +// TODO: we are hardcoding filter coefficients for 60 Hz +// TODO: this is tolerable for 50 Hz + +// TODO: check if we can do with floats instead of doubles +// (check how much the [already bad] accuracy of filtering suffers) + +// TODO: in Java, check if delta timestamps effectively match FPS +// TODO: FPS constant should be passed as argument to C++ (but we keep an FPS define to validate the coefficients) + +// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass' +static std::vector hpf_taps_b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845}; +static std::vector hpf_taps_a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171}; +static size_t upslope_width = 4; +const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60)); + +StepDetector::StepDetector(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), + debug(debug) +{} + +#if (FPS != 60) +#error "FPS must currently be 60, as highpass taps are pre-computed for that value" +#endif + +void StepDetector::filter(std::vector values) { + // TODO: later on, we should use a vector projection towards gravity + auto s0 = (double) values[1]; // take y-axis value for now + auto s1 = f_highpass.filter(s0); + auto s2 = f_neg.filter(s1); + auto s3 = f_ssf.filter(s2); + auto s4 = f_ssd.filter(s3); + auto q5 = f_sqi.filter(s2, s3, s4); + if (debug) { + buf_ssd.push_back(s4); + buf_sqi.push_back(q5); + buf_out.push_back(s4 * (q5 > 0.0 ? 1.0 : 0.0)); + } + // is step, step quality is OK, and we have a listener? + if(s4 > 0.0 && q5 > 0.0 && listener != nullptr) { + listener->playBeat(); + } +} + +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(); + // initialize: feed for priming the filters + for (size_t i = 0; i < N_INIT; i++) { + const auto a_i = static_cast(sig[i]); + filter(std::vector {0.0f, a_i, 0.0f}); + } + // clear debug buffers + buf_ssd.clear(); + buf_sqi.clear(); + buf_out.clear(); +}