diff --git a/google-tests/CMakeLists.txt b/google-tests/CMakeLists.txt index e609dd2..cd8bbc4 100644 --- a/google-tests/CMakeLists.txt +++ b/google-tests/CMakeLists.txt @@ -14,6 +14,7 @@ add_executable(Google_Tests_run test2.cpp test3.cpp test4.cpp + test5.cpp ) file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) @@ -29,6 +30,8 @@ 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) +file(COPY test5/acc_1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test5) + 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/test5.cpp b/google-tests/test5.cpp new file mode 100644 index 0000000..a75e703 --- /dev/null +++ b/google-tests/test5.cpp @@ -0,0 +1,66 @@ +// +// Created by david on 10.05.2026. +// + +#include + +#include "step_detector.h" +#include "npy.hpp" +#include "test_helpers.h" + +/** + * These test a sweep over running speed (6.0 - 18.0 km/h with - normally - 20 sec steps). + * - 'acc_1' is with phone oscillations in a loose pocket + * - 'acc_2' is with phone fixed in front, in a side orientation (TODO: getting y axis does not work here) + * Both are 4-dim vectors with (ts, x, y, z) entries. + */ +TEST(HelloTest, Zong_SSF_Test5_a1) { + npy::npy_data acc = npy::read_npy("test5/acc_1.npy"); + + std::vector signal = fetch_y_axis(acc, 2); // (ts, x, y, z) entries -> fetch 'y' + +#if (FPS != 60) +#error "FPS must currently be 60, as highpass taps are pre-computed for that value" +#endif + + // 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); + //auto y = 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("test5/ssf_a1_y.npy", y); + npy_save("test5/ssf_a1_ssf.npy", ssf); + npy_save("test5/ssf_a1_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("test5/ssf_a1_steps.npy", steps); +} diff --git a/google-tests/test5/acc_1.npy b/google-tests/test5/acc_1.npy new file mode 100644 index 0000000..dfdb0f9 Binary files /dev/null and b/google-tests/test5/acc_1.npy differ diff --git a/google-tests/test5/acc_2.npy b/google-tests/test5/acc_2.npy new file mode 100644 index 0000000..1983949 Binary files /dev/null and b/google-tests/test5/acc_2.npy differ diff --git a/google-tests/test_helpers.cpp b/google-tests/test_helpers.cpp index e00b5ec..153125f 100644 --- a/google-tests/test_helpers.cpp +++ b/google-tests/test_helpers.cpp @@ -18,7 +18,7 @@ void npy_save(std::string path, std::vector& x) { npy::write_npy(path, d); } -std::vector fetch_y_axis(npy::npy_data& acc) { +std::vector fetch_y_axis(npy::npy_data& acc, int dim) { // TODO: later on, we should use a vector projection towards gravity std::vector signal; const size_t rows_real = acc.shape[0]; @@ -27,12 +27,12 @@ std::vector fetch_y_axis(npy::npy_data& acc) { #else const size_t rows = acc.shape[0]; #endif - int stride = 3; - int offset = 1; // [x,y,z] per row - fetch y + int stride = (int) acc.shape[1]; + int offset = dim; // [x,y,z] per row - fetch y by default signal.resize(rows); if (acc.fortran_order) { stride = 1; - offset = (int) rows_real; + offset = (int) rows_real * offset; } /* std::cout << "is_fortran=" << acc.fortran_order << std::endl; diff --git a/google-tests/test_helpers.h b/google-tests/test_helpers.h index 479a98e..3fec1c4 100644 --- a/google-tests/test_helpers.h +++ b/google-tests/test_helpers.h @@ -22,7 +22,7 @@ template static std::vector apply_filter(T& filter, std::ve void npy_save(std::string path, std::vector& x); void npy_save(std::string path, std::vector& x); -std::vector fetch_y_axis(npy::npy_data& acc); +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 { diff --git a/pasada-lib/include/iir_filter.h b/pasada-lib/include/iir_filter.h index 6d13d86..07e3dc0 100644 --- a/pasada-lib/include/iir_filter.h +++ b/pasada-lib/include/iir_filter.h @@ -42,7 +42,18 @@ protected: Filt y; Filt x; public: + /** + * Create IIR filter from coefficients 'b' and 'a' (numerator and denominator polynomial coefficients). + */ IirFilter(std::vector b, std::vector a); + + /** + * Create Butterworth lowpass filter. + * @param N order of Butterworth filter + * @param fc cutoff frequency in Hz + * @param fs sampling rate in Hz + */ + IirFilter(int N, double fc, double fs); double filter(double val); };