feat: RQF: filter() style interface RunningQuality

google-tests/test3.cpp

@@ -81,19 +81,26 @@ TEST(SignalTest, ranges) {
 class DebugRunningQuality : public RunningQuality {
 protected:
     virtual void dispatchLocked() { locked = true; }
-    virtual void dispatchBeat(int idx, bool good, double posCorr) { goods.push_back(good); corrs.push_back(posCorr); }
+    virtual void dispatchBeat(int idx, bool good, double posCorr) {
+        if (locked && lockedAt == -1)
+            lockedAt = idx;
+        goods.push_back(good);
+        corrs.push_back(posCorr);
+    }
 
+    int lockedAt;
     bool locked;
     std::vector<double> corrs;
     std::vector<bool> goods;
 
 public:
-    DebugRunningQuality(): locked(false) {}
+    DebugRunningQuality(): lockedAt(-1), locked(false) {}
     explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
     virtual ~DebugRunningQuality() {}
     bool isLocked() { return locked; }
     std::vector<double> getCorrs() { return corrs; }
     std::vector<bool> getGoods() { return goods; }
+    int getLockedAt() { return lockedAt; }
     std::vector<double> getBeatTemplate() { return this->beatTemplate; }
 };
 
@@ -210,6 +217,9 @@ TEST(SignalTest, RunningQuality_t2) {
     EXPECT_TRUE(sqi.isLocked());
     EXPECT_TRUE(sqi.getCorrs().size() > 50);
 
+    EXPECT_TRUE(sqi.getLockedAt() < 10);
+    std::cout << "lockedAt=" << sqi.getLockedAt() << std::endl;
+
     std::vector<double> corrs(sqi.getCorrs());
     npy_save("test3/ssf_t3_sqi_corrs.npy", corrs);
 

pasada-lib/include/ssf_filter.h

@@ -98,8 +98,23 @@ public:
     // note: arg should be an iterator really, but can do later
     /**
      * @param beat individual beat accelero signal
+     * @return true if it is good beat
      */
-    void append(std::vector<double> &rawBeat, std::vector<double> &rawSsf);
+    bool append(std::vector<double> &rawBeat, std::vector<double> &rawSsf);
+};
+
+/**
+ * Signal quality indicator.
+ */
+class RunningQualityFilter {
+protected:
+    RunningQuality f_sqi;
+    std::vector<double> beat_buf;
+    std::vector<double> ssf_buf;
+    double sqi;
+public:
+    RunningQualityFilter(size_t upslope_width);
+    double filter(double y, double ssf, double step);
 };
 
 #endif //PASADASUPERPROJECT_SSF_FILTER_H

pasada-lib/ssf_filter.cpp

@@ -124,7 +124,7 @@ RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2),
 RunningQuality::~RunningQuality() {}
 
 // note: arg should be an iterator really, but can do later
-void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &rawSsf) {
+bool RunningQuality::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;
@@ -181,4 +181,20 @@ void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &r
         dispatchBeat(idx, goodBeat, posCorr);
     }
     idx++;
+    if (!goodBeat) return 0.0;
+    return posCorr;
+}
+
+
+RunningQualityFilter::RunningQualityFilter(size_t upslope_width) : sqi(0.0) {}
+
+double RunningQualityFilter::filter(double y, double ssf, double step) {
+    if (step == 1.0) {
+        sqi = f_sqi.append(beat_buf, ssf_buf);
+        beat_buf.clear();
+        ssf_buf.clear();
+    }
+    beat_buf.push_back(y);
+    ssf_buf.push_back(ssf);
+    return sqi;
 }