From 40e10c1718c3bbca666fc12081e8ec0a6dffe10e Mon Sep 17 00:00:00 2001 From: David Madl Date: Tue, 3 Mar 2026 00:33:03 +0100 Subject: [PATCH] feat: SSF step detector --- google-tests/test2.cpp | 157 ++++++++++++++++++++++++++++++-- pasada-lib/CMakeLists.txt | 2 + pasada-lib/include/ssf_filter.h | 54 +++++++++++ pasada-lib/ssf_filter.cpp | 65 +++++++++++++ 4 files changed, 268 insertions(+), 10 deletions(-) create mode 100644 pasada-lib/include/ssf_filter.h create mode 100644 pasada-lib/ssf_filter.cpp diff --git a/google-tests/test2.cpp b/google-tests/test2.cpp index 7ff00af..6aa49df 100644 --- a/google-tests/test2.cpp +++ b/google-tests/test2.cpp @@ -5,15 +5,30 @@ #include "npy.hpp" #include #include "iir_filter.h" +#include "ssf_filter.h" #include +#include -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"); +#define FPS 60 +#define MAX_BPM 300 - // - // fetch y axis - // +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]; @@ -38,6 +53,16 @@ TEST(HelloTest, Zong_SSF_Stage1) { 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"); + + std::vector signal = fetch_y_axis(acc); + const size_t N = signal.size(); + ASSERT_NEAR(1.7, signal[0], 1e-5); ASSERT_NEAR(3.6, signal[1], 1e-5); ASSERT_NEAR(4.3, signal[2], 1e-5); @@ -46,6 +71,10 @@ TEST(HelloTest, Zong_SSF_Stage1) { // # but it adds delay... // # <- we try reducing that to 100 ms delay. +#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}; @@ -55,14 +84,14 @@ TEST(HelloTest, Zong_SSF_Stage1) { // apply high-pass filter // std::vector y; - y.resize(rows); - for (int i = 0; i < rows; i++) { + y.resize(N); + for (int i = 0; i < N; i++) { y[i] = filter.filter(signal[i]); } // see: http://localhost:8888/notebooks/2026-02-25%20Accelero1/2026-03-01%20SSF3.ipynb // check results - for (int i = 0; i < rows; i++) { + for (int i = 0; i < N; i++) { //double rel_error = std::abs((y[i] - y_ref.data[i]) / y_ref.data[i]); //ASSERT_NEAR(0, rel_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg double abs_error = (y[i] - y_ref.data[i]); @@ -72,7 +101,115 @@ TEST(HelloTest, Zong_SSF_Stage1) { npy::npy_data_ptr d; d.data_ptr = y.data(); - d.shape = {(unsigned long) rows}; + d.shape = {(unsigned long) N}; const std::string path{"test2/ssf_t2_y_out.npy"}; npy::write_npy(path, d); } + +TEST(HelloTest, Filter_Delta_U) { + Filt f_delta_u(2, 0, 0, std::vector {1.0, -1.0}); + std::vector x { 1.0, 3.0, 2.0 }; + std::vector y_ref { 1.0, 2.0, -1.0 }; + std::vector y; + y.resize(x.size()); + for (int i = 0; i < x.size(); i++) { + y[i] = f_delta_u.filter(x[i]); + } + for (int i = 0; i < x.size(); i++) { + ASSERT_NEAR(y_ref[i], y[i], 1e-5); + } +} + +// NOTE: later SSF must be fed -u, not u +TEST(HelloTest, Filter_SSF) { + SsfFilter f_ssf(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 } + // ssf { 1.0, 3.0, 3.0, 5.0, 3.0, 3.5 } + std::vector ssf_ref { 1.0, 3.0, 3.0, 5.0, 3.0, 3.5 }; + std::vector ssf; + ssf.resize(x.size()); + for (int i = 0; i < x.size(); i++) { + ssf[i] = f_ssf.filter(x[i]); + } + for (int i = 0; i < x.size(); i++) { + ASSERT_NEAR(ssf_ref[i], ssf[i], 1e-5); + } +} + +TEST(HelloTest, Zong_SSF_Stage2) { + npy::npy_data acc = npy::read_npy("test2/ssf_t2_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 + + // 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); + + // 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); + + // Stage 2: sum slope function + const size_t upslope_width = 4; + SsfFilter f_ssf(upslope_width); + auto ssf = apply_filter(f_ssf, y_neg); + + 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"); + + 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 + + // 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); + + // 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); + + // Stage 2: sum slope function + const size_t upslope_width = 4; + SsfFilter f_ssf(upslope_width); + auto ssf = apply_filter(f_ssf, y_neg); + + // 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); + + npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold); + + SsfStepDetector f_ssd(len_refr); + auto steps = apply_filter(f_ssd, ssf); + + npy_save("test2/ssf_t2_steps.npy", steps); +} diff --git a/pasada-lib/CMakeLists.txt b/pasada-lib/CMakeLists.txt index 64f1494..304900a 100644 --- a/pasada-lib/CMakeLists.txt +++ b/pasada-lib/CMakeLists.txt @@ -3,6 +3,8 @@ project(Pasada_Lib) SET(PASADA_SRC library.cpp iir_filter.cpp + ssf_filter.cpp + include/ssf_filter.h ) if(PASADA_BUILD_TESTS) diff --git a/pasada-lib/include/ssf_filter.h b/pasada-lib/include/ssf_filter.h new file mode 100644 index 0000000..0197132 --- /dev/null +++ b/pasada-lib/include/ssf_filter.h @@ -0,0 +1,54 @@ +// +// Created by david on 03.03.2026. +// + +#ifndef PASADASUPERPROJECT_SSF_FILTER_H +#define PASADASUPERPROJECT_SSF_FILTER_H + +#include "iir_filter.h" + +#define FPS 60 +#define MAX_BPM 300 + +/** + * Sum-Slope Function filter. + * + * See Zong et al, 2003: An open-source algorithm to detect onset of arterial blood pressure pulses. + */ +class SsfFilter { +protected: + size_t sw; + Filt f_delta_u; + Filt f_window; +public: + SsfFilter(size_t upslope_width); + double filter(double val); +}; + +/** + * Provides dirac pulses upon steps, based on SSF input. + * + * Settling time is LEN_INIT (currently 3.0 sec), + * no steps are detected before. + */ +class SsfStepDetector { +protected: + const size_t LEN_INIT; + const size_t LEN_TH_WIN; + size_t num_samples; + double ssf_threshold; + size_t len_refr; + size_t n_refr; + bool is_refr; + double nm1_ssf; + Filt f_ssf_mean; +public: + /** + * @param len_refr duration of refractory period, in samples + */ + SsfStepDetector(size_t len_refr); + double filter(double val); + double peek_threshold(); +}; + +#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 new file mode 100644 index 0000000..e760d20 --- /dev/null +++ b/pasada-lib/ssf_filter.cpp @@ -0,0 +1,65 @@ +// +// Created by david on 03.03.2026. +// + +#include "include/ssf_filter.h" +#include +#include +#include + +static std::vector make_ones(size_t sw) { + std::vector ones; + ones.resize(sw); + ones.assign(sw, 1.0); + return ones; +} + +SsfFilter::SsfFilter(size_t upslope_width) : + sw(upslope_width), + // 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)) +{} +double SsfFilter::filter(double val) { + double du = f_delta_u.filter(val); + double duc = std::max(0.0, du); + double ssf = f_window.filter(duc); + return ssf; +} + + +SsfStepDetector::SsfStepDetector(size_t len_refr) : + 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), + ssf_threshold(std::numeric_limits::infinity()), + len_refr(len_refr), n_refr(0), is_refr(false), + nm1_ssf(0.0), + 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"); +} +double SsfStepDetector::filter(double val) { + double ssf_mean = f_ssf_mean.filter(val) / ((double) LEN_TH_WIN); + double rv = 0.0; + if (num_samples >= LEN_INIT) { + // initial and subsequent threshold setting. + ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers + } + // threshold crossing detection + bool is_txing = nm1_ssf < ssf_threshold && val >= ssf_threshold; + // refractory period reset + if (num_samples - n_refr >= len_refr) is_refr = false; + // transition and not in refractory period? detected a step. + if (is_txing && !is_refr) { + rv = 1.0; + is_refr = true; + n_refr = num_samples; + } + nm1_ssf = val; + num_samples++; + return rv; +} +double SsfStepDetector::peek_threshold() { + return ssf_threshold; +}