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