From 1af670a0a9c9c64fc63b2438aed71a0b9033cfdd Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 9 May 2018 10:02:53 +0200 Subject: [PATCH] adjusted code and test-cases for fixed-freq-interpolater --- math/FixedFrequencyInterpolator.h | 96 +++++++++++++++---- tests/math/TestFixedFrequencyInterpolator.cpp | 71 +++++++++++++- 2 files changed, 147 insertions(+), 20 deletions(-) diff --git a/math/FixedFrequencyInterpolator.h b/math/FixedFrequencyInterpolator.h index bb98db3..3e386e8 100644 --- a/math/FixedFrequencyInterpolator.h +++ b/math/FixedFrequencyInterpolator.h @@ -4,21 +4,36 @@ #include "Interpolator.h" #include "../data/Timestamp.h" #include +#include "../Assertions.h" /** * performs interpolation on provided sensor data * for sensors that do not send their data at a fixed frequency - * or to adjust the frequency of the data provided by a sensor + * or to adjust the frequency of the data provided by a sensor. + * supports both: up and downscaling */ template class FixedFrequencyInterpolator { private: + bool first = true; + /** how often to provide output data */ Timestamp outputInterval; - Timestamp lastTS; - Entry lastEntry; + /** track the timestamps when to output the next value */ + Timestamp nextOutput; + + /** combine value-at-timestamp */ + struct Timed { + Timestamp ts; + Entry entry; + Timed(const Timestamp ts, const Entry entry) : ts(ts), entry(entry) {;} + Timed() : ts(), entry() {;} + }; + + Timed last; + public: @@ -32,31 +47,74 @@ public: void add(const Timestamp ts, const Entry& entry, std::function callback) { // first value? - if (lastTS.isZero()) { - lastTS = ts; - lastEntry = entry; + if (first) { + first = false; + last = Timed(ts, entry); + nextOutput = last.ts;// + outputInterval; return; } - // available timeslice between last and current entry - const Timestamp diff = ts - lastTS; + // new value + Timed cur(ts, entry); - // the region to output - const uint64_t start = std::ceil(lastTS.ms() / (float)outputInterval.ms() + 0.00001f) * outputInterval.ms(); - const uint64_t end = std::floor(ts.ms() / (float)outputInterval.ms()) * outputInterval.ms(); + // no time-change? -> ignore + if (last.ts == cur.ts) {return;} - // perform output - for (uint64_t t = start; t <= end; t += outputInterval.ms()) { + // output needed? + //if (nextOutput > last.ts) { - const float percent = (t - lastTS.ms()) / (float) (diff.ms()); - const Entry res = lastEntry + (entry - lastEntry) * percent; + // available timeslice ybetween last and current entry + const Timestamp diff = cur.ts - last.ts; - callback(Timestamp::fromMS(t), res); + // create outputs + while(nextOutput < cur.ts) { - } + // interpolation rate + const float percent = (nextOutput.ms() - last.ts.ms()) / (float) (diff.ms()); - lastEntry = entry; - lastTS = ts; + // sanity checks + Assert::isNotNaN(percent, "detected NaN for interpolation"); + Assert::isTrue(percent <= 1, "detected an invalid interpolation value"); + + const Entry res = last.entry + (cur.entry - last.entry) * percent; + callback(nextOutput, res); + + // increment + nextOutput += outputInterval; + + } + + //} + + // next step + last = cur; + + + +// // available timeslice between last and current entry +// const Timestamp diff = ts - lastTS; + +// // the region to output +// const uint64_t start = std::ceil(lastTS.ms() / (float)outputInterval.ms() + 0.00001f) * outputInterval.ms(); +// const uint64_t end = std::floor(ts.ms() / (float)outputInterval.ms()) * outputInterval.ms(); + +// // perform output +// for (uint64_t t = start; t <= end; t += outputInterval.ms()) { + +// const float percent = (t - lastTS.ms()) / (float) (diff.ms()); + +// // sanity checks +// Assert::isNotNaN(percent, "detected NaN for interpolation"); +// Assert::isTrue(percent <= 1, "detected an invalid interpolation value"); + +// const Entry res = lastEntry + (entry - lastEntry) * percent; + +// callback(Timestamp::fromMS(t), res); + +// } + +// lastEntry = entry; +// lastTS = ts; } diff --git a/tests/math/TestFixedFrequencyInterpolator.cpp b/tests/math/TestFixedFrequencyInterpolator.cpp index ec7fa8e..e1fdcdb 100644 --- a/tests/math/TestFixedFrequencyInterpolator.cpp +++ b/tests/math/TestFixedFrequencyInterpolator.cpp @@ -4,8 +4,10 @@ #include "../Tests.h" #include "../../math/FixedFrequencyInterpolator.h" #include +#include -TEST(FixedFrequencyInterpolator, test) { +/* +TEST(FixedFrequencyInterpolator, testOLD) { FixedFrequencyInterpolator ffi(Timestamp::fromMS(10)); @@ -55,7 +57,74 @@ TEST(FixedFrequencyInterpolator, test) { ASSERT_NEAR(data[18], x*240, d); } +*/ +TEST(FixedFrequencyInterpolator, testNEW) { + + FixedFrequencyInterpolator ffi(Timestamp::fromMS(9)); + + // randomly draw points along a line using random intervals + Timestamp pos; + std::minstd_rand gen; + std::uniform_int_distribution dist(2, 65); + auto func = [] (const Timestamp ts) { + return 0.5 * ts.ms() + 12; + }; + + // compare the randomly drawn points against interpolated fixed-frequency interpolated versions + auto cb = [&] (const Timestamp ts, const float val) { + ASSERT_NEAR(val, func(ts), 0.001); // interpolated vs original position on line + }; + + // run + for (int i = 0; i < 200; ++i) { + const float y = func(pos); + ffi.add(pos, y, cb); + pos += Timestamp::fromMS(dist(gen)); + } + + + +} + +TEST(FixedFrequencyInterpolator, testUpsample) { + + FixedFrequencyInterpolator ffi(Timestamp::fromMS(10)); + int cnt = 0; + + auto cb = [&] (const Timestamp ts, const float f) { + if (cnt == 0) {ASSERT_EQ(ts.ms(), 0); ASSERT_NEAR( 0, f, 0.001);} + if (cnt == 1) {ASSERT_EQ(ts.ms(), 10); ASSERT_NEAR( 10, f, 0.001);} + if (cnt == 10) {ASSERT_EQ(ts.ms(),100); ASSERT_NEAR(100, f, 0.001);} + ++cnt; + }; + + // add two entries. one at t=0 one at t=100. must be up-sampled into 11 entries (t=0, t=10, t=100) + ffi.add(Timestamp::fromMS(0), 0, cb); + ffi.add(Timestamp::fromMS(101), 101, cb); + ASSERT_EQ(11, cnt); + +} + +TEST(FixedFrequencyInterpolator, testDownsample) { + + FixedFrequencyInterpolator ffi(Timestamp::fromMS(50)); + int cnt = 0; + + auto cb = [&] (const Timestamp ts, const float f) { + if (cnt == 0) {ASSERT_EQ(ts.ms(), 0); ASSERT_NEAR( 0, f, 0.001);} + if (cnt == 1) {ASSERT_EQ(ts.ms(), 50); ASSERT_NEAR( 50, f, 0.001);} + if (cnt == 2) {ASSERT_EQ(ts.ms(), 100); ASSERT_NEAR(100, f, 0.001);} + ++cnt; + }; + + // add 100+ entries in 1 ms steps. must be downsampled to 3 entries (t=0, t=50, t=100) + for (int i = 0; i <= 101; ++i) { + ffi.add(Timestamp::fromMS(i), i, cb); + } + ASSERT_EQ(3, cnt); + +} #endif