193 lines
6.7 KiB
C++
193 lines
6.7 KiB
C++
//
|
|
// Created by david on 04.03.2026.
|
|
//
|
|
#include <gtest/gtest.h>
|
|
//#include <utility>
|
|
#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:
|
|
/** 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.8;
|
|
/** threshold for accepting subsequent beats */
|
|
const double BEAT_CORR_THR_2 = 0.6;
|
|
|
|
std::vector<std::vector<double> > beatTemplates;
|
|
std::vector<double> beatTemplate;
|
|
//std::vector<std::pair<int, int> > badBeatRanges;
|
|
double beatCorrThr2;
|
|
bool justLocked;
|
|
int idx;
|
|
|
|
void addTemplate(std::vector<double>& x) {
|
|
beatTemplates.push_back(x);
|
|
pd_signal::mean(beatTemplate, beatTemplates);
|
|
}
|
|
|
|
void replaceTemplate(std::vector<double>& x) {
|
|
beatTemplates.clear();
|
|
beatTemplates.push_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) {}
|
|
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) {
|
|
// TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
|
|
|
|
std::vector<double> beat;
|
|
resample(beat, rawBeat, BEAT_LEN);
|
|
//std::ranges::copy(rawBeat, std::back_inserter(beat));
|
|
|
|
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(beat, beatTemplate);
|
|
posCorr = pd_signal::clip(corr, 0.0, 1.0);
|
|
double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1;
|
|
goodBeat = (corr > corrThreshold);
|
|
}
|
|
|
|
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) {}
|
|
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;
|
|
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);
|
|
sqi.append(b);
|
|
sqi.append(c);
|
|
EXPECT_FALSE(sqi.isLocked());
|
|
sqi.append(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);
|
|
} |