david/libpasada
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: move test_helpers, RunningQuality

Browse Source
This commit is contained in:
David Madl
2026-03-12 20:34:54 +01:00
parent 90f8943930
commit 9aaec182a8
8 changed files with 297 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
google-tests/CMakeLists.txt
View File

@@ -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")

55
google-tests/test2.cpp
View File

@@ -6,56 +6,13 @@
#include <vector>
#include "iir_filter.h"
#include "ssf_filter.h"
#include "test_helpers.h"
#include <cmath>
#include <limits>
#define FPS 60
#define MAX_BPM 300
template <typename T> static std::vector<double> apply_filter(T& filter, std::vector<double>& x) {
std::vector<double> 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<double>& x) {
npy::npy_data_ptr<double> d;
d.data_ptr = x.data();
d.shape = {(unsigned long) x.size()};
npy::write_npy(path, d);
}
static std::vector<double> fetch_y_axis(npy::npy_data<double>& acc) {
// TODO: later on, we should use a vector projection towards gravity
std::vector<double> 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<double>("test2/ssf_t2_acc.npy");
npy::npy_data y_ref = npy::read_npy<double>("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<double>("test2/ssf_t2_acc.npy");

195
google-tests/test3.cpp
View File

@@ -2,9 +2,12 @@
// Created by david on 04.03.2026.
//
#include <gtest/gtest.h>
#include "npy.hpp"
//#include <utility>
#include <deque>
#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<std::vector<double> > beatTemplates;
std::vector<double> beatTemplate;
//std::vector<std::pair<int, int> > badBeatRanges;
double beatCorrThr2;
bool justLocked;
int idx;
/** for debugging only - disable SSF_THRESHOLD */
bool disableSsf;
void addTemplate(std::vector<double>& 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<double>& 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<double> &rawBeat, std::vector<double> &rawSsf) {
// TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
std::vector<double> beat;
std::vector<double> 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<double>::quiet_NaN();
double posCorr = std::numeric_limits<double>::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; }
@@ -213,3 +99,78 @@ TEST(SignalTest, RunningQuality_t1) {
//ASSERT_NEAR(0.7, sqi.getBeatTemplate()[4], 1e-6); // nb. resampled!
ASSERT_NEAR(num/norm, sqi.getCorrs()[0], 1e-3);
}
TEST(SignalTest, RunningQuality_t2) {
npy::npy_data acc = npy::read_npy<double>("test3/ssf_t3_acc.npy");
std::vector<double> 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<double> beat_buf;
std::vector<double> 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<double> corrs(sqi.getCorrs());
npy_save("test3/ssf_t3_sqi_corrs.npy", corrs);
}

BIN
google-tests/test3/ssf_t3_acc.npy Normal file
View File

Binary file not shown.

46
google-tests/test_helpers.cpp Normal file
View File

@@ -0,0 +1,46 @@
//
// Created by david on 11.03.2026.
//
#include "test_helpers.h"
void npy_save(std::string path, std::vector<double>& x) {
npy::npy_data_ptr<double> d;
d.data_ptr = x.data();
d.shape = {(unsigned long) x.size()};
npy::write_npy(path, d);
}
std::vector<double> fetch_y_axis(npy::npy_data<double>& acc) {
// TODO: later on, we should use a vector projection towards gravity
std::vector<double> 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();
}

33
google-tests/test_helpers.h Normal file
View File

@@ -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 <string>
#include <vector>
template <typename T> static std::vector<double> apply_filter(T& filter, std::vector<double>& x) {
std::vector<double> 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<double>& x);
std::vector<double> fetch_y_axis(npy::npy_data<double>& 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

49
pasada-lib/include/ssf_filter.h
View File

@@ -6,6 +6,7 @@
#define PASADASUPERPROJECT_SSF_FILTER_H
#include "iir_filter.h"
#include <deque>
#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<std::vector<double> > beatTemplates;
std::vector<double> beatTemplate;
//std::vector<std::pair<int, int> > badBeatRanges;
double beatCorrThr2;
bool justLocked;
int idx;
/** for debugging only - disable SSF_THRESHOLD */
bool disableSsf;
void addTemplate(std::vector<double>& x);
void replaceTemplate(std::vector<double>& 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<double> &rawBeat, std::vector<double> &rawSsf);
};
#endif //PASADASUPERPROJECT_SSF_FILTER_H

88
pasada-lib/ssf_filter.cpp
View File

@@ -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 <limits>
#include <cmath>
#include <cassert>
#include <iostream>
@@ -86,3 +86,87 @@ double SsfStepDetector::filter(double ssf) {
double SsfStepDetector::peek_threshold() {
return ssf_threshold;
}
void RunningQuality::addTemplate(std::vector<double>& 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<double>& 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<double> &rawBeat, std::vector<double> &rawSsf) {
// TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
std::vector<double> beat;
std::vector<double> 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<double>::quiet_NaN();
double posCorr = std::numeric_limits<double>::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++;
}