adjusted code and test-cases for fixed-freq-interpolater

math/FixedFrequencyInterpolator.h

@@ -4,21 +4,36 @@
 #include "Interpolator.h"
 #include "../data/Timestamp.h"
 #include <functional>
+#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 <typename Entry> 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<void(Timestamp, const Entry&)> 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;
 
 	}
 

tests/math/TestFixedFrequencyInterpolator.cpp

@@ -4,8 +4,10 @@
 #include "../Tests.h"
 #include "../../math/FixedFrequencyInterpolator.h"
 #include <fstream>
+#include <random>
 
-TEST(FixedFrequencyInterpolator, test) {
+/*
+TEST(FixedFrequencyInterpolator, testOLD) {
 
 	FixedFrequencyInterpolator<float> ffi(Timestamp::fromMS(10));
 
@@ -55,7 +57,74 @@ TEST(FixedFrequencyInterpolator, test) {
 	ASSERT_NEAR(data[18], x*240, d);
 
 }
+*/
 
+TEST(FixedFrequencyInterpolator, testNEW) {
+
+	FixedFrequencyInterpolator<float> ffi(Timestamp::fromMS(9));
+
+	// randomly draw points along a line using random intervals
+	Timestamp pos;
+	std::minstd_rand gen;
+	std::uniform_int_distribution<int> 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<float> 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<float> 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