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

merged

Browse Source
This commit is contained in:
toni
2018-01-17 10:26:16 +01:00
parent 74981c6a45 55061ef0da
commit c02d07af51
67 changed files with 16100 additions and 2117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,6 +14,8 @@
#include "Helper.h"
#include "Structs.h"
#include "WalkEvaluator.h"
#include "Reachable.h"
#include "ReachableSampler.h"
namespace GW3 {
@@ -26,18 +28,22 @@ namespace GW3 {
public:
/** get a new destination for the given start */
virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
virtual const WalkResult getDestination(const WalkParams& params) const = 0;
};
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
//Random::RandomGenerator rnd;
Grid<Node>& grid;
std::vector<WalkEvaluator<Node>*> evals;
public:
/** ctor */
WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
;
}
/** make the code a little more readable */
using Helper = GW3::Helper<Node>;
using Walk = typename GW3::Walk<Node>;
@@ -50,44 +56,76 @@ namespace GW3 {
}
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
Assert::isNot0(params.distance_m, "walking distance must be > 0");
static std::mt19937 rndGen;
const GridPoint gpStart = Helper::p3ToGp(params.start);
const GridPoint gpStart = grid.toGridPoint(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
// calculate a walk's probability
auto getP = [&] (const Point3 dst) {
double p = 1;
const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst));
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(pWalk);
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.getDistanceInMeter(grid) + 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);
const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid));
// 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);
const GridPoint gp = grid.toGridPoint(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 {
@@ -111,9 +149,11 @@ namespace GW3 {
const Point3 start = params.start;
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
const PotentialWalk pWalk(params, start, end, start.getDistance(end));
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params);
const double p1 = eval->getProbability(pWalk);
p *= p1;
}
@@ -123,8 +163,31 @@ namespace GW3 {
}
res.heading = Heading(start.xy(), end.xy());
res.probability = p;
res.probability *= getP(end);
res.position = end;
if (!grid.hasNodeFor(grid.toGridPoint(res.position))) {
std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl;
std::cout << "issue:" << res.position.asString() << std::endl;
for (int i = -80; i <= +80; i+=10) {
Point3 pos = res.position + Point3(0,0,i/100.0f);
std::cout << pos.asString() << " ----- " << res.position.asString() << std::endl;
std::cout << (grid.toGridPoint(pos)).asString() << std::endl;
std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl;
std::cout << std::endl;
}
int i = 0; (void) i;
}
#if (GRID_MODE == GM_BOX)
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
#endif
return res;
}
@@ -142,8 +205,20 @@ namespace GW3 {
std::vector<WalkEvaluator<Node>*> evals;
Grid<Node>& grid;
const float gridSize_m;
mutable std::minstd_rand rndGen;
mutable std::uniform_real_distribution<float> dFinal;
public:
/** ctor */
WalkerWeightedRandom(Grid<Node>& grid) :
grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
;
}
/** make the code a little more readable */
using Helper = GW3::Helper<Node>;
using Walk = typename GW3::Walk<Node>;
@@ -156,101 +231,189 @@ namespace GW3 {
}
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.distance_m, "walking distance must be > 0");
const float walkDist_m = params.getDistanceInMeter(grid);
static std::minstd_rand rndGen;
Assert::isNot0(walkDist_m, "walking distance must be > 0");
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 = walkDist_m + gridSize_m;
const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
// 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<int> dNode(0, (int)reachableNodes.size() - 1);
Point3 best;
double bestP = 0;
// DrawList<Point3> drawer;
Point3 best; double bestP = 0;
//DrawList<Point3> 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<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
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;
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<Node, RICond> 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;
#define MODE 2
#if (MODE == 1)
double bestNodeP = 0;
const Node* bestNode = nullptr;
ReachableByDepthUnsorted<Node> reach(grid);
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
for (const Node* dstNode : nodes) {
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
double p = 1.0;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
p *= p1;
}
if (p > bestNodeP) {
bestNodeP = p;
bestNode = dstNode;
}
}
// while(ri.hasNext()) {
// const Node* dstNode = &ri.next();
// const Point3 nodeCenter = Helper::gpToP3(*dstNode);
// double p = 1.0;
// for (const WalkEvaluator<Node>* eval : evals) {
// const double p1 = eval->getProbability(start, nodeCenter, params);
// p *= p1;
// }
// if (p > bestNodeP) {
// bestNodeP = p;
// bestNode = dstNode;
// }
// }
for (int i = 0; i < 25; ++i) {
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);
const float walkDist_m = end.getDistance(start);//*1.05;
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params);
const double p1 = eval->getProbability(start, end, walkDist_m, params);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
//drawer.add(end, p);
}
}
#elif (MODE == 2)
//const Point3 end = drawer.get();
ReachableByDepthUnsorted<Node> reach(grid);
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
// all reachable nodes
//while(ri.hasNext()) {
//const Node* dstNode = &ri.next();
for (const Node* dstNode : nodes) {
++numVisitedNodes;
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
// try multiple locations within each reachable node
for (int i = 0; i < 3; ++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");
const float walkDist_m = end.getDistance(start);//*1.05;
const PotentialWalk pWalk(params, start, end, walkDist_m);
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(pWalk);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
// drawer.add(end, p);
}
}
#elif (MODE == 3)
using Reachable = ReachableByDepthWithDistanceSorted<Node>;
using ReachableNode = typename Reachable::Entry;
Reachable reach(grid);
std::vector<ReachableNode> reachableNodes = reach.get(*startNode, depth);
using Sampler = ReachableSamplerByDepth<Node>;
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<Node>* 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;
//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;