From 95d1fee44d9f5a125430a4c175c0fe6c2c8109ff Mon Sep 17 00:00:00 2001 From: David Madl Date: Mon, 9 Mar 2026 18:38:21 +0100 Subject: [PATCH] feat: RunningQuality - running SQI --- google-tests/test3.cpp | 156 +++++++++++++++++++++++++++++++++ pasada-lib/include/pd_signal.h | 14 ++- pasada-lib/pd_signal.cpp | 40 ++++++++- pasada-lib/ssf_filter.cpp | 2 +- 4 files changed, 209 insertions(+), 3 deletions(-) diff --git a/google-tests/test3.cpp b/google-tests/test3.cpp index e148b92..433bdbc 100644 --- a/google-tests/test3.cpp +++ b/google-tests/test3.cpp @@ -2,6 +2,7 @@ // Created by david on 04.03.2026. // #include +//#include #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 > beatTemplates; + std::vector beatTemplate; + //std::vector > badBeatRanges; + double beatCorrThr2; + bool justLocked; + int idx; + + void addTemplate(std::vector& x) { + beatTemplates.push_back(x); + pd_signal::mean(beatTemplate, beatTemplates); + } + + void replaceTemplate(std::vector& 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 &rawBeat) { + // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector) + + std::vector beat; + resample(beat, rawBeat, BEAT_LEN); + //std::ranges::copy(rawBeat, std::back_inserter(beat)); + + double corr = std::numeric_limits::quiet_NaN(); + double posCorr = std::numeric_limits::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 corrs; + +public: + DebugRunningQuality(): locked(false) {} + virtual ~DebugRunningQuality() {} + bool isLocked() { return locked; } + std::vector getCorrs() { return corrs; } + std::vector getBeatTemplate() { return this->beatTemplate; } +}; + +/* +TEST(SignalTest, resample_same_len) { + std::vector rawBeat {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1}; + std::vector beat; + resample(beat, rawBeat, 7); + // TODO + ASSERT_NEAR(0.3, beat[1], 1e-6); +} +*/ +/* +TEST(SignalTest, resample_same_len) { + std::vector rawBeat {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1}; + std::vector 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); +} \ No newline at end of file diff --git a/pasada-lib/include/pd_signal.h b/pasada-lib/include/pd_signal.h index 2e32cac..65c085d 100644 --- a/pasada-lib/include/pd_signal.h +++ b/pasada-lib/include/pd_signal.h @@ -20,7 +20,19 @@ namespace pd_signal { void interp(std::vector& y, std::vector& x, std::vector& xp, std::vector& fp); /** resample to BEAT_LEN */ - void resample(std::vector &out, std::vector x, int beat_len); + void resample(std::vector &out, std::vector &x, int beat_len); + + /** + * normalized cross-correlation of the two signals of same length. + * normalization factor is 1 / sqrt(\sum_i x_i^2 * \sum_i y_i^2) + */ + double crossCorr(std::vector &x, std::vector &y); + + /** clip 'val' to between 'a_min' and 'a_max'. */ + double clip(double val, double a_min, double a_max); + + /** two-dimensional mean of a collection of signals */ + void mean(std::vector &out, std::vector >& m); } diff --git a/pasada-lib/pd_signal.cpp b/pasada-lib/pd_signal.cpp index dfc6d3c..84ca03e 100644 --- a/pasada-lib/pd_signal.cpp +++ b/pasada-lib/pd_signal.cpp @@ -5,6 +5,7 @@ #include "include/pd_signal.h" #include #include +#include namespace pd_signal { @@ -86,7 +87,7 @@ void interp(std::vector& y, std::vector& x, std::vector& } // resample to BEAT_LEN -void resample(std::vector &out, std::vector x, int beat_len) { +void resample(std::vector &out, std::vector &x, int beat_len) { std::vector t; std::vector i; linspace(t, 0, (double) x.size(), beat_len, false); @@ -94,4 +95,41 @@ void resample(std::vector &out, std::vector x, int beat_len) { interp(out, t, i, x); } +// normalized cross-correlation of the two signals of same length +double crossCorr(std::vector &x, std::vector &y) { + if (x.size() != y.size()) throw std::invalid_argument("x.size() != y.size()"); + double xs = 0.0, ys = 0.0, cs = 0.0; + for (size_t i = 0; i < x.size(); i++) { + xs += x[i] * x[i]; + ys += y[i] * y[i]; + cs += x[i] * y[i]; + } + return cs / sqrt(xs * ys); +} + +// clip 'val' to between 'a_min' and 'a_max'. +double clip(double val, double a_min, double a_max) { + return std::min(std::max(val, a_min), a_max); +} + +// two-dimensional mean of a collection of signals +void mean(std::vector &out, std::vector >& m) { + if (m.empty()) { + out.resize(0); + return; + } + const size_t sz = m[0].size(); + out.resize(sz); + out.assign(sz, 0.0); + const size_t N = m.size(); + for (size_t i = 0; i < N; i++) { + for (size_t j = 0; j < sz; j++) { + out[j] += m[i][j]; + } + } + for (size_t j = 0; j < sz; j++) { + out[j] /= static_cast(N); + } +} + } diff --git a/pasada-lib/ssf_filter.cpp b/pasada-lib/ssf_filter.cpp index e760d20..65c01b8 100644 --- a/pasada-lib/ssf_filter.cpp +++ b/pasada-lib/ssf_filter.cpp @@ -17,7 +17,7 @@ static std::vector make_ones(size_t sw) { SsfFilter::SsfFilter(size_t upslope_width) : sw(upslope_width), // Filt(N, shift, offset, taps) - f_delta_u(2, 0, 0, std::vector {1.0, -1.0}), + f_delta_u(2, 0, 0, std::vector {1.0, -1.0}), f_window(upslope_width, 0, 0, make_ones(upslope_width)) {} double SsfFilter::filter(double val) {