203 lines
7.7 KiB
C++
203 lines
7.7 KiB
C++
//
|
|
// Created by david on 03.03.2026.
|
|
//
|
|
|
|
#include "ssf_filter.h"
|
|
#include "pd_signal.h"
|
|
#include <limits>
|
|
#include <cassert>
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
|
|
#ifndef DEBUG_SSF
|
|
#define DEBUG_SSF 0
|
|
#endif
|
|
|
|
#if (DEBUG_SSF == 1)
|
|
#define DEBUG_PRINT(expr) do { expr; } while (0)
|
|
#else
|
|
#define DEBUG_PRINT(expr) while(0) { expr; }
|
|
#endif
|
|
|
|
|
|
static std::vector<double> make_ones(size_t sw) {
|
|
std::vector<double> ones;
|
|
ones.resize(sw);
|
|
ones.assign(sw, 1.0);
|
|
return ones;
|
|
}
|
|
|
|
SsfFilter::SsfFilter(size_t upslope_width) :
|
|
sw(upslope_width),
|
|
// Filt(N, shift, offset, taps)
|
|
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) {
|
|
double du = f_delta_u.filter(val);
|
|
double duc = std::max(0.0, du);
|
|
double ssf = f_window.filter(duc);
|
|
return ssf;
|
|
}
|
|
|
|
size_t SsfStepDetector::initial_samples() { return (size_t) (3.0 * FPS); }
|
|
|
|
SsfStepDetector::SsfStepDetector(size_t len_refr) :
|
|
// note: also change above, in initial_samples()
|
|
LEN_INIT((size_t) (3.0 * FPS)), // initial window length for ssf_threshold
|
|
LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
|
|
num_samples(0),
|
|
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),
|
|
ssf_nm1(0.0),
|
|
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");
|
|
}
|
|
double SsfStepDetector::filter(double ssf) {
|
|
double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN);
|
|
double rv = 0.0;
|
|
if (num_samples >= LEN_INIT) {
|
|
// initial and subsequent threshold setting.
|
|
ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
|
|
}
|
|
// threshold crossing detection
|
|
// '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
|
|
if (num_samples - n_refr >= len_refr) is_refr = false;
|
|
// transition and not in refractory period? detected a step.
|
|
if (is_txing && !is_refr) {
|
|
rv = 1.0;
|
|
is_refr = true;
|
|
n_refr = num_samples;
|
|
}
|
|
if (num_samples == LEN_INIT) {
|
|
// initial threshold setting
|
|
ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
|
|
//DEBUG_PRINT(std::cerr << "before prime()" << std::endl);
|
|
f_ssf_threshold_smoothing.prime(ssf_threshold);
|
|
} else if (num_samples > LEN_TH_WIN) {
|
|
//DEBUG_PRINT(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) {
|
|
//DEBUG_PRINT(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++;
|
|
return rv;
|
|
}
|
|
double SsfStepDetector::peek_threshold() {
|
|
return ssf_threshold;
|
|
}
|
|
|
|
|
|
void RunningQuality::addTemplate(std::vector<double>& 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);
|
|
}
|
|
|
|
void RunningQuality::replaceTemplate(std::vector<double>& x) {
|
|
beatTemplates.clear();
|
|
beatTemplates.emplace_back(x);
|
|
// essentially just a copy
|
|
pd_signal::mean(beatTemplate, beatTemplates);
|
|
}
|
|
|
|
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) {}
|
|
RunningQuality::~RunningQuality() {}
|
|
|
|
// note: arg should be an iterator really, but can do later
|
|
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;
|
|
std::vector<double> ssf;
|
|
pd_signal::resample(beat, rawBeat, BEAT_LEN);
|
|
pd_signal::resample(ssf, rawSsf, BEAT_LEN);
|
|
//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 posCorr = std::numeric_limits<double>::quiet_NaN();
|
|
bool goodBeat = false;
|
|
if (beatTemplates.size() > 0) {
|
|
corr = pd_signal::crossCorr(ssf, beatTemplate);
|
|
posCorr = pd_signal::clip(corr, 0.0, 1.0);
|
|
double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1;
|
|
goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf);
|
|
}
|
|
|
|
if (beatTemplates.size() == 0) {
|
|
// cannot correlate the first beat, no template yet
|
|
DEBUG_PRINT(std::cerr << "(0) first beat -> addTemplate()" << std::endl);
|
|
addTemplate(ssf);
|
|
justLocked = false;
|
|
} else if (beatTemplates.size() <= 2) {
|
|
// restart if there is no clear correlation between beats
|
|
if (goodBeat) {
|
|
DEBUG_PRINT(std::cerr << "(2) good initial beat -> addTemplate()" << std::endl);
|
|
addTemplate(ssf);
|
|
if (beatTemplates.size() > 2)
|
|
justLocked = true;
|
|
//DEBUG_PRINT(std::cerr << " (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl);
|
|
} else {
|
|
DEBUG_PRINT(std::cerr << "(2) bad initial beat idx=" << idx << " -> replaceTemplate() corr=" << std::fixed << std::setw(7) << std::setprecision(4) << corr << " checkedSsf=" << checkedSsf << std::endl);
|
|
replaceTemplate(ssf);
|
|
//badBeatRanges.clear();
|
|
justLocked = false;
|
|
}
|
|
} else {
|
|
// running mode: collect bad beats, but may be OK not to restart immediately
|
|
|
|
DEBUG_PRINT(std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl);
|
|
if (goodBeat) {
|
|
addTemplate(ssf);
|
|
} else {
|
|
// badBeatRanges.add(s, e)
|
|
// numNoisy++
|
|
}
|
|
// runningCorrs.add(posCorr)
|
|
if (justLocked) { dispatchLocked(); justLocked = false; }
|
|
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;
|
|
}
|