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

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;