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

worked on grid-walker and synthetic steps/turns

Browse Source
This commit is contained in:
k-a-z-u
2017-11-29 16:35:29 +01:00
parent 55c061b344
commit 63bc2f3046
5 changed files with 370 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -65,35 +65,63 @@ namespace GW3 {
const GridPoint gpStart = Helper::p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
// calculate a walk's probability
auto getP = [&] (const Point3 dst) {
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params);
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.distance_m+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);
// 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);
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 {
@@ -129,7 +157,7 @@ namespace GW3 {
}
res.heading = Heading(start.xy(), end.xy());
res.probability = p;
res.probability *= getP(end);
res.position = end;
return res;