worked on grid-walker

started adding code for synthetic sensor simulation based on given paths

grid/walk/v3/GridWalker3.h

@@ -26,12 +26,19 @@ private:
 
 public:
 
+	/** paremters for the walk */
 	struct WalkParams {
 		Point3 start;
 		float distance_m;
 		Heading heading = Heading(0);
 	};
 
+	/** result of the random walk */
+	struct WalkResult {
+		Point3 position;
+		Heading heading = Heading(0);
+	};
+
 
 	using Helper = GridWalk3Helper<Node>;
 	using Walk = typename GridWalk3Helper<Node>::Walk3;
@@ -48,7 +55,7 @@ public:
 	}
 
 	/** perform the walk based on the configured setup */
-	const Point3 getDestination(Grid<Node>& grid, const WalkParams& params) {
+	const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) {
 		//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
 		return _drawThenCheck(grid, params);
 
@@ -62,6 +69,7 @@ public:
 //		throw "error";
 //	}
 
+	/** does the given grid-node contain the provided point-in-question? */
 	const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
 		const float gridSize_m = grid.getGridSize_cm() / 100.0f;
 		const float d = gridSize_m / 2.0f;
@@ -71,7 +79,7 @@ public:
 		return bbox.contains(pt);
 	}
 
-	const Point3 _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
+	const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
 
 		const GridPoint gpStart = p3ToGp(params.start);
 		const Node* startNode = grid.getNodePtrFor(gpStart);
@@ -84,13 +92,15 @@ public:
 		const float range_m = params.distance_m + secBuffer_m;
 		const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
 
+		WalkResult res;
+		res.heading = params.heading;
+		res.position = params.start;
 		float realDist_m = params.distance_m;
-		Heading realHead = params.heading;// + dHead.draw();
 
 		int cnt = 0;
 		while(true) {
 
-			const Point2 dir = realHead.asVector();
+			const Point2 dir = res.heading.asVector();
 			const Point2 dst = params.start.xy() + (dir * realDist_m);
 
 			// is dst reachable?
@@ -101,7 +111,9 @@ public:
 					const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
 					const GridPoint gp = p3ToGp(p3);
 					if (grid.hasNodeFor(gp)) {
-						return p3;
+						res.position = p3;	// new position
+						res.heading;		// keep as-is
+						return res;
 					} else {
 						std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
 					}
@@ -111,11 +123,16 @@ public:
 			// before trying again, modify distance and angle
 			if (1 == 0) {
 				realDist_m *= dDist.draw();
-				realHead += dHead.draw();
+				res.heading += dHead.draw();
 			}
 
 			// reached max retries?
-			if (++cnt > 10) {return params.start;}	// did not work out....
+			if (++cnt > 10) {
+				WalkResult res;
+				res.position = params.start;
+				res.heading = params.heading;
+				return res;
+			}	// did not work out....
 
 		}
 

sensors/imu/StepDetection.h

@@ -38,11 +38,31 @@ private:
     const float upperThreshold = +0.4*0.6f;					// + is usually smaller than down (look at graphs)
     const float lowerThreshold = -1.5*0.6f;					// the 0.8 is for testing!
 
+
+	#ifdef WITH_DEBUG_PLOT
+		K::Gnuplot gp;
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementLines lineDet;
+		K::GnuplotPlotElementLines lineX;
+		K::GnuplotPlotElementLines lineY;
+		K::GnuplotPlotElementLines lineZ;
+		Timestamp plotRef;
+		int plotCnt = 0;
+	#endif
+
+
 public:
 
 	/** ctor */
 	StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
-		;
+
+		#ifdef WITH_DEBUG_PLOT
+			plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000");
+			plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00");
+			plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff");
+			plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
+		#endif
+
 	}
 
 	/** does the given data indicate a step? */
@@ -73,22 +93,25 @@ public:
 			step = true;
 		}
 
-//		static K::Gnuplot gp;
-//		static K::GnuplotPlot plot;
-//		static K::GnuplotPlotElementLines lines1; plot.add(&lines1);
-//		static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff");
-//		static Timestamp ref = ts;
 
-//		static int i = 0;
+		#ifdef WITH_DEBUG_PLOT
 
-//		//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
-//		lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) );
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
 
-//		if (++i % 100 == 0) {
-//			gp.draw(plot);
-//			gp.flush();
-//			usleep(1000*25);
-//		}
+			//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
+			lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
+			lineY.add( K::GnuplotPoint2(tsPlot.ms(), acc.y) );
+			lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
+			lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );
+
+			if (++plotCnt % 25 == 0) {
+				gp.draw(plot);
+				gp.flush();
+				//usleep(1000*25);
+			}
+
+		#endif
 
 		return step;
 

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