feat: RunningQuality - running SQI

google-tests/test3.cpp

@@ -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);
+}

pasada-lib/include/pd_signal.h

@@ -20,7 +20,19 @@ namespace pd_signal {
     void interp(std::vector<double>& y, std::vector<double>& x, std::vector<double>& xp, std::vector<double>& fp);
 
     /** resample to BEAT_LEN */
-    void resample(std::vector<double> &out, std::vector<double> x, int beat_len);
+    void resample(std::vector<double> &out, std::vector<double> &x, int beat_len);
+
+    /**
+     * normalized cross-correlation of the two signals of same length.
+     * normalization factor is <c>1 / sqrt(\sum_i x_i^2 * \sum_i y_i^2)</c>
+     */
+    double crossCorr(std::vector<double> &x, std::vector<double> &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<double> &out, std::vector<std::vector<double> >& m);
 
 }
 

pasada-lib/pd_signal.cpp

@@ -5,6 +5,7 @@
 #include "include/pd_signal.h"
 #include <stdexcept>
 #include <algorithm>
+#include <iostream>
 
 namespace pd_signal {
 
@@ -86,7 +87,7 @@ void interp(std::vector<double>& y, std::vector<double>& x, std::vector<double>&
 }
 
 // resample to BEAT_LEN
-void resample(std::vector<double> &out, std::vector<double> x, int beat_len) {
+void resample(std::vector<double> &out, std::vector<double> &x, int beat_len) {
     std::vector<double> t;
     std::vector<double> i;
     linspace(t, 0, (double) x.size(), beat_len, false);
@@ -94,4 +95,41 @@ void resample(std::vector<double> &out, std::vector<double> x, int beat_len) {
     interp(out, t, i, x);
 }
 
+// normalized cross-correlation of the two signals of same length
+double crossCorr(std::vector<double> &x, std::vector<double> &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<double> &out, std::vector<std::vector<double> >& 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<double>(N);
+    }
+}
+
 }

pasada-lib/ssf_filter.cpp

@@ -17,7 +17,7 @@ static std::vector<double> 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<double> {1.0, -1.0}),
     f_window(upslope_width, 0, 0, make_ones(upslope_width))
 {}
 double SsfFilter::filter(double val) {