From 7af5131ccf8d27f5c03ea1bf90f727b8405a3a54 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 15 Nov 2017 16:41:57 +0100 Subject: [PATCH] worked on grid walker --- grid/walk/v3/Helper.h | 50 +++++++--- grid/walk/v3/WalkEvaluator.h | 27 ++++-- grid/walk/v3/Walker.h | 173 +++++++++++++++++++++++------------ synthetic/SyntheticTurns.h | 32 ++++--- 4 files changed, 190 insertions(+), 92 deletions(-) diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index 9d51178..a47b3f6 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -84,43 +84,67 @@ namespace GW3 { }; + /** + * 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 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 an iterator over all nodes reachable from the given start */ - template class ReachableIteratorUnsorted { + template class ReachableIteratorUnsorted { const Grid& grid; const Node& start; Node* curNode = nullptr; std::unordered_set visited; - std::vector toVisit; + ToVisit toVisit; + + Conditions cond; public: - ReachableIteratorUnsorted(const Grid& grid, const Node& start) : grid(grid), start(start) { - toVisit.push_back(start.getIdx()); + ReachableIteratorUnsorted(const Grid& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) { + toVisit.add(start.getIdx()); } bool hasNext() const { return !toVisit.empty(); } - const Node& next(const std::function& skip) { - - const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance) - toVisit.erase(toVisit.begin()); - visited.insert(curIdx); + //const Node& next(const std::function& skip) { + //template const Node& next(const Skip skip) { + const Node& next() { + // 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]; - for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + // mark as "visited" + visited.insert(curIdx); - const int neighborIdx = curNode.getNeighborIdx(i); + // get all neighbors + const int numNeighbors = curNode.getNumNeighbors(); + for (int i = 0; i < numNeighbors; ++i) { + + const uint32_t neighborIdx = curNode.getNeighborIdx(i); const Node& neighbor = grid[neighborIdx]; + const bool visit = cond.visit(neighbor) ; + // not yet reached -> store distance - if (!skip(neighbor)) { + if (visit) { if (visited.find(neighborIdx) == visited.end()) { - toVisit.push_back(neighborIdx); + toVisit.add(neighborIdx); } } diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index 8b73dbe..ccc9029 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -39,7 +39,8 @@ namespace GW3 { const GridPoint gp = Helper::p3ToGp(pEnd); const Node& node = grid->getNodeFor(gp); const double p = node.getWalkImportance(); - return std::pow(p,10); + return p; + //return std::pow(p,10); } @@ -50,11 +51,19 @@ namespace GW3 { /** evaluate the difference between head(start,end) and the requested heading */ template class WalkEvalHeadingStartEnd : public WalkEvaluator { - const double sigma; + const double sigma_rad; + const double kappa; + Distribution::VonMises _dist; + Distribution::LUT dist; public: - WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;} + // kappa = 1/var = 1/sigma^2 + // https://en.wikipedia.org/wiki/Von_Mises_distribution + WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) : + sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) { + ; + } virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { @@ -68,7 +77,8 @@ namespace GW3 { const Heading head(pStart.xy(), pEnd.xy()); const float diff = head.getDiffHalfRAD(params.heading); //const float diff = Heading::getSignedDiff(params.heading, head); - return Distribution::Normal::getProbability(0, sigma, diff); + //return Distribution::Normal::getProbability(0, sigma, diff); + return dist.getProbability(diff); } @@ -79,14 +89,19 @@ namespace GW3 { const double sigma; + const Distribution::Normal dist; + public: - WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;} + WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + const float requestedDistance_m = params.distance_m; const float walkedDistance_m = pStart.getDistance(pEnd); - return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); + const float diff = walkedDistance_m - requestedDistance_m; + return dist.getProbability(diff); + //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); } diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 543059b..92f2d67 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -26,18 +26,22 @@ namespace GW3 { public: /** get a new destination for the given start */ - virtual const WalkResult getDestination(Grid& grid, const WalkParams& params) const = 0; + virtual const WalkResult getDestination(const WalkParams& params) const = 0; }; template class WalkerDirectDestination : public WalkerBase { - //RandomGenerator rnd; - + Grid& grid; std::vector*> evals; public: + /** ctor */ + WalkerDirectDestination(Grid& grid) : grid(grid) { + ; + } + /** make the code a little more readable */ using Helper = GW3::Helper; using Walk = typename GW3::Walk; @@ -50,7 +54,7 @@ namespace GW3 { } /** perform the walk based on the configured setup */ - const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + const WalkResult getDestination(const WalkParams& params) const override { Assert::isNot0(params.distance_m, "walking distance must be > 0"); @@ -142,8 +146,20 @@ namespace GW3 { std::vector*> evals; + Grid& grid; + const float gridSize_m; + + mutable std::minstd_rand rndGen; + mutable std::uniform_real_distribution dFinal; + public: + /** ctor */ + WalkerWeightedRandom(Grid& grid) : + grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f) { + ; + } + /** make the code a little more readable */ using Helper = GW3::Helper; using Walk = typename GW3::Walk; @@ -156,79 +172,71 @@ namespace GW3 { } /** perform the walk based on the configured setup */ - const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + const WalkResult getDestination(const WalkParams& params) const override { Assert::isNot0(params.distance_m, "walking distance must be > 0"); - static std::minstd_rand rndGen; - const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); + if (!startNode) {throw Exception("start node not found!");} -// // include one additional grid-cell (increased distance) -// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05); + const float maxDist = params.distance_m + gridSize_m; -// ReachableSettings set; -// set.limitDistance = true; -// set.limitHeading = true; -// set.dist_m = params.distance_m + secBuffer_m; -// set.heading = params.heading; -// set.maxHeadingDiff_rad = M_PI/2; -// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); - - const float gridSize_m = grid.getGridSize_cm() / 100.0f; - //std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); - - - Point3 best; - double bestP = 0; -// DrawList drawer; + Point3 best; double bestP = 0; + //DrawList drawer; const Point3 start = params.start; - // try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles") - //for (int i = 0; i < 500; ++i) { - - // const Node* dstNode = reachableNodes[dNode(rndGen)]; - - std::uniform_real_distribution dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f); - - ReachableIteratorUnsorted ri(grid, *startNode); - const float maxDist = params.distance_m * 1.25 + gridSize_m; - - auto skip = [&] (const Node& n) { - const float dist_m = n.getDistanceInMeter(gpStart); - return dist_m > maxDist; + struct RICond { + const GridPoint gpStart; + const float maxDist; + RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;} + bool visit (const Node& n) const { + const float dist_m = n.getDistanceInMeter(gpStart); + return dist_m < maxDist; + } }; + RICond riCond(gpStart, maxDist); - //for (const Node* dstNode : reachableNodes) { - while(ri.hasNext()) { + // iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance) + ReachableIteratorUnsorted ri(grid, *startNode, riCond); - const Node* dstNode = &ri.next(skip); -// const float dist_m = dstNode->getDistanceInMeter(gpStart); + int numVisitedNodes = 0; -// if (dist_m > maxDist) { -// break; -// } - for (int i = 0; i < 25; ++i) { + #define MODE 1 + + #if (MODE == 1) + + double bestNodeP = 0; + const Node* bestNode = nullptr; + + while(ri.hasNext()) { + const Node* dstNode = &ri.next(); + const Point3 nodeCenter = Helper::gpToP3(*dstNode); + double p = 1.0; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, nodeCenter, params); + p *= p1; + } + if (p > bestNodeP) { + bestNodeP = p; + bestNode = dstNode; + } + } + + for (int i = 0; i < 10; ++i) { + + const Point3 nodeCenter = Helper::gpToP3(*bestNode); // random position within destination-node - const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + const float ox = dFinal(rndGen); + const float oy = dFinal(rndGen); - // destination = node-center + offset (within the node's bbox) - const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; - - // sanity check - if (start == end) {continue;} - if (!grid.hasNodeFor(Helper::p3ToGp(end))) { - std::cout << "random destination is not part of the grid" << std::endl; - continue; - } - - //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) + const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); double p = 1; for (const WalkEvaluator* eval : evals) { @@ -237,20 +245,63 @@ namespace GW3 { } if (p > bestP) {bestP = p; best = end;} - //drawer.add(end, p); } - } + #elif (MODE == 2) - //const Point3 end = drawer.get(); + // all reachable nodes + while(ri.hasNext()) { + + const Node* dstNode = &ri.next(); + ++numVisitedNodes; + + const Point3 nodeCenter = Helper::gpToP3(*dstNode); + + // try multiple locations within each reachable node + for (int i = 0; i < 1; ++i) { + + // random position within destination-node + const float ox = dFinal(rndGen); + const float oy = dFinal(rndGen); + + // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) + const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); + + // sanity check + if (start == end) {continue;} + // if (!grid.hasNodeFor(Helper::p3ToGp(end))) { + // std::cout << "random destination is not part of the grid" << std::endl; + // continue; + // } + //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = end;} + // drawer.add(end, p); + + } + + } + + #endif + + //std::cout << numVisitedNodes << std::endl; + + //double drawProb = 0; const Point3 end = drawer.get(drawProb); const Point3 end = best; WalkResult res; if (start == end) { res.probability = 0; } else { res.heading = Heading(start.xy(), end.xy()); - res.probability = bestP; + //res.probability = drawProb; // when using DrawList + res.probability = bestP; // when using bestP } res.position = end; return res; diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index bec5bf7..f94db0c 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -7,7 +7,7 @@ #include "../sensors/imu/GyroscopeData.h" #include "../geo/Heading.h" -#include "../math/distribution/Normal.h" +#include "../math/Distributions.h" /** * simulates acceleromter and gyroscope data @@ -41,6 +41,8 @@ private: Distribution::Normal dChange = Distribution::Normal(1.0, 0.25); Distribution::Normal dHeadErr = Distribution::Normal(0.15, 0.10); // heading error, slightly biased + Distribution::Uniform dRadDiff = Distribution::Uniform(40,100); + std::vector listeners; @@ -68,7 +70,7 @@ protected: Heading desiredHead = Heading(0); Heading curHead = Heading(0); Point3 lastPos = Point3(NAN, NAN, NAN); - float change; + double change = 0; inline float clamp(const float val, const float min, const float max) { if (val < min) {return min;} @@ -86,27 +88,33 @@ protected: if (lastPos.x != lastPos.x) { lastPos = curPos; } else { - desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();; + desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw(); lastPos = curPos; } // difference between current-heading and desired-heading - const float diffRad = Heading::getSignedDiff(curHead, desiredHead); + const double maxChange = dMaxChange.draw(); + const double diffRad = Heading::getSignedDiff(curHead, desiredHead); + //change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange); + change = clamp(diffRad / 25, -maxChange, +maxChange); - // slowly change the current heading to match the desired one - const float maxChange = dMaxChange.draw(); - const float toChange = clamp(diffRad, -maxChange, +maxChange); - //if (change < toChange) {change += toChange*0.01;} - if (change > toChange) {change *= 0.93;} - if (change < toChange) {change += dChange.draw()/10000;} - //if (change > toChange) {change -= dChange.draw();} + +// // slowly change the current heading to match the desired one +// //const double maxChange = dMaxChange.draw(); +// //const double toChange = clamp(diffRad, -maxChange, +maxChange); +// const double toChange = diffRad; +// //if (change < toChange) {change += toChange*0.01;} +// if (change > toChange) {change *= 0.93;} +// //if (change < toChange) {change += dChange.draw()/10000;} // does not work for small changes?! +// if (change < toChange) {change += (toChange-change) * 0.07;} +// //if (change > toChange) {change -= dChange.draw();} curHead += change; // convert to gyro's radians-per-second - const float radPerSec = change * 1000 / deltaTs.ms();; + const double radPerSec = change * 1000 / deltaTs.ms();; const float accX = 0.00 + dAccX.draw(); const float accY = 0.00 + dAccY.draw();