feat: StepDetector update FPS, more FPS fixes

google-tests/test3.cpp

@@ -94,8 +94,8 @@ protected:
     std::vector<bool> goods;
 
 public:
-    DebugRunningQuality(): lockedAt(-1), locked(false) {}
-    explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
+    DebugRunningQuality(double fps): RunningQuality(fps), lockedAt(-1), locked(false) {}
+    explicit DebugRunningQuality(double fps, bool disableSsf): RunningQuality(fps, disableSsf), locked(false) {}
     virtual ~DebugRunningQuality() {}
     bool isLocked() { return locked; }
     std::vector<double> getCorrs() { return corrs; }
@@ -126,7 +126,8 @@ TEST(SignalTest, resample_same_len) {
 */
 
 TEST(SignalTest, RunningQuality_t1) {
-    DebugRunningQuality sqi(true);
+    double fps = 60.0;
+    DebugRunningQuality sqi(fps, true);
     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};
@@ -194,7 +195,7 @@ TEST(SignalTest, RunningQuality_t2) {
 
     // Debug SQI
 
-    DebugRunningQuality sqi;
+    DebugRunningQuality sqi(fps);
 
     std::vector<double> beat_buf;
     std::vector<double> ssf_buf;

google-tests/test4.cpp

@@ -20,12 +20,13 @@ TEST(StepDetector, t1_sub_sample_resolution) {
     StepDetector det(fps, nullptr, true);
 
     // initialize: feed for priming the filters
-    det.primeFilters(fps, signal);
+    double ts = det.primeFilters(fps, signal);
 
     // feed for actual test
     for (size_t i = 0; i < N; i++) {
         const auto a_i = static_cast<float>(signal[i]);
-        det.filter(std::vector<float> {0.0f, a_i, 0.0f});
+        det.filter(ts * 1e9, std::vector<float> {0.0f, a_i, 0.0f});
+        ts += 1.0 / fps;
     }
 
     std::vector<double> ssd = det.getBufSsd(); // raw SsfStepDetector

pasada-lib/include/pd_signal.h

@@ -44,6 +44,8 @@ namespace pd_signal {
     double mean(const std::vector<double>& in);
     void diff(std::vector<double>& out, const std::vector<double>& in);
 
+    std::vector<double> gauss(size_t N, double mu, double sigma);
+
     /**
      * Convolution of two polynomials given in ASCENDING power order.
      * If <c>p = p_0 + p_1 x + ... + p_{P-1} x^{P-1}</c> and likewise for q,

pasada-lib/include/ssf_filter.h

@@ -33,8 +33,8 @@ public:
  */
 class SsfStepDetector {
 protected:
-    const size_t LEN_INIT;
-    const size_t LEN_TH_WIN;
+    size_t LEN_INIT;
+    size_t LEN_TH_WIN;
     size_t num_samples;
     double ssf_threshold;
     double ssf_threshold_nm1;
@@ -50,6 +50,7 @@ public:
      * @param len_refr duration of refractory period, in samples
      */
     SsfStepDetector(double fps);
+    ~SsfStepDetector();
     double filter(double val);
     double peek_threshold();
 
@@ -62,18 +63,19 @@ public:
 class RunningQuality {
 protected:
     // TODO: make it a filter (output proper samples)
+    // TODO: use fps info
 
     /** template beat is resampled to this #samples */
-    const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */;
+    int BEAT_LEN;
 
     /** threshold for accepting initial beats */
-    const double BEAT_CORR_THR_1 = 0.9;
+    double BEAT_CORR_THR_1 = 0.9;
     /** threshold for accepting subsequent beats */
-    const double BEAT_CORR_THR_2 = 0.8;
+    double BEAT_CORR_THR_2 = 0.8;
     /** absolute SSF threshold for accepting any beat */
-    const double SSF_THRESHOLD = 5.0;
+    double SSF_THRESHOLD = 5.0;
     /** number of recent beats to use for beat template. must be even (alternating feet have different patterns; make it symmetric) */
-    const int    NUM_BEATS = 4;
+    int    NUM_BEATS = 4;
 
     std::deque<std::vector<double> > beatTemplates;
     std::vector<double> beatTemplate;
@@ -93,8 +95,8 @@ protected:
     virtual void dispatchBeat(int idx, bool good, double posCorr);
 
 public:
-    RunningQuality();
-    explicit RunningQuality(bool disableSsf);
+    RunningQuality(double fps);
+    explicit RunningQuality(double fps, bool disableSsf);
     virtual ~RunningQuality();
 
     // note: arg should be an iterator really, but can do later
@@ -116,6 +118,7 @@ protected:
     double sqi;
 public:
     RunningQualityFilter(double fps);
+    ~RunningQualityFilter();
     double filter(double y, double ssf, double step);
 };
 

pasada-lib/include/step_detector.h

@@ -7,6 +7,7 @@
 
 #include "iir_filter.h"
 #include "ssf_filter.h"
+#include "pd_resamp.h"
 #include <vector>
 
 class StepListener {
@@ -15,6 +16,19 @@ public:
     virtual void playBeat() = 0;
 };
 
+/** mean-filter the gravity vector, then take acceleration downwards */
+class GravityFilter {
+    size_t N;
+    std::vector<double> gauss_taps;
+    Filt gx;
+    Filt gy;
+    Filt gz;
+public:
+    // 5 secs buffer, prime y with direction of gravity (for tests & faster init)
+    GravityFilter(double fps);
+    double filter(std::vector<double> values);
+};
+
 /**
  * Step detector from accelerometer signal.
  *
@@ -24,7 +38,7 @@ public:
 class StepDetector {
 protected:
     StepListener *listener;
-    Filt f_neg;
+    GravityFilter f_grav;
     SsfFilter f_ssf;
     SsfStepDetector f_ssd;
     RunningQualityFilter f_sqi;
@@ -34,9 +48,15 @@ protected:
     std::vector<double> buf_sqi;
     std::vector<double> buf_out;
 
+    Resampler res_x;
+    Resampler res_y;
+    Resampler res_z;
+    double fps;
+
 public:
     StepDetector(double fps, StepListener *listener, bool debug = false);
-    void filter(std::vector<float> values);
+    void filter(double ts, std::vector<float> values);
+    void filter_a(double s1);
     std::vector<double> getBufSsd();
     std::vector<double> getBufSqi();
     std::vector<double> getBufOut();
@@ -45,7 +65,7 @@ public:
      * Prime the filters using the given input signal.
      * Used for debugging (non-realtime processing) to align the signal.
      */
-    void primeFilters(double fps, std::vector<double> sig);
+    double primeFilters(double fps, std::vector<double> sig);
 };
 
 #endif //PASADASUPERPROJECT_STEP_DETECTOR_H

pasada-lib/pd_signal.cpp

@@ -161,6 +161,16 @@ void diff(std::vector<double>& out, const std::vector<double>& in) {
     }
 }
 
+std::vector<double> gauss(size_t N, double mu, double sigma) {
+    const double norm = sigma * sqrt(2.0 * kPi);
+    std::vector<double> data(N);
+    for (int i = 0; i < N; i++) {
+        const double x = i;
+        data[i] = std::exp(-0.5 * (x - mu) * (x - mu) / (sigma * sigma)) / norm;
+    }
+    return data;
+}
+
 // Convolution of two polynomials in ascending power order.
 void polymul(std::vector<cplx>& out,
              const std::vector<cplx>& p, const std::vector<cplx>& q) {

pasada-lib/ssf_filter.cpp

@@ -86,6 +86,7 @@ SsfStepDetector::SsfStepDetector(double fps) :
 {
     assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold");
 }
+SsfStepDetector::~SsfStepDetector() {}
 double SsfStepDetector::filter(double ssf) {
     double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN);
     double rv = 0.0;
@@ -154,8 +155,13 @@ void RunningQuality::replaceTemplate(std::vector<double>& x) {
 void RunningQuality::dispatchLocked() { /* implement me, add Listener etc. */ }
 void RunningQuality::dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }
 
-RunningQuality::RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
-RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
+static int get_beat_len(double fps) {
+    /* 2*FPS for 30 bpm lower end */
+    return 2.0 * fps;
+}
+
+RunningQuality::RunningQuality(double fps): BEAT_LEN(get_beat_len(fps)), beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
+RunningQuality::RunningQuality(double fps, bool disableSsf): BEAT_LEN(get_beat_len(fps)), beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
 RunningQuality::~RunningQuality() {}
 
 // note: arg should be an iterator really, but can do later
@@ -221,7 +227,8 @@ bool RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &r
 }
 
 
-RunningQualityFilter::RunningQualityFilter(double fps) : sqi(0.0) {}
+RunningQualityFilter::RunningQualityFilter(double fps) : f_sqi(fps), sqi(0.0) {}
+RunningQualityFilter::~RunningQualityFilter() {}
 
 double RunningQualityFilter::filter(double y, double ssf, double step) {
     if (step == 1.0) {

pasada-lib/step_detector.cpp

@@ -4,19 +4,72 @@
 
 #include "step_detector.h"
 
+#include "pd_signal.h"
+
 StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
     listener(listener),
-    f_neg(1, 0, 0, std::vector<double> {-1.0}),
+    f_grav(fps),
     f_ssf(fps),
     f_ssd(fps),
     f_sqi(fps),
-    debug(debug)
+    debug(debug),
+    fps(0.0)
 {}
 
-void StepDetector::filter(std::vector<float> values) {
-    // TODO: later on, we should use a vector projection towards gravity
-    auto s1 = (double) values[1]; // take y-axis value for now
-    auto s2 = f_neg.filter(s1);
+static int gravity_num_taps(double fps) {
+    return 5.0 * fps;
+}
+
+// 5 secs buffer, prime y with direction of gravity (for tests & faster init)
+GravityFilter::GravityFilter(double fps) :
+    N(gravity_num_taps(fps)),
+    gauss_taps(pd_signal::gauss(N, N/2, N/4)),
+    gx(N, 0, 0, gauss_taps),
+    gy(N, 0, 0, gauss_taps),
+    gz(N, 0, 0, gauss_taps)
+{
+    gy.prime(-9.81);
+}
+
+double GravityFilter::filter(std::vector<double> values) {
+    gx.push(values[0]);
+    gy.push(values[1]);
+    gz.push(values[2]);
+    double x = gx.peek(), y = gy.peek(), z = gz.peek();
+    double g = sqrt(x * x + y * y + z * z);
+    // e = mean(a)
+    double ex = x / g, ey = y / g, ez = z / g;
+    // e \in a
+    double vx = values[0] * ex;
+    double vy = values[1] * ey;
+    double vz = values[2] * ez;
+    return vx + vy + vz;
+}
+
+void StepDetector::filter(double ts, std::vector<float> values) {
+    // resample to smooth over Android sensor FPS variations
+    res_x.push(ts, values[0]);
+    res_y.push(ts, values[1]);
+    res_z.push(ts, values[2]);
+    // as soon as there is 'fps' information, re-init the classes requiring fps info
+    if (fps == 0.0 && res_x.peek()) {
+        fps = res_x.get_fs();
+        f_grav = GravityFilter(fps);
+        f_ssf = SsfFilter(fps);
+        f_ssd = SsfStepDetector(fps);
+        f_sqi = RunningQualityFilter(fps);
+    }
+    while (res_x.peek()) {
+        double x = res_x.get(), y = res_y.get(), z = res_z.get();
+        std::vector<double> samp { x, y, z };
+        // gravity filtering
+        double a = f_grav.filter(samp);
+        // pass on accel sample
+        filter_a(a);
+    }
+}
+
+void StepDetector::filter_a(double s2) {
     auto s3 = f_ssf.filter(s2);
     auto s4 = f_ssd.filter(s3);
     auto q5 = f_sqi.filter(s2, s3, s4);
@@ -35,15 +88,18 @@ std::vector<double> StepDetector::getBufSsd() { return buf_ssd; }
 std::vector<double> StepDetector::getBufSqi() { return buf_sqi; }
 std::vector<double> StepDetector::getBufOut() { return buf_out; }
 
-void StepDetector::primeFilters(double fps, std::vector<double> sig) {
+double StepDetector::primeFilters(double fps, std::vector<double> sig) {
     const size_t N_INIT = SsfStepDetector::initial_samples(fps);
     // initialize: feed for priming the filters
+    double ts = 0;
     for (size_t i = 0; i < N_INIT; i++) {
         const auto a_i = static_cast<float>(sig[i]);
-        filter(std::vector<float> {0.0f, a_i, 0.0f});
+        filter(ts * 1e9, std::vector<float> {0.0f, a_i, 0.0f});
+        ts += 1.0 / fps;
     }
     // clear debug buffers
     buf_ssd.clear();
     buf_sqi.clear();
     buf_out.clear();
+    return ts;
 }