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

feat: RunningQuality - running SQI

Browse Source
This commit is contained in:
David Madl
2026-03-09 18:38:21 +01:00
parent fe300dabd3
commit 95d1fee44d
4 changed files with 209 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
google-tests/test3.cpp
View File

@@ -2,6 +2,7 @@
// Created by david on 04.03.2026.
//
#include <gtest/gtest.h>
//#include <utility>
#include "pd_signal.h"
using namespace pd_signal;
@@ -35,3 +36,158 @@ TEST(SignalTest, ranges) {
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);
}