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

feat: iterate on SsfStepDetector

Browse Source 
* use SSF signal instead of accelerometer signal
* use higher BEAT_CORR_THR_{12} for SSF signal
* add absolute SSF_THRESHOLD to ignore small accelero bumps
* compute ssf_threshold according to detected SSF peaks, not the mean (more robust vs. noise)
This commit is contained in:
David Madl
2026-03-11 20:47:53 +01:00
parent 95d1fee44d
commit 90f8943930
8 changed files with 103 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
google-tests/test2.cpp
View File

@@ -190,26 +190,36 @@ TEST(HelloTest, Zong_SSF_Stage3) {
std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171}; std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171};
IirFilter filter(b, a); IirFilter filter(b, a);
//std::cerr << "before stage 1" << std::endl;
// Stage 1: high-pass // Stage 1: high-pass
auto y = apply_filter(filter, signal); auto y = apply_filter(filter, signal);
Filt f_neg(1, 0, 0, std::vector {-1.0}); Filt f_neg(1, 0, 0, std::vector {-1.0});
auto y_neg = apply_filter(f_neg, y); auto y_neg = apply_filter(f_neg, y);
//std::cerr << "before stage 2" << std::endl;
// Stage 2: sum slope function // Stage 2: sum slope function
const size_t upslope_width = 4; const size_t upslope_width = 4;
SsfFilter f_ssf(upslope_width); SsfFilter f_ssf(upslope_width);
auto ssf = apply_filter(f_ssf, y_neg); auto ssf = apply_filter(f_ssf, y_neg);
//std::cerr << "before stage 3" << std::endl;
// Stage 3: threshold detection // Stage 3: threshold detection
const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60)); const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
DebugSsfStepDetectorThreshold f_ssd_thr(len_refr); DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
auto ssf_threshold = apply_filter(f_ssd_thr, ssf); auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
//std::cerr << "before writing results 1 and doing step detection" << std::endl;
npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold); npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold);
SsfStepDetector f_ssd(len_refr); SsfStepDetector f_ssd(len_refr);
auto steps = apply_filter(f_ssd, ssf); auto steps = apply_filter(f_ssd, ssf);
//std::cerr << "before writing results 2" << std::endl;
npy_save("test2/ssf_t2_steps.npy", steps); npy_save("test2/ssf_t2_steps.npy", steps);
} }

50
google-tests/test3.cpp
View File

@@ -3,6 +3,7 @@
// //
#include <gtest/gtest.h> #include <gtest/gtest.h>
//#include <utility> //#include <utility>
#include <deque>
#include "pd_signal.h" #include "pd_signal.h"
using namespace pd_signal; using namespace pd_signal;
@@ -42,29 +43,42 @@ TEST(SignalTest, ranges) {
*/ */
class RunningQuality { class RunningQuality {
protected: protected:
// TODO: make it a filter (output proper samples)
/** template beat is resampled to this #samples */ /** template beat is resampled to this #samples */
const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */; const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */;
/** threshold for accepting initial beats */ /** threshold for accepting initial beats */
const double BEAT_CORR_THR_1 = 0.8; const double BEAT_CORR_THR_1 = 0.9;
/** threshold for accepting subsequent beats */ /** threshold for accepting subsequent beats */
const double BEAT_CORR_THR_2 = 0.6; const double BEAT_CORR_THR_2 = 0.8;
/** absolute SSF threshold for accepting any beat */
const 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;
std::vector<std::vector<double> > beatTemplates; std::deque<std::vector<double> > beatTemplates;
std::vector<double> beatTemplate; std::vector<double> beatTemplate;
//std::vector<std::pair<int, int> > badBeatRanges; //std::vector<std::pair<int, int> > badBeatRanges;
double beatCorrThr2; double beatCorrThr2;
bool justLocked; bool justLocked;
int idx; int idx;
/** for debugging only - disable SSF_THRESHOLD */
bool disableSsf;
void addTemplate(std::vector<double>& x) { void addTemplate(std::vector<double>& x) {
beatTemplates.push_back(x); beatTemplates.emplace_back(x);
while (beatTemplates.size() > NUM_BEATS) {
// sliding window on 'beat_templates', do not use all history
beatTemplates.pop_front();
}
pd_signal::mean(beatTemplate, beatTemplates); pd_signal::mean(beatTemplate, beatTemplates);
} }
void replaceTemplate(std::vector<double>& x) { void replaceTemplate(std::vector<double>& x) {
beatTemplates.clear(); beatTemplates.clear();
beatTemplates.push_back(x); beatTemplates.emplace_back(x);
// essentially just a copy // essentially just a copy
pd_signal::mean(beatTemplate, beatTemplates); pd_signal::mean(beatTemplate, beatTemplates);
} }
@@ -73,28 +87,35 @@ protected:
virtual void dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ } virtual void dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }
public: public:
RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0) {} RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
explicit RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
virtual ~RunningQuality() {} virtual ~RunningQuality() {}
// note: arg should be an iterator really, but can do later // note: arg should be an iterator really, but can do later
/** /**
* @param beat individual beat accelero signal * @param beat individual beat accelero signal
*/ */
void append(std::vector<double> &rawBeat) { void append(std::vector<double> &rawBeat, std::vector<double> &rawSsf) {
// TODO: should ignore crazy-long and very short beats here. (filter up on beat detector) // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
std::vector<double> beat; std::vector<double> beat;
std::vector<double> ssf;
resample(beat, rawBeat, BEAT_LEN); resample(beat, rawBeat, BEAT_LEN);
resample(ssf, rawSsf, BEAT_LEN);
//std::ranges::copy(rawBeat, std::back_inserter(beat)); //std::ranges::copy(rawBeat, std::back_inserter(beat));
// check ssf at sample 2 (mid-slope of 4 window of ssf)
// TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding
double checkedSsf = ssf[(int) (2*((double)beat.size())/((double)rawBeat.size()))];
double corr = std::numeric_limits<double>::quiet_NaN(); double corr = std::numeric_limits<double>::quiet_NaN();
double posCorr = std::numeric_limits<double>::quiet_NaN(); double posCorr = std::numeric_limits<double>::quiet_NaN();
bool goodBeat = false; bool goodBeat = false;
if (beatTemplates.size() > 0) { if (beatTemplates.size() > 0) {
corr = pd_signal::crossCorr(beat, beatTemplate); corr = pd_signal::crossCorr(ssf, beatTemplate);
posCorr = pd_signal::clip(corr, 0.0, 1.0); posCorr = pd_signal::clip(corr, 0.0, 1.0);
double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1; double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1;
goodBeat = (corr > corrThreshold); goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf);
} }
if (beatTemplates.size() == 0) { if (beatTemplates.size() == 0) {
@@ -144,6 +165,7 @@ protected:
public: public:
DebugRunningQuality(): locked(false) {} DebugRunningQuality(): locked(false) {}
explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
virtual ~DebugRunningQuality() {} virtual ~DebugRunningQuality() {}
bool isLocked() { return locked; } bool isLocked() { return locked; }
std::vector<double> getCorrs() { return corrs; } std::vector<double> getCorrs() { return corrs; }
@@ -172,16 +194,16 @@ TEST(SignalTest, resample_same_len) {
*/ */
TEST(SignalTest, RunningQuality_t1) { TEST(SignalTest, RunningQuality_t1) {
DebugRunningQuality sqi; DebugRunningQuality sqi(true);
std::vector a {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1}; 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 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 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}; std::vector d {0.0, 0.3, 0.9, 1.0, 0.7, 0.4, 0.1};
sqi.append(a); sqi.append(a, a);
sqi.append(b); sqi.append(b, b);
sqi.append(c); sqi.append(c, c);
EXPECT_FALSE(sqi.isLocked()); EXPECT_FALSE(sqi.isLocked());
sqi.append(d); sqi.append(d, d);
EXPECT_TRUE(sqi.isLocked()); EXPECT_TRUE(sqi.isLocked());
ASSERT_EQ(1, sqi.getCorrs().size()); 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 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

14
pasada-lib/iir_filter.cpp
View File

@@ -13,13 +13,13 @@
#define DEBUG_PRINT(expr) while(0) { expr; } #define DEBUG_PRINT(expr) while(0) { expr; }
#endif #endif
Buf::Buf(size_t N): size(N), n(0) { Buf::Buf(size_t N): N(N), n(0) {
data.resize(N); data.resize(N);
data.assign(N, 0.0); data.assign(N, 0.0);
} }
void Buf::push(double val) { void Buf::push(double val) {
data[n] = val; data[n] = val;
n = (n+1) % size; n = (n+1) % N;
} }
Filt::Filt(size_t N, size_t shift, size_t offset, std::vector<double> taps): Buf(N), shift(shift), offset(offset), taps(taps) { Filt::Filt(size_t N, size_t shift, size_t offset, std::vector<double> taps): Buf(N), shift(shift), offset(offset), taps(taps) {
@@ -31,9 +31,9 @@ double Filt::filter(double val) {
} }
double Filt::peek() { double Filt::peek() {
double sum = 0; double sum = 0;
for (size_t i = offset; i < this->size; i++) { for (size_t i = offset; i < this->N; i++) {
//size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative //size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative
size_t n = (this->size + this->n - i + shift - 1) % this->size; size_t n = (this->N + this->n - i + shift - 1) % this->N;
DEBUG_PRINT(std::cout << " t[" << i << "] * v[" << n << "]" << std::endl); DEBUG_PRINT(std::cout << " t[" << i << "] * v[" << n << "]" << std::endl);
sum += this->data[n] * this->taps[i]; sum += this->data[n] * this->taps[i];
} }
@@ -42,6 +42,12 @@ double Filt::peek() {
void Filt::push(double val) { void Filt::push(double val) {
Buf::push(val); Buf::push(val);
} }
void Filt::prime(double val) {
data.assign(this->N, val);
}
size_t Filt::size() {
return this->N;
}
IirFilter::IirFilter(std::vector<double> b, std::vector<double> a) : x(b.size(), 0, 0, b), y(a.size(), 1, 1, a) { IirFilter::IirFilter(std::vector<double> b, std::vector<double> a) : x(b.size(), 0, 0, b), y(a.size(), 1, 1, a) {
if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()"); if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()");

7
pasada-lib/include/iir_filter.h
View File

@@ -13,7 +13,7 @@
class Buf { class Buf {
protected: protected:
std::vector<double> data; std::vector<double> data;
size_t size; size_t N;
size_t n; size_t n;
public: public:
Buf(size_t N); Buf(size_t N);
@@ -21,7 +21,7 @@ public:
}; };
/** Running filter base. */ /** Running filter base. */
class Filt : Buf { class Filt : public Buf {
protected: protected:
std::vector<double> taps; std::vector<double> taps;
size_t shift; size_t shift;
@@ -31,6 +31,9 @@ public:
double filter(double val); double filter(double val);
double peek(); double peek();
void push(double val); void push(double val);
/** prime the filter by overwriting the entire buffer with 'val' */
void prime(double val);
size_t size();
}; };
/** Running IIR filter. */ /** Running IIR filter. */

3
pasada-lib/include/pd_signal.h
View File

@@ -6,6 +6,7 @@
#define PASADASUPERPROJECT_SIGNAL_H #define PASADASUPERPROJECT_SIGNAL_H
#include <vector> #include <vector>
#include <deque>
namespace pd_signal { namespace pd_signal {
/** `num` evenly spaced numbers over interval [start,stop] */ /** `num` evenly spaced numbers over interval [start,stop] */
@@ -33,6 +34,8 @@ namespace pd_signal {
/** two-dimensional mean of a collection of signals */ /** two-dimensional mean of a collection of signals */
void mean(std::vector<double> &out, std::vector<std::vector<double> >& m); void mean(std::vector<double> &out, std::vector<std::vector<double> >& m);
/** two-dimensional mean of a collection of signals */
void mean(std::vector<double> &out, std::deque<std::vector<double> >& m);
} }

4
pasada-lib/include/ssf_filter.h
View File

@@ -37,10 +37,12 @@ protected:
const size_t LEN_TH_WIN; const size_t LEN_TH_WIN;
size_t num_samples; size_t num_samples;
double ssf_threshold; double ssf_threshold;
double ssf_threshold_nm1;
Filt f_ssf_threshold_smoothing;
size_t len_refr; size_t len_refr;
size_t n_refr; size_t n_refr;
bool is_refr; bool is_refr;
double nm1_ssf; double ssf_nm1;
Filt f_ssf_mean; Filt f_ssf_mean;
public: public:
/** /**

9
pasada-lib/pd_signal.cpp
View File

@@ -113,7 +113,7 @@ double clip(double val, double a_min, double a_max) {
} }
// two-dimensional mean of a collection of signals // two-dimensional mean of a collection of signals
void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) { template<class T> void mean_tpl(std::vector<double> &out, T& m) {
if (m.empty()) { if (m.empty()) {
out.resize(0); out.resize(0);
return; return;
@@ -132,4 +132,11 @@ void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) {
} }
} }
void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) {
mean_tpl(out, m);
}
void mean(std::vector<double> &out, std::deque<std::vector<double> >& m) {
mean_tpl(out, m);
}
} }

33
pasada-lib/ssf_filter.cpp
View File

@@ -6,6 +6,7 @@
#include <limits> #include <limits>
#include <cmath> #include <cmath>
#include <cassert> #include <cassert>
#include <iostream>
static std::vector<double> make_ones(size_t sw) { static std::vector<double> make_ones(size_t sw) {
std::vector<double> ones; std::vector<double> ones;
@@ -33,21 +34,26 @@ SsfStepDetector::SsfStepDetector(size_t len_refr) :
LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
num_samples(0), num_samples(0),
ssf_threshold(std::numeric_limits<double>::infinity()), ssf_threshold(std::numeric_limits<double>::infinity()),
ssf_threshold_nm1(std::numeric_limits<double>::infinity()),
f_ssf_threshold_smoothing(6, 0, 0, make_ones(6)),
len_refr(len_refr), n_refr(0), is_refr(false), len_refr(len_refr), n_refr(0), is_refr(false),
nm1_ssf(0.0), ssf_nm1(0.0),
f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN)) f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN))
{ {
assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold"); assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold");
} }
double SsfStepDetector::filter(double val) { double SsfStepDetector::filter(double ssf) {
double ssf_mean = f_ssf_mean.filter(val) / ((double) LEN_TH_WIN); double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN);
double rv = 0.0; double rv = 0.0;
if (num_samples >= LEN_INIT) { if (num_samples >= LEN_INIT) {
// initial and subsequent threshold setting. // initial and subsequent threshold setting.
ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
} }
// threshold crossing detection // threshold crossing detection
bool is_txing = nm1_ssf < ssf_threshold && val >= ssf_threshold; // 'is_prev_lower' fixes a glitch where a falling threshold leads to undetected crossings
bool is_prev_lower = ssf_nm1 < ssf_threshold || ssf_nm1 < ssf_threshold_nm1;
bool is_cur_higher = ssf >= ssf_threshold;
bool is_txing = is_prev_lower && is_cur_higher;
// refractory period reset // refractory period reset
if (num_samples - n_refr >= len_refr) is_refr = false; if (num_samples - n_refr >= len_refr) is_refr = false;
// transition and not in refractory period? detected a step. // transition and not in refractory period? detected a step.
@@ -56,7 +62,24 @@ double SsfStepDetector::filter(double val) {
is_refr = true; is_refr = true;
n_refr = num_samples; n_refr = num_samples;
} }
nm1_ssf = val; if (num_samples == LEN_INIT) {
// initial threshold setting
ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
//std::cerr << "before prime()" << std::endl;
f_ssf_threshold_smoothing.prime(ssf_threshold);
} else if (num_samples > LEN_TH_WIN) {
//std::cerr << "adaptive threshold setting" << std::endl;
// adaptive threshold setting
// +2 is half the window size
// TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding -- also f_ssf_threshold_smoothing(), nb. should be even number
if (num_samples == n_refr + 2) {
//std::cerr << "setting adaptive threshold setting" << std::endl;
ssf_threshold_nm1 = ssf_threshold;
// the ssf peak comes 3 samples (half-window + 1 sample) after the crossing
ssf_threshold = f_ssf_threshold_smoothing.filter(ssf) / ((double) f_ssf_threshold_smoothing.size()) * 0.6;
}
}
ssf_nm1 = ssf;
num_samples++; num_samples++;
return rv; return rv;
} }