From d4e02415906fab56113b5295a9d430f1cd35a9d0 Mon Sep 17 00:00:00 2001 From: David Madl Date: Tue, 19 May 2026 21:57:27 +0200 Subject: [PATCH] feat: Resampler: Normalizes incoming Android sensor sampling rate --- google-tests/CMakeLists.txt | 1 + google-tests/test6.cpp | 122 +++++++++++++++++++++++++++++++++ pasada-lib/CMakeLists.txt | 1 + pasada-lib/include/pd_resamp.h | 51 ++++++++++++++ pasada-lib/include/pd_signal.h | 4 ++ pasada-lib/pd_resamp.cpp | 103 ++++++++++++++++++++++++++++ pasada-lib/pd_signal.cpp | 20 ++++++ 7 files changed, 302 insertions(+) create mode 100644 google-tests/test6.cpp create mode 100644 pasada-lib/include/pd_resamp.h create mode 100644 pasada-lib/pd_resamp.cpp diff --git a/google-tests/CMakeLists.txt b/google-tests/CMakeLists.txt index cd8bbc4..b43f8e9 100644 --- a/google-tests/CMakeLists.txt +++ b/google-tests/CMakeLists.txt @@ -15,6 +15,7 @@ add_executable(Google_Tests_run test3.cpp test4.cpp test5.cpp + test6.cpp ) file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) diff --git a/google-tests/test6.cpp b/google-tests/test6.cpp new file mode 100644 index 0000000..4cbaaeb --- /dev/null +++ b/google-tests/test6.cpp @@ -0,0 +1,122 @@ +// +// Created by david on 19.05.2026. +// + +#include +#include +#include +#include +#include "pd_resamp.h" +#include "pd_signal.h" +#include "test_helpers.h" + +#define M_PI 3.14159265358979323846 + +void make_test_signal_1(int N, std::vector &ts, std::vector &sig) { + double f = 10.0; + double fs = 100.0; + pd_signal::linspace(ts, 0.0, (N-1) / fs * 1e9, N, false); + sig.resize(N); + for (int i = 0; i < N; i++) { + sig[i] = std::cos(2 * M_PI * f * i / fs); + } +} + +void add_noise(std::vector& x, double mu, double sigma) { + if (sigma < 0.0) { throw std::invalid_argument("sigma must be non-negative"); } + std::mt19937 rng { 42 }; /* std::random_device{}() */ + std::normal_distribution dist(mu, sigma); + for (double& v : x) { + v += dist(rng); + } +} + +int make_spiky_times(int N_hint, std::vector& ts_in, std::vector& ts_out, std::vector &sig_in, std::vector &sig_out) { + double fs = 100.0; + // note that resulting indices will be 100 + halfdist, because of sampling rate change at i=100 => 120, 139, 149 + std::vector spikes {140, 178, 198}; + std::vector rel_spikes { 1.8, 5.6, 2.51 }; + int N = N_hint + static_cast(std::accumulate(rel_spikes.begin(), rel_spikes.end(), 0) + 1.0); + + // at certain indices, add a larger time spike + ts_out.resize(ts_in.size()); + std::ranges::copy(ts_in, ts_out.begin()); + for (int i = 0; i < spikes.size(); i++) { + int i_spike = spikes[i]; + double dt_spike = (rel_spikes[i] - 1.0) * 1.0 / fs * 1e9; + for (int j = i_spike; j < N_hint; j++) { + ts_out[j] += dt_spike; + } + } + + // add gaussian noise to times + add_noise(ts_out, 0.0, 0.01 / fs * 1e9); + std::ranges::sort(ts_out); // make sure they remain sorted + + // reduce sampling rate in second half + for (int i = 100; i < 100 + (N_hint - 100) / 2; i++) { + ts_out[i] = ts_out[100 + (i-100)*2]; + } + ts_out.resize(100 + (N_hint - 100) / 2); + + // compute signal at times + pd_signal::interp(sig_out, ts_out, ts_in, sig_in); + + return N; +} + +TEST(HelloTest, Resampler_Test1) { + std::vector ts_orig, ts_spiky; + std::vector a_orig, a_spiky; + std::vector sig_res; + + make_test_signal_1(207, ts_orig, a_orig); // N = 200+sum(rel_spikes)-len(rel_spikes) + double fs = 1e9 / (ts_orig[1]-ts_orig[0]); + //std::cout << "fs=" << fs << std::endl; + make_spiky_times(200, ts_orig, ts_spiky, a_orig, a_spiky); + + Resampler res; + + const int INITIAL_SAMPLES = 100; // Resampler.INITIAL_SAMPLES; + int i; + // push - initial samples are buffered + for (i = 0; i < INITIAL_SAMPLES - 1; i++) { + res.push(ts_spiky[i], a_spiky[i]); + ASSERT_FALSE(res.peek()); + } + res.push(ts_spiky[i], a_spiky[i]); + //ASSERT_NEAR(res.get_fs(), fs, 1e-7); // should fail + ASSERT_NEAR(res.get_fs(), fs, 1e-2); + + // get - initial samples are pushed out + sig_res.resize(ts_orig.size()+1); // despite gaussian time noise, sum(ts) should roughly be same length as we guessed initially + for (i = 0; i < INITIAL_SAMPLES; i++) { + ASSERT_TRUE(res.peek()); + sig_res[i] = res.get(); + } + + // push - additional samples are all pushed out + int j = INITIAL_SAMPLES; + for (i = INITIAL_SAMPLES; i < ts_spiky.size(); i++) { + res.push(ts_spiky[i], a_spiky[i]); + // potentially get multiple samples + while (res.peek()) + sig_res[j++] = res.get(); + } + + std::filesystem::create_directories("test6"); + npy_save("test6/ts_orig_t1.npy", ts_orig); + npy_save("test6/a_orig_t1.npy", a_orig); + + npy_save("test6/ts_spiky_t1.npy", ts_spiky); + npy_save("test6/a_spiky_t1.npy", a_spiky); + + npy_save("test6/sig_res_t1.npy", sig_res); + std::vector fs_t1{res.get_fs()}; + npy_save("test6/fs_t1.npy", fs_t1); + /* + * ts_gen = np.arange(sig_res_t1.shape[0]) / fs * 1e9 + * plt.plot(ts_spiky_t1, a_spiky_t1) + * plt.plot(ts_gen, sig_res_t1) + */ +} diff --git a/pasada-lib/CMakeLists.txt b/pasada-lib/CMakeLists.txt index e0af4f0..fbcb8e8 100644 --- a/pasada-lib/CMakeLists.txt +++ b/pasada-lib/CMakeLists.txt @@ -6,6 +6,7 @@ SET(PASADA_SRC ssf_filter.cpp pd_signal.cpp step_detector.cpp + pd_resamp.cpp ) if(PASADA_BUILD_TESTS) diff --git a/pasada-lib/include/pd_resamp.h b/pasada-lib/include/pd_resamp.h new file mode 100644 index 0000000..06883fd --- /dev/null +++ b/pasada-lib/include/pd_resamp.h @@ -0,0 +1,51 @@ +// +// Created by david on 17.05.2026. +// + +#ifndef PASADASUPERPROJECT_PD_RESAMP_H +#define PASADASUPERPROJECT_PD_RESAMP_H + +#include "iir_filter.h" + +/** Filter that changes sampling rate between input and output. */ +class ResamplingFilter { +public: + ResamplingFilter() {} + virtual ~ResamplingFilter() {} + virtual void push(double ts, double val) = 0; + virtual bool peek() = 0; + virtual double get() = 0; +}; + +/** Normalizes incoming Android sensor sampling rate. */ +class Resampler : public ResamplingFilter { +protected: + const size_t INITIAL_SAMPLES = 100; + std::vector times; + std::vector data; + /** circular buffer size */ + size_t N; + /** write index */ + size_t n; + /** read index */ + size_t m; + bool initialized; + bool read_valid; + /** computed sampling frequency, this will be the output rate */ + double fs; + void compute_fs(); +public: + Resampler(); + /** + * Push a value into the buffer. + * Caller is responsible for polling via peek() and get() afterward. + * @param ts timestamp in nanoseconds + * @param val signal sample + */ + void push(double ts, double val) override; + bool peek() override; + double get() override; + double get_fs() const; +}; + +#endif //PASADASUPERPROJECT_PD_RESAMP_H \ No newline at end of file diff --git a/pasada-lib/include/pd_signal.h b/pasada-lib/include/pd_signal.h index 223d727..6914f2b 100644 --- a/pasada-lib/include/pd_signal.h +++ b/pasada-lib/include/pd_signal.h @@ -40,6 +40,10 @@ namespace pd_signal { /** two-dimensional mean of a collection of signals */ void mean(std::vector &out, std::deque >& m); + /** simple mean of 1-d signal */ + double mean(const std::vector& in); + void diff(std::vector& out, const std::vector& in); + /** * Convolution of two polynomials given in ASCENDING power order. * If p = p_0 + p_1 x + ... + p_{P-1} x^{P-1} and likewise for q, diff --git a/pasada-lib/pd_resamp.cpp b/pasada-lib/pd_resamp.cpp new file mode 100644 index 0000000..419c31f --- /dev/null +++ b/pasada-lib/pd_resamp.cpp @@ -0,0 +1,103 @@ +// +// Created by david on 17.05.2026. +// + +#include "pd_resamp.h" +#include "pd_signal.h" + +Resampler::Resampler(): N(INITIAL_SAMPLES+1), n(0), m(0), initialized(false), read_valid(false), fs(0.0) { + times.resize(N); + data.resize(N); + times.assign(N, 0.0); + data.assign(N, 0.0); +} + +void Resampler::push(double ts, double val) { + // i: previous write position + auto i = static_cast((static_cast(n) - 1 + static_cast(N)) % static_cast(N)); + auto im = static_cast((static_cast(m) - 1 + static_cast(N)) % static_cast(N)); + if (ts < times[i]) throw std::invalid_argument("we expect ts to be time-ascending"); + // j: current write position + auto j = n; + times[n] = ts; + data[n] = val; + n = (n+1) % N; + // note: we do not currently handle overrun (assume caller keeps contract) + if (n == INITIAL_SAMPLES && !initialized) { + compute_fs(); + read_valid = true; + return; // returns INITIAL_SAMPLES all at once + // ??? need to compute 'dt' etc. for last sample! -> fall through + } + // once initialized, we skip ahead as much as possible - avoid buffering too much + double dt = ts - times[im]; + double dtr = dt * 1e-9 * fs; + if (dtr < 0) { throw std::invalid_argument("dt is negative"); } + // case 1: 'ts' is less than next sample + if (dtr < 0.99) { + // drop samples (cannot be bothered with interpolation): + // practice shows that Android initially provides a high sampling rate, which subsequently drops later on, + // so we simply skip the implementation here. + read_valid = false; + return; + } + // case 2: 'ts' is exactly next sample + if (0.99 < dtr && dtr < 1.01) { + m = j; // skip directly to actual sample + read_valid = true; + return; + } + // case 3: 'ts' skips samples + if (dtr >= 1.01) { + auto ts_nm1 = times[i]; + // x = ts[n-1] + np.linspace(1.0, dtr, int(np.round(dtr)), endpoint=True) / fs * 1e9 + // xp = [ts[n-1], ts[n]] + // fp = [a[n-1], a[n]] + // y = np.interp(x, xp, fp) + std::vector y; + std::vector x; + std::vector xp { times[i], ts }; + std::vector fp { data[i], val }; + pd_signal::linspace(x, 1.0, dtr, static_cast(round(dtr)), false); + for (auto& e : x) e = ts_nm1 + e / fs * 1e9; + pd_signal::interp(y, x, xp, fp); + // write to data[_ : n] + int s = static_cast(x.size()); + auto p0 = static_cast((static_cast(n) - s + static_cast(N)) % static_cast(N)); + for (int p = 0; p < s; p++) { + data[(p0 + p) % N] = y[p]; + } + m = p0; // provides round(dtr) samples output, interpolated + read_valid = true; + return; + } +} + +bool Resampler::peek() { + if (!initialized) { return false; } + if (!read_valid) { return false; } + return n != m; +} + +double Resampler::get() { + if (!initialized) { throw std::runtime_error("not initialized"); } + if (n == m) { throw std::runtime_error("empty buffer"); } + double val = data[m]; + m = (m+1) % N; + return val; +} + +double Resampler::get_fs() const { + return fs; +} + +void Resampler::compute_fs() { + // compute 'fs' according to first INITIAL_SAMPLES + // we ignore 'n' as this is only ever called once per Resampler lifetime, and assume 'times' has been filled from 0 on + std::vector delta_times; + pd_signal::diff(delta_times, times); + delta_times.resize(INITIAL_SAMPLES-1); // trim off trailing buffer slot (which is not filled) + double mean_dt = pd_signal::mean(delta_times); + fs = 1e9 / mean_dt; + initialized = true; +} diff --git a/pasada-lib/pd_signal.cpp b/pasada-lib/pd_signal.cpp index f97ada7..e3e4158 100644 --- a/pasada-lib/pd_signal.cpp +++ b/pasada-lib/pd_signal.cpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace pd_signal { @@ -141,6 +142,25 @@ void mean(std::vector &out, std::deque >& m) { mean_tpl(out, m); } +double mean(const std::vector &in) { + if (in.empty()) { + throw std::invalid_argument("mean: input vector is empty"); + } + double sum = std::accumulate(in.begin(), in.end(), 0.0); + return sum / static_cast(in.size()); +} + +void diff(std::vector& out, const std::vector& in) { + if (in.size() < 2) { + out.clear(); + return; + } + out.resize(in.size() - 1); + for (std::size_t i = 1; i < in.size(); ++i) { + out[i - 1] = in[i] - in[i - 1]; + } +} + // Convolution of two polynomials in ascending power order. void polymul(std::vector& out, const std::vector& p, const std::vector& q) {