worked on grid-walker

started adding code for synthetic sensor simulation based on given paths

synthetic/SyntheticPath.h

@@ -0,0 +1,109 @@
+#ifndef INDOOR_SYNTHETICPATH_H
+#define INDOOR_SYNTHETICPATH_H
+
+#include "../math/Interpolator.h"
+#include "../floorplan/v2/Floorplan.h"
+#include "../floorplan/v2/FloorplanHelper.h"
+
+/** allows interpolation along a synthetic path */
+class SyntheticPath : private Interpolator<float, Point3> {
+
+	using Base = Interpolator<float, Point3>;
+	using Entry = Base::InterpolatorEntry;
+
+public:
+
+	/** create path using the given ground-truth points from the map */
+	void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {
+
+		// get all ground-truth points from the map
+		auto gtps = FloorplanHelper::getGroundTruthPoints(map);
+		float dist = 0;
+
+		// create distance-based entries within the interpolator
+		for (int i = 0; i < ids.size(); ++i) {
+			const int id = ids[i];
+			Point3 gtp = gtps[id];
+			if (i >= 1) {
+				Point3 gtpPrev = gtps[id-1];
+				dist += gtpPrev.getDistance(gtp);
+			}
+			Base::add(dist, gtp);
+		}
+
+	}
+
+	/** get all individual entries from the underlying data-structure */
+	const std::vector<Entry>& getEntries() const {
+		return Base::getEntries();
+	}
+
+	/** smooth harsh angles */
+	void smooth(float delta = 1, int numRuns = 1) {
+
+		float t = delta*2;
+
+		for (int j = 0; j < numRuns; ++j) {
+
+			t/=2;
+
+			for (int i = 1; i < (int)entries.size()-1; ++i) {
+
+				// the entry to-be-replaced by two others
+				const Entry& e = entries[i];
+				const float key = e.key;
+
+				const Entry e1(key-t, Base::get(key-t));
+				const Entry e2(key+t, Base::get(key+t));
+
+				entries.erase(entries.begin()+i); --i;
+				++i; entries.insert(entries.begin()+i, e1);
+				++i; entries.insert(entries.begin()+i, e2);
+
+			}
+
+		}
+
+	}
+
+//	/** smooth harsh angles */
+//	void smooth(float delta = 1, int numRuns = 1) {
+
+//		float t = delta/numRuns;
+
+//		for (int i = 1; i < (int)entries.size()-1; ++i) {
+
+//			// the entry to-be-replaced by several others
+//			const Entry& e = entries[i];
+//			const float key = e.key;
+
+//			std::vector<Entry> newEntries;
+//			for (int x = -numRuns; x <= +numRuns; x+= 2) {
+//				const float percent = (float)x / (float)numRuns;
+//				const float keyO = key + percent*t;
+//				const Point3 pos1 = Base::get(keyO-t*2);
+//				const Point3 pos2 = Base::get(keyO+t*2);
+//				const Point3 posO = (pos1+pos2) / 2;
+//				Entry e(keyO, posO);
+//				newEntries.push_back(e);
+//			}
+
+//			entries.erase(entries.begin()+i); --i;
+
+//			for (const Entry& e : newEntries) {
+//				++i;
+//				entries.insert(entries.begin()+i, e);
+//			}
+
+//		}
+
+//	}
+
+	/** get the position along the path after the given walking distance */
+	Point3 getPosAfterDistance(const float distance) const {
+		return Base::get(distance);
+	}
+
+};
+
+#endif // INDOOR_SYNTEHTICPATH_H

synthetic/SyntheticSteps.h

@@ -0,0 +1,67 @@
+#ifndef SYNTHETICSTEPS_H
+#define SYNTHETICSTEPS_H
+
+#include "../sensors/imu/AccelerometerData.h"
+#include "SyntheticWalker.h"
+#include "../math/distribution/Normal.h"
+
+/** fakes accelerometer-data based on synthetic walking data */
+class SyntheticSteps : SyntheticWalker::Listener {
+
+public:
+
+	class Listener {
+	public:
+		virtual void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) = 0;
+	};
+
+private:
+
+	/** the walker to listen to */
+	SyntheticWalker* walker;
+
+	/** the pedestrian's step-size (in meter) */
+	float stepSize_m = 0.7;
+
+	float lastStepAtDistance = 0;
+
+	Distribution::Normal<float> dX = Distribution::Normal<float>(0, 1);
+	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 1);
+	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 1);
+
+	std::vector<Listener*> listeners;
+
+public:
+
+	/** ctor with the walker to follow */
+	SyntheticSteps(SyntheticWalker* walker) {
+		walker->addListener(this);
+	}
+
+	void addListener(Listener* l) {
+		this->listeners.push_back(l);
+	}
+
+protected:
+
+
+	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
+
+		const float nextStepAt = (lastStepAtDistance + stepSize_m);
+
+		const float x = dX.draw();
+		const float y = dY.draw();
+		const float z = dZ.draw();
+		AccelerometerData acc(x, y, z);
+
+		if (walkedDistance > nextStepAt) {
+			lastStepAtDistance = walkedDistance;
+		}
+
+		for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);}
+
+	}
+
+};
+
+#endif // SYNTHETICSTEPS_H

synthetic/SyntheticTurns.h

@@ -0,0 +1,4 @@
+#ifndef SYNTHETICTURNS_H
+#define SYNTHETICTURNS_H
+
+#endif // SYNTHETICTURNS_H

synthetic/SyntheticWalker.h

@@ -0,0 +1,70 @@
+#ifndef SYNTHETICWALKER_H
+#define SYNTHETICWALKER_H
+
+#include "SyntheticPath.h"
+
+/** walk along a path using a known walking speed */
+class SyntheticWalker {
+
+public:
+
+	class Listener {
+	public:
+		virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0;
+	};
+
+private:
+
+	/** the path to walk along */
+	SyntheticPath path;
+
+	/** walking-speed in meter per sec */
+	float walkSpeed_meterPerSec = 1.2;
+
+	/** adjusted while walking */
+	float walkedDistance = 0;
+
+	/** adjusted while walking */
+	Timestamp walkedTime;
+
+	/** the listener to inform */
+	std::vector<Listener*> listeners;
+
+	const char* name = "SynWalker";
+
+public:
+
+	/** ctor */
+	SyntheticWalker(SyntheticPath path) : path(path) {
+		;
+	}
+
+	/** attach a new listener */
+	void addListener(Listener* l) {
+		this->listeners.push_back(l);
+	}
+
+	/** increment the walk */
+	void tick(const Timestamp timePassed) {
+
+		// update time
+		this->walkedTime += timePassed;
+
+		// update the walked distance using the walking speed
+		this->walkedDistance += walkSpeed_meterPerSec * timePassed.sec();
+
+		// get the current position along the path
+		const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance);
+
+		Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() );
+
+		// inform listener
+		for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);}
+
+	}
+
+
+
+};
+
+#endif // SYNTHETICWALKER_H