FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on grid-walker

Browse Source
This commit is contained in:
k-a-z-u
2017-10-25 16:58:41 +02:00
parent feaa2ea12c
commit ea351d6377
4 changed files with 273 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
grid/walk/v3/Helper.h
View File

@@ -6,10 +6,146 @@
#include "Structs.h"
#include <vector>
#include <set>
#include <KLib/math/random/RandomIterator.h>
//#define SHOW_DEBUG_PLOT
#ifdef SHOW_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
#endif
namespace GW3 {
/** get an iterator over all nodes reachable from the given start */
template <typename Node> class ReachableIteratorSorted {
const Grid<Node>& grid;
const Node& start;
struct Next {
uint32_t idx;
float distToStart;
Next(uint32_t idx, float distToStart) : idx(idx), distToStart(distToStart) {;}
/** compare by weight. same weight? : compare by pointer */
bool operator < (const Next& o) const {
return (distToStart != o.distToStart) ? (distToStart < o.distToStart) : (idx < o.idx);
}
};
Node* curNode = nullptr;
std::unordered_set<uint32_t> visited;
std::set<Next> toVisit;
public:
ReachableIteratorSorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
toVisit.insert(Next(start.getIdx(),0));
}
bool hasNext() const {
return !toVisit.empty();
}
const Node& next() {
const Next cur = *(toVisit.begin()); // visit from inside out (needed for correct distance)
toVisit.erase(toVisit.begin());
visited.insert(cur.idx);
const Node& curNode = grid[cur.idx];
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
const int neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
const float newDist = cur.distToStart + curNode.getDistanceInMeter(neighbor);
// not yet reached -> store distance
if (visited.find(neighborIdx) == visited.end()) {
toVisit.insert(Next(neighborIdx, newDist));
}
}
// done
return curNode;
}
};
/** get an iterator over all nodes reachable from the given start */
template <typename Node> class ReachableIteratorUnsorted {
const Grid<Node>& grid;
const Node& start;
Node* curNode = nullptr;
std::unordered_set<uint32_t> visited;
std::vector<uint32_t> toVisit;
public:
ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
toVisit.push_back(start.getIdx());
}
bool hasNext() const {
return !toVisit.empty();
}
const Node& next(const std::function<bool(const Node&)>& skip) {
const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance)
toVisit.erase(toVisit.begin());
visited.insert(curIdx);
const Node& curNode = grid[curIdx];
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
const int neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
// not yet reached -> store distance
if (!skip(neighbor)) {
if (visited.find(neighborIdx) == visited.end()) {
toVisit.push_back(neighborIdx);
}
}
}
// done
return curNode;
}
};
struct ReachableSettings {
float dist_m;
bool limitDistance = true;
Heading heading = Heading(0);
float maxHeadingDiff_rad;
bool limitHeading = false;
};
template <typename Node> class Helper {
public:
@@ -74,54 +210,80 @@ namespace GW3 {
return walks;
}
}
/** get all reachable nodes that are within a given range */
static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const ReachableSettings& set ) {
//auto tStart = std::chrono::system_clock::now();
Nodes<Node> res;
std::unordered_map<uint32_t, float> distances;
std::vector<uint32_t> toVisit;
std::vector<uint32_t> toVisit; // std::queue was only barely faster: 900 vs 880 microseconds
toVisit.push_back(start->getIdx());
distances[start->getIdx()] = 0.0f;
#ifdef SHOW_DEBUG_PLOT
static K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementColorPoints pts1; pts1.setPointType(7); pts1.setPointSize(1);
plot.add(&pts1);
#endif
while (!toVisit.empty()) {
int curIdx = toVisit.front();
const int curIdx = toVisit.front(); // visit from inside out (needed for correct distance)
toVisit.erase(toVisit.begin());
const Node& curNode = grid[curIdx];
const float curDistance = distances[curIdx];
res.push_back(&curNode); // remember for output
res.push_back(&curNode); // remember for output
if (curDistance <= dist_m) {
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
#ifdef SHOW_DEBUG_PLOT
pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance);
gp.draw(plot);
gp.flush();
#endif
const int neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
const float totalDist = curDistance + addDist;
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
// this is like in dijkstra. keep the smallest distance to reach a node:
const int neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
const float addDist = neighbor.getDistanceInMeter(curNode);
const float totalDist = curDistance + addDist;
// not yet reached -> store distance
if (distances.find(neighborIdx) == distances.end()) {
toVisit.push_back(neighborIdx);
distances[neighborIdx] = totalDist;
// this is like in dijkstra. keep the smallest distance to reach a node:
// reached earlier but found shorter way
} else {
if (distances[neighborIdx] > totalDist) {
distances[neighborIdx] = totalDist;
}
// not yet reached -> store distance
if (distances.find(neighborIdx) == distances.end()) {
distances[neighborIdx] = totalDist;
if (set.limitDistance) {
if (totalDist > set.dist_m) {continue;}
}
if (set.limitHeading) {
const Heading head(start->x_cm, start->y_cm, neighbor.x_cm, neighbor.y_cm); // angle between start and current node
const float diff = head.getDiffHalfRAD(set.heading); // difference between above angle and requested angle
if (diff > set.maxHeadingDiff_rad) {continue;} // more than 90 degree difference? -> ignore
}
toVisit.push_back(neighborIdx); // needs a visit? (still some distance left)
// reached earlier but found shorter way
} else if (distances[neighborIdx] > totalDist) {
distances[neighborIdx] = totalDist;
}
}
}
//auto tEnd = std::chrono::system_clock::now();
//auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(tEnd - tStart);
//std::cout << elapsed.count() << std::endl;
return res;
}

4
grid/walk/v3/Structs.h
View File

@@ -9,9 +9,13 @@ namespace GW3 {
/** paremters for the walk */
struct WalkParams {
Point3 start;
float distance_m;
Heading heading = Heading(0);
float lookFurther_m = 1.5;
};
/** result of the random walk */

10
grid/walk/v3/WalkEvaluator.h
View File

@@ -50,8 +50,12 @@ namespace GW3 {
/** evaluate the difference between head(start,end) and the requested heading */
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
const double sigma;
public:
WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
(void) params;
@@ -64,7 +68,7 @@ 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<double>::getProbability(0, 0.04, diff);
return Distribution::Normal<double>::getProbability(0, sigma, diff);
}
@@ -73,10 +77,12 @@ namespace GW3 {
/** evaluate the difference between distance(start, end) and the requested distance */
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
const double sigma = 0.1f;
const double sigma;
public:
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
const float walkedDistance_m = pStart.getDistance(pEnd);

110
grid/walk/v3/Walker.h
View File

@@ -52,24 +52,30 @@ namespace GW3 {
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
Assert::isNot0(params.distance_m, "walking distance must be > 0");
static std::mt19937 rndGen;
const GridPoint gpStart = Helper::p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
// include one additional grid-cell (increased distance)
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
const float range_m = params.distance_m + secBuffer_m;
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
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);
WalkResult res;
res.heading = params.heading;
res.position = params.start;
float realDist_m = params.distance_m;
const Point2 dir = res.heading.asVector();
const Point2 dst = params.start.xy() + (dir * realDist_m);
const Point2 dst = params.start.xy() + (dir * params.distance_m);
// is dst reachable?
const Node* n = Helper::contains(grid, reachableNodes, dst);
@@ -80,12 +86,14 @@ namespace GW3 {
if (grid.hasNodeFor(gp)) {
res.position = p3; // update position
res.heading; // keep as-is
res.probability; // keep as-is
//res.heading; // keep as-is
//res.probability; // keep as-is
return res; // done
} else {
std::cout << "WARN dst not found" << std::endl;
//throw "should not happen";
}
}
@@ -150,61 +158,99 @@ namespace GW3 {
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, 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);
// include one additional grid-cell (increased distance)
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
const float range_m = params.distance_m + secBuffer_m;
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
// // include one additional grid-cell (increased distance)
// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
// 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);
// not found -> try random pick among all reachable nodes
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
Point3 best;
double bestP = 0;
// DrawList<Point3> drawer;
const Point3 start = params.start;
// try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles")
for (int i = 0; i < 25; ++i) {
// 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)];
// const Node* dstNode = reachableNodes[dNode(rndGen)];
// random position within destination-node
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f);
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
// destination = node-center + offset (within the node's bbox)
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
// sanity check
Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
if (start == end) {continue;}
ReachableIteratorUnsorted<Node> 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;
};
//for (const Node* dstNode : reachableNodes) {
while(ri.hasNext()) {
const Node* dstNode = &ri.next(skip);
// const float dist_m = dstNode->getDistanceInMeter(gpStart);
// if (dist_m > maxDist) {
// break;
// }
for (int i = 0; i < 25; ++i) {
// random position within destination-node
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
// 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");
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
//drawer.add(end, p);
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
}
//const Point3 end = lst.get();
//const Point3 end = drawer.get();
const Point3 end = best;
WalkResult res;
if (start == end) {
res.probability = 0;
} else {
res.heading = Heading(start.xy(), end.xy());
res.probability = bestP; // 1
res.probability = bestP;
}
res.position = end;
return res;