David Madl
90f8943930
* use SSF signal instead of accelerometer signal
* use higher BEAT_CORR_THR_{12} for SSF signal
* add absolute SSF_THRESHOLD to ignore small accelero bumps
* compute ssf_threshold according to detected SSF peaks, not the mean (more robust vs. noise)
215 lines
8.0 KiB
C++
215 lines
8.0 KiB
C++
//
|
|
// Created by david on 04.03.2026.
|
|
//
|
|
#include <gtest/gtest.h>
|
|
//#include <utility>
|
|
#include <deque>
|
|
#include "pd_signal.h"
|
|
|
|
using namespace pd_signal;
|
|
|
|
TEST(SignalTest, interp_t1) {
|
|
//EXPECT_EQ();
|
|
std::vector<double> xp { 1.0, 2.0, 4.0, 5.0, 5.01, 6.0 };
|
|
std::vector<double> fp { 1.0, 2.0, 4.0, 5.0, 5.01, 7.0 };
|
|
|
|
std::vector<double> x { 0.9, 1.0, 1.5, 2.0, 3.9, 4.1, 5.0, 5.5, 6.0, 6.1 };
|
|
std::vector<double>y_e{ 1.0, 1.0, 1.5, 2.0, 3.9, 4.1, 5.0, 5.99494949495, 7.0, 7.0 };
|
|
size_t N = x.size();
|
|
// 5.99494949495 = (5.5-5.01)/0.99*(7-5.01)+5.01
|
|
|
|
std::vector<double> y;
|
|
interp(y, x, xp, fp);
|
|
|
|
// assert y == y_e, nb. upto 5 digits
|
|
double abs_err = 1e-5;
|
|
for (size_t i = 0; i < N; i++) {
|
|
ASSERT_NEAR(y_e[i], y[i], abs_err + 1e-9 * i);
|
|
}
|
|
}
|
|
|
|
TEST(SignalTest, ranges) {
|
|
const double abs_error = 1e-5;
|
|
std::vector<double> i;
|
|
size_t N = 3;
|
|
linspace(i, 0, (int) (N-1), (int) N, false);
|
|
ASSERT_NEAR(0.0, i[0], abs_error);
|
|
ASSERT_NEAR(1.0, i[1], abs_error);
|
|
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; }
|
|
virtual void dispatchBeat(int idx, bool good, double posCorr) { corrs.push_back(posCorr); }
|
|
|
|
bool locked;
|
|
std::vector<double> corrs;
|
|
|
|
public:
|
|
DebugRunningQuality(): locked(false) {}
|
|
explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
|
|
virtual ~DebugRunningQuality() {}
|
|
bool isLocked() { return locked; }
|
|
std::vector<double> getCorrs() { return corrs; }
|
|
std::vector<double> getBeatTemplate() { return this->beatTemplate; }
|
|
};
|
|
|
|
/*
|
|
TEST(SignalTest, resample_same_len) {
|
|
std::vector<double> rawBeat {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
|
|
std::vector<double> beat;
|
|
resample(beat, rawBeat, 7);
|
|
// TODO
|
|
ASSERT_NEAR(0.3, beat[1], 1e-6);
|
|
}
|
|
*/
|
|
/*
|
|
TEST(SignalTest, resample_same_len) {
|
|
std::vector<double> rawBeat {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
|
|
std::vector<double> beat;
|
|
resample(beat, rawBeat, 7);
|
|
// TODO
|
|
//ASSERT_NEAR(0.3, beat[1], 1e-6);
|
|
for (int i = 0; i < 7; i++)
|
|
std::cout << "b[" << i << "]=" << beat[i] << std::endl;
|
|
}
|
|
*/
|
|
|
|
TEST(SignalTest, RunningQuality_t1) {
|
|
DebugRunningQuality sqi(true);
|
|
std::vector a {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
|
|
std::vector b {0.0, 0.3, 0.9, 1.0, 0.5, 0.5, 0.1};
|
|
std::vector c {0.0, 0.3, 0.9, 1.0, 0.9, 0.5, 0.1};
|
|
std::vector d {0.0, 0.3, 0.9, 1.0, 0.7, 0.4, 0.1};
|
|
sqi.append(a, a);
|
|
sqi.append(b, b);
|
|
sqi.append(c, c);
|
|
EXPECT_FALSE(sqi.isLocked());
|
|
sqi.append(d, d);
|
|
EXPECT_TRUE(sqi.isLocked());
|
|
ASSERT_EQ(1, sqi.getCorrs().size());
|
|
double norm = sqrt((0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.5*0.5 + 0.1*0.1) // \sum x_i^2
|
|
* (0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.4*0.4 + 0.1*0.1)); // \sum y_i^2
|
|
double num = (0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.5*0.4 + 0.1*0.1); // \sum x_i * y_i
|
|
//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);
|
|
} |