worked on grid-walker and synthetic steps/turns

grid/walk/v3/Helper.h

@@ -193,10 +193,22 @@ namespace GW3 {
 			return bbox.contains(pt);
 		}
 
+//		/** does one of the given grid-nodes contains the provided point-in-question? */
+//		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+//			for (const Node* n : nodes) {
+//				if (contains(grid, n, pt)) {
+//					return n;
+//				}
+//			}
+//			return nullptr;
+//		}
+
 		/** does one of the given grid-nodes contains the provided point-in-question? */
-		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+		static const Node* contains(const Grid<Node>& grid, const std::vector<const Node*>& nodes, Point2 pt) {
 			for (const Node* n : nodes) {
-				if (contains(grid, n, pt)) {return n;}
+				if (contains(grid, n, pt)) {
+					return n;
+				}
 			}
 			return nullptr;
 		}

grid/walk/v3/Reachable.h

@@ -7,15 +7,15 @@
 
 namespace GW3 {
 
-#define likely(x)       __builtin_expect((x),1)
-#define unlikely(x)     __builtin_expect((x),0)
+	#define likely(x)       __builtin_expect((x),1)
+	#define unlikely(x)     __builtin_expect((x),0)
 
 
 
-/**
+	/**
 	 * get all grid nodes that are reachable within x-edges (depth)
 	 */
-template <typename Node> class ReachableByDepthUnsorted {
+	template <typename Node> class ReachableByDepthUnsorted {
 
 		struct VisitEntry {
 			const Node* gn;
@@ -43,7 +43,7 @@ template <typename Node> class ReachableByDepthUnsorted {
 
 		const Grid<Node>& grid;
 
-public:
+	public:
 
 		ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
 			;
@@ -101,14 +101,14 @@ public:
 
 		}
 
-};
+	};
 
 
-/**
+	/**
 	 * get all grid nodes that are reachable within x-edges (depth)
 	 * additionally returns the needed walking distance in meter
 	 */
-template <typename Node> class ReachableByDepthWithDistanceSorted {
+	template <typename Node> class ReachableByDepthWithDistanceSorted {
 
 		struct VisitEntry {
 			const Node* gn;
@@ -145,7 +145,7 @@ template <typename Node> class ReachableByDepthWithDistanceSorted {
 
 		const Grid<Node>& grid;
 
-public:
+	public:
 
 		/** result */
 		struct Entry {
@@ -197,19 +197,19 @@ public:
 
 					const Node* gnCur = e.gn;
 
-//				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
-//					const int nIdx = gnCur->getNeighborIdx(i);
-//					const Node& gnNext = grid[nIdx];
-//					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
-//						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
-//						const float dist_m = e.dist_m + nodeNodeDist_m;
-//						toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
-//						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
-//						checked.insert(&gnNext);
-//					}
-//				}
+	//				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+	//					const int nIdx = gnCur->getNeighborIdx(i);
+	//					const Node& gnNext = grid[nIdx];
+	//					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+	//						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
+	//						const float dist_m = e.dist_m + nodeNodeDist_m;
+	//						toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+	//						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
+	//						checked.insert(&gnNext);
+	//					}
+	//				}
 
-//				const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
+	//				const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
 
 					std::vector<VisitEntry> sub;
 
@@ -250,7 +250,90 @@ public:
 
 		}
 
-};
+	};
+
+
+	/**
+	 * data-structure to track to-be-visited nodes
+	 * push_back, pop_front
+	 * as pop_front is costly, we omit the pop and use a head-index instead
+	 * memory-consumption vs speed
+	 */
+	struct _ToVisit {
+		size_t nextIdx = 0;
+		std::vector<uint32_t> vec;
+		_ToVisit() {vec.reserve(256);}
+		void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
+		uint32_t next() {return vec[nextIdx++];}
+		bool empty() const {return nextIdx >= vec.size();}
+	};
+
+
+
+	/** get a list of all nodes that are reachable after checking several conditions */
+	template <typename Node, typename Conditions> class ReachableByConditionUnsorted {
+
+
+	public:
+
+
+		static std::vector<const Node*> get(const Grid<Node>& grid, const Node& start, const Conditions cond) {
+
+			//Node* curNode = nullptr;
+			std::unordered_set<uint32_t> scheduled;
+			_ToVisit toVisit;
+			toVisit.add(start.getIdx());
+
+			std::vector<const Node*> res;
+
+			while(!toVisit.empty()) {
+
+				// get the next to-be-visited node
+				const uint32_t curIdx = toVisit.next();			//visit from inside out (needed for correct distance)
+				const Node& curNode = grid[curIdx];
+
+				// process current node
+				res.push_back(&curNode);
+				scheduled.insert(curIdx);
+
+				// get all neighbors
+				const int numNeighbors = curNode.getNumNeighbors();
+				for (int i = 0; i < numNeighbors; ++i) {
+
+					const uint32_t neighborIdx = curNode.getNeighborIdx(i);
+
+					// already visited?
+					if (scheduled.find(neighborIdx) != scheduled.end()) {continue;}
+					scheduled.insert(neighborIdx);
+
+					// matches the used condition?
+					const Node& neighbor = grid[neighborIdx];
+					if (!cond.visit(neighbor)) {continue;}
+
+					// OK!
+					toVisit.add(neighborIdx);
+
+				}
+
+			}
+
+			// done
+			return res;
+
+		}
+
+
+
+		//const Node& next(const std::function<bool(const Node&)>& skip) {
+		//template <typename Skip> const Node& next(const Skip skip) {
+		const Node& next() {
+
+
+
+		}
+
+	};
+
 
 }
 

grid/walk/v3/Walker.h

@@ -65,35 +65,63 @@ namespace GW3 {
 			const GridPoint gpStart = Helper::p3ToGp(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
 
+			// calculate a walk's probability
+			auto getP = [&] (const Point3 dst) {
+				double p = 1;
+				for (const WalkEvaluator<Node>* eval : evals) {
+					const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params);
+					p *= p1;
+				}
+				return p;
+			};
+
 			// include one additional grid-cell (increased distance)
-			const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
-			ReachableSettings set;
-			set.limitDistance = true;
-			set.dist_m = params.distance_m + secBuffer_m;
-			set.limitHeading = false;
-			set.heading = params.heading;
-			set.maxHeadingDiff_rad = M_PI/2;
-			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+			//const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1);
+			const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15);
+
+//			ReachableSettings set;
+//			set.limitDistance = true;
+//			set.dist_m = params.distance_m + secBuffer_m;
+//			set.limitHeading = false;
+//			set.heading = params.heading;
+//			set.maxHeadingDiff_rad = M_PI/2;
+
+//			// get all nodes that satisfy above constraints
+//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+
+			struct Cond {
+				const float maxDist_m;
+				const Node* startNode;
+				Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
+				bool visit(const Node& n) const {
+					return (startNode->getDistanceInMeter(n)) < maxDist_m;
+				}
+			};
+			Cond cond(params.distance_m+secBuffer_m, startNode);
+			std::vector<const Node*>  reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
 
 			WalkResult res;
 			res.heading = params.heading;
 			res.position = params.start;
 
-
+			// get the to-be-reached destination's position (using start+distance+heading)
 			const Point2 dir = res.heading.asVector();
 			const Point2 dst = params.start.xy() + (dir * params.distance_m);
 
-			// is dst reachable?
+			// is above destination reachable?
 			const Node* n = Helper::contains(grid, reachableNodes, dst);
+			//const Node* n = ri.contains(dst);
 			if (n) {
 
 				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
 				const GridPoint gp = Helper::p3ToGp(p3);
 
+
+
 				if (grid.hasNodeFor(gp)) {
 					res.position = p3;					// update position
 					//res.heading;						// keep as-is
-					//res.probability;					// keep as-is
+					res.probability *= getP(p3);		// keep as-is
 					return res;							// done
 
 				} else {
@@ -129,7 +157,7 @@ namespace GW3 {
 			}
 
 			res.heading = Heading(start.xy(), end.xy());
-			res.probability = p;
+			res.probability *= getP(end);
 			res.position = end;
 			return res;
 

synthetic/SyntheticSteps.h

@@ -37,14 +37,22 @@ private:
 	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
 	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
 
+
 	int stepPatternPos = -1;
 
 	std::vector<Listener*> listeners;
 
+	//float stepSize_m;
+	//float stepSizeSigma_m;
+	float noiseLevel;
+	Distribution::Normal<float> dNextStep;
+
 public:
 
 	/** ctor with the walker to follow */
-	SyntheticSteps(SyntheticWalker* walker) {
+	SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) :
+		//stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m),
+		noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m) {
 
 		walker->addListener(this);
 		dX.setSeed(1);
@@ -82,15 +90,15 @@ protected:
 	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
 
 		(void) curPos;
-		const float nextStepAt = (lastStepAtDistance + stepSize_m);
+		const float nextStepAt = lastStepAtDistance + dNextStep.draw();
 
 		// 1st, start with random noise on the accelerometer
 		const float x = dX.draw();
 		const float y = dY.draw();
 		const float z = dZ.draw();
-		const AccelerometerData base(0, 4, 9.7);
-		const AccelerometerData noise(x, y, z);
-		AccelerometerData acc = base + noise;
+		const AccelerometerData aBase(0, 4, 9.7);
+		const AccelerometerData aNoise(x, y, z);
+		AccelerometerData acc = aBase + aNoise * noiseLevel;
 
 		// is it time to inject a "step" into the accelerometer data?
 		if (walkedDistance > nextStepAt) {
@@ -105,7 +113,7 @@ protected:
 				refStepPattern = Timestamp::fromMS(0);
 			} else {
 				const AccelerometerData step = stepPattern.get(curPatPos);
-				acc = base + noise*2.5f + step;
+				acc = aBase + (aNoise * noiseLevel) + step;
 			}
 		}
 

synthetic/SyntheticTurns.h

@@ -39,17 +39,23 @@ private:
 
 	Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
 	Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
-	Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10);	// heading error, slightly biased
 
 	Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
 
+	//float headingDrift_rad;
+	//float headingSigma_rad;
+	float noiseLevel;
+	Distribution::Normal<float> dHeadErr;
 
 	std::vector<Listener*> listeners;
 
 public:
 
 	/** ctor with the walker to follow */
-	SyntheticTurns(SyntheticWalker* walker) {
+	SyntheticTurns(SyntheticWalker* walker, const float headingDrift_rad = 0, const float headingSigma_rad = 0, const float noiseLevel = 0) :
+		//headingDrift_rad(headingDrift_rad), headingSigma_rad(headingSigma_rad),
+		noiseLevel(noiseLevel + 0.00001f), dHeadErr(headingDrift_rad, headingSigma_rad) {
+
 		walker->addListener(this);
 		dAccX.setSeed(1);
 		dAccY.setSeed(3);
@@ -57,6 +63,7 @@ public:
 		dGyroX.setSeed(7);
 		dGyroY.setSeed(9);
 		dGyroZ.setSeed(11);
+
 	}
 
 	/** attach a listener to this provider */
@@ -116,14 +123,14 @@ protected:
 		// convert to gyro's radians-per-second
 		const double radPerSec = change * 1000 / deltaTs.ms();;
 
-		const float accX = 0.00 + dAccX.draw();
-		const float accY = 0.00 + dAccY.draw();
-		const float accZ = 9.81 + dAccZ.draw();
+		const float accX = 0.00 + dAccX.draw() * (noiseLevel);
+		const float accY = 0.00 + dAccY.draw() * (noiseLevel);
+		const float accZ = 9.81 + dAccZ.draw() * (noiseLevel);
 		AccelerometerData acc(accX, accY, accZ);
 
-		const float gyroX = dGyroX.draw();
-		const float gyroY = dGyroY.draw();
-		const float gyroZ = dGyroZ.draw() + radPerSec;
+		const float gyroX = dGyroX.draw() * (noiseLevel);
+		const float gyroY = dGyroY.draw() * (noiseLevel);
+		const float gyroZ = dGyroZ.draw() * (noiseLevel) + radPerSec;
 		GyroscopeData gyro(gyroX, gyroY, gyroZ);
 
 		for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}