From d03372ad3da425159d024f79d093621a2b75eee5 Mon Sep 17 00:00:00 2001 From: frank Date: Wed, 22 Nov 2017 13:00:02 +0100 Subject: [PATCH] worked on grid-walking --- grid/walk/v3/Reachable.h | 257 ++++++++++++++++++++++++++++++++ grid/walk/v3/ReachableSampler.h | 81 ++++++++++ grid/walk/v3/WalkEvaluator.h | 10 +- grid/walk/v3/Walker.h | 67 +++++++-- 4 files changed, 401 insertions(+), 14 deletions(-) create mode 100644 grid/walk/v3/Reachable.h create mode 100644 grid/walk/v3/ReachableSampler.h diff --git a/grid/walk/v3/Reachable.h b/grid/walk/v3/Reachable.h new file mode 100644 index 0000000..e353bcf --- /dev/null +++ b/grid/walk/v3/Reachable.h @@ -0,0 +1,257 @@ +#ifndef INDOOR_GW3_REACHABLE_H +#define INDOOR_GW3_REACHABLE_H + +#include +#include +#include "../../Grid.h" + +namespace GW3 { + +#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 class ReachableByDepthUnsorted { + + struct VisitEntry { + const Node* gn; + int depth; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} + }; + + struct Visits { + VisitEntry visits[512];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 512); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + }; + + const Grid& grid; + +public: + + ReachableByDepthUnsorted(const Grid& grid) : grid(grid) { + ; + } + + /** get all nodes reachable from start using maxDepth steps */ + std::unordered_set get(const Node& start, const int maxDepth) { + + std::unordered_set checked; + + // assuming max 8 neighbors per node, we need + // we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth) + // which is 1 + (1+2+3+4+5)*neighbors + // which is 1 + (n*n + n)/2*neighbors + // however this seems to be slow?! + //const int n = maxDepth + 1; + //const int maxEntries = (n * n + n) / 2 * 10 + 1; + //const int toAlloc = 4096 / sizeof(VisitEntry); + //if ( unlikely(toAlloc < maxEntries) ) {return checked;} + //if (maxDepth > 9) {throw Exception("will not fit!");} + + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + checked.insert(&gnNext); + toVisit.add(VisitEntry(&gnNext, 1)); + } + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + 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()) ) { + toVisit.add(VisitEntry(&gnNext, e.depth+1)); + checked.insert(&gnNext); + } + } + + } + + } + + return checked; + + } + +}; + + +/** + * get all grid nodes that are reachable within x-edges (depth) + * additionally returns the needed walking distance in meter + */ +template class ReachableByDepthWithDistanceSorted { + + struct VisitEntry { + const Node* gn; + int depth; + float dist_m; + int myIdx; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) : + gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;} + }; + + struct Visits { + VisitEntry visits[1024];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 1024); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + void sort() { + const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { + return e1.dist_m < e2.dist_m; + }; + std::sort(&visits[tail], &visits[head], comp); + } + }; + + const Grid& grid; + +public: + + /** result */ + struct Entry { + + const Node* node; + const float walkDistToStart_m; + const int prevIdx; + + Entry(const Node* node, const float dist, const size_t prevIdx) : + node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;} + + bool hasPrev() const { + return prevIdx >= 0; + } + + }; + + ReachableByDepthWithDistanceSorted(const Grid& grid) : grid(grid) { + ; + } + + /** get all nodes reachable from start using maxDepth steps */ + std::vector get(const Node& start, const int maxDepth) { + + std::unordered_set checked; + std::vector res; + + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + res.push_back(Entry(&start, 0, -1)); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + const float dist_m = gnNext.getDistanceInMeter(start); + toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, 0)); + checked.insert(&gnNext); + } + toVisit.sort(); + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + 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); +// } +// } + +// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01; + + std::vector sub; + + 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())); + sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, e.myIdx)); + checked.insert(&gnNext); + + } + } + + // dijkstra.. sort the new nodes by destination to start + // only sorting the 8 new nodes seems enough due to the graph's layout + const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { + return e1.dist_m < e2.dist_m; + }; + + std::sort(sub.begin(), sub.end(), comp); + + for (const VisitEntry& e : sub) { + toVisit.add(e); + } + + } + + // slower with same result ;) + //toVisit.sort(); + + } + + return res; + + } + +}; + +} + +#endif // REACHABLE_H diff --git a/grid/walk/v3/ReachableSampler.h b/grid/walk/v3/ReachableSampler.h new file mode 100644 index 0000000..7222ece --- /dev/null +++ b/grid/walk/v3/ReachableSampler.h @@ -0,0 +1,81 @@ +#ifndef INDOOR_GW3_REACHABLESAMPLER_H +#define INDOOR_GW3_REACHABLESAMPLER_H + +#include "../../../math/Random.h" + +#include "Reachable.h" +#include "Helper.h" + +namespace GW3 { + + template class ReachableSamplerByDepth { + + public: + + using Entry = typename ReachableByDepthWithDistanceSorted::Entry; + + struct SampleResult { + Point3 pos; + float walkDistToStart_m; + SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;} + }; + + private: + + const Grid& grid; + const float gridSize_m; + + const std::vector& reachableNodes; + + mutable RandomGenerator gen; + + mutable std::uniform_real_distribution dOffset; + + + public: + + /** ctor */ + ReachableSamplerByDepth(const Grid& grid, const std::vector& reachableNodes) : + grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) { + ; + } + + SampleResult sample() { + + std::uniform_int_distribution dIdx(0, reachableNodes.size() - 1); + + const int idx = dIdx(gen); + + const Entry* e = &reachableNodes[idx]; + const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]); + const Node* nDst = e->node; + + // center of the destination node + const Point3 nodeCenter = Helper::gpToP3(*nDst); + + // random position within destination-node + const float ox = dOffset(gen); + const float oy = dOffset(gen); + + // 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); + + // calculate end's walking-distance towards the start + float distToStart_m; + if (ePrev1) { + distToStart_m = ePrev1->walkDistToStart_m + (Helper::gpToP3(*(ePrev1->node)).getDistance(end)); + } else { + distToStart_m = nodeCenter.getDistance(end); + } + + // done + return SampleResult(end, distToStart_m); + + } + + + }; + +} + +#endif // REACHABLESAMPLER_H diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index ccc9029..c34080a 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -17,7 +17,7 @@ namespace GW3 { /** get the probability for the given walk */ //virtual double getProbability(const Walk& walk) const = 0; - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0; + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0; }; @@ -31,7 +31,7 @@ namespace GW3 { WalkEvalEndNodeProbability(Grid* grid) : grid(grid) {;} - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override { (void) params; (void) pStart; @@ -65,7 +65,7 @@ namespace GW3 { ; } - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override { (void) params; @@ -95,10 +95,10 @@ namespace GW3 { 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 { + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override { const float requestedDistance_m = params.distance_m; - const float walkedDistance_m = pStart.getDistance(pEnd); + const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd); 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 92f2d67..f15101d 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -14,6 +14,8 @@ #include "Helper.h" #include "Structs.h" #include "WalkEvaluator.h" +#include "Reachable.h" +#include "ReachableSampler.h" namespace GW3 { @@ -117,7 +119,7 @@ namespace GW3 { double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); + const double p1 = eval->getProbability(start, end, start.getDistance(end), params); p *= p1; } @@ -156,7 +158,7 @@ namespace GW3 { /** ctor */ WalkerWeightedRandom(Grid& grid) : - grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f) { + grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) { ; } @@ -181,6 +183,7 @@ namespace GW3 { if (!startNode) {throw Exception("start node not found!");} const float maxDist = params.distance_m + gridSize_m; + const int depth = std::ceil(params.distance_m / gridSize_m) + 1; Point3 best; double bestP = 0; //DrawList drawer; @@ -206,15 +209,17 @@ namespace GW3 { int numVisitedNodes = 0; - #define MODE 1 + #define MODE 3 #if (MODE == 1) double bestNodeP = 0; const Node* bestNode = nullptr; - while(ri.hasNext()) { - const Node* dstNode = &ri.next(); + ReachableByDepthUnsorted reach(grid); + std::unordered_set nodes = reach.get(*startNode, depth); + + for (const Node* dstNode : nodes) { const Point3 nodeCenter = Helper::gpToP3(*dstNode); double p = 1.0; for (const WalkEvaluator* eval : evals) { @@ -227,6 +232,20 @@ namespace GW3 { } } +// 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); @@ -250,16 +269,21 @@ namespace GW3 { #elif (MODE == 2) - // all reachable nodes - while(ri.hasNext()) { + ReachableByDepthUnsorted reach(grid); + std::unordered_set nodes = reach.get(*startNode, depth); + + // all reachable nodes + //while(ri.hasNext()) { + //const Node* dstNode = &ri.next(); + + for (const Node* dstNode : nodes) { - 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) { + for (int i = 0; i < 3; ++i) { // random position within destination-node const float ox = dFinal(rndGen); @@ -289,6 +313,31 @@ namespace GW3 { } + #elif (MODE == 3) + + using Reachable = ReachableByDepthWithDistanceSorted; + using ReachableNode = typename Reachable::Entry; + Reachable reach(grid); + std::vector reachableNodes = reach.get(*startNode, depth); + + using Sampler = ReachableSamplerByDepth; + using SamplerResult = typename Sampler::SampleResult; + Sampler sampler(grid, reachableNodes); + + for (int i = 0; i < 1500; ++i) { + + const SamplerResult sample = sampler.sample(); + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, sample.pos, sample.walkDistToStart_m*0.94, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = sample.pos;} + + } + #endif //std::cout << numVisitedNodes << std::endl;