66 lines
2.0 KiB
C++
66 lines
2.0 KiB
C++
//
|
|
// Created by david on 03.03.2026.
|
|
//
|
|
|
|
#include "include/ssf_filter.h"
|
|
#include <limits>
|
|
#include <cmath>
|
|
#include <cassert>
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
SsfStepDetector::SsfStepDetector(size_t len_refr) :
|
|
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()),
|
|
len_refr(len_refr), n_refr(0), is_refr(false),
|
|
nm1_ssf(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 val) {
|
|
double ssf_mean = f_ssf_mean.filter(val) / ((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
|
|
bool is_txing = nm1_ssf < ssf_threshold && val >= ssf_threshold;
|
|
// 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;
|
|
}
|
|
nm1_ssf = val;
|
|
num_samples++;
|
|
return rv;
|
|
}
|
|
double SsfStepDetector::peek_threshold() {
|
|
return ssf_threshold;
|
|
}
|