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

worked on grid-walking

Browse Source
This commit is contained in:
FrankE
2017-11-22 13:00:02 +01:00
parent 89c6b07e68
commit d03372ad3d
4 changed files with 401 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
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 {
@@ -117,7 +119,7 @@ namespace GW3 {
double p = 1;
for (const WalkEvaluator<Node>* 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<Node>& 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<Point3> 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<Node> reach(grid);
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
for (const Node* dstNode : nodes) {
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
double p = 1.0;
for (const WalkEvaluator<Node>* 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<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 < 10; ++i) {
const Point3 nodeCenter = Helper::gpToP3(*bestNode);
@@ -250,16 +269,21 @@ namespace GW3 {
#elif (MODE == 2)
// all reachable nodes
while(ri.hasNext()) {
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) {
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<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;