refactor: move test_helpers, RunningQuality
This commit is contained in:
@@ -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; }
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user