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

added new helper methods

Browse Source 
worked on gridWalker v3
This commit is contained in:
FrankE
2017-10-17 13:01:26 +02:00
parent 556bbe8829
commit 3807c621c7
8 changed files with 299 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
grid/walk/v3/GridWalker3.h
View File

@@ -8,12 +8,14 @@
#include "../../../math/Stats.h"
#include "../../../geo/Heading.h"
#include "../../../math/stats/Variance.h"
#include "GridWalk3Helper.h"
#include "../../../geo/BBox2.h"
/**
* modular grid-walker that takes various sub-components to determine
* p(e) and thus randomly pick edges
*/
template <typename Node, typename WalkState, typename Control> class GridWalker3 {
template <typename Node> class GridWalker3 {
private:
@@ -24,38 +26,135 @@ private:
public:
/** one walk along several nodes */
struct Walk : public std::vector<const Node*> {
struct WalkParams {
Point3 start;
float distance_m;
Heading heading = Heading(0);
};
// struct Walks : std::vector<Walk> {
// };
using Helper = GridWalk3Helper<Node>;
using Walk = typename GridWalk3Helper<Node>::Walk3;
using Walks = typename GridWalk3Helper<Node>::Walks3;
using Nodes = typename GridWalk3Helper<Node>::Nodes3;
GridPoint p3ToGp(const Point3 p) const {
const Point3 p100 = p*100;
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
}
Point3 gpToP3(const GridPoint gp) const {
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
}
/** perform the walk based on the configured setup */
const Node* getDestination(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
const Point3 getDestination(Grid<Node>& grid, const WalkParams& params) {
//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
return _drawThenCheck(grid, params);
const Node* startNode = grid.getNodePtrFor(start);
}
Heading desiredHeading = ctrl.heading;
// /** perform the walk based on the configured setup */
// const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {
DrawList<Walk> walks;
// //return _getFromPossibleWalks(grid, start, ctrl, dist_m);
// //return _drawThenCheck(grid, start, ctrl, dist_m);
// throw "error";
// }
const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
const float d = gridSize_m / 2.0f;
const Point2 pMin = n->inMeter().xy() - Point2(d, d);
const Point2 pMax = n->inMeter().xy() + Point2(d, d);
const BBox2 bbox(pMin, pMax);
return bbox.contains(pt);
}
const Point3 _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
const GridPoint gpStart = p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
static Distribution::Normal<float> dDist(1, 0.02);
static Distribution::Normal<float> dHead(0, 0.02);
// 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 nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
float realDist_m = params.distance_m;
Heading realHead = params.heading;// + dHead.draw();
int cnt = 0;
while(true) {
const Point2 dir = realHead.asVector();
const Point2 dst = params.start.xy() + (dir * realDist_m);
// is dst reachable?
for (const Node* n : nodes) {
//const float distToNode = n->inMeter().xy().getDistance(dst);
//if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) {
if (contains(grid, n, dst)) {
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = p3ToGp(p3);
if (grid.hasNodeFor(gp)) {
return p3;
} else {
std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
}
}
}
// before trying again, modify distance and angle
if (1 == 0) {
realDist_m *= dDist.draw();
realHead += dHead.draw();
}
// reached max retries?
if (++cnt > 10) {return params.start;} // did not work out....
for (int i = 0; i < 100; ++i) {
const Walk walk = getRandomWalk(grid, startNode, dist_m);
const double prob = eval(walk, desiredHeading);
walks.add(walk, prob);
}
Walk res = walks.get();
const Node* dst = res.back();
return dst;
throw "error";
}
// const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
// const Node* startNode = grid.getNodePtrFor(start);
// Heading desiredHeading = ctrl.heading;
// DrawList<Walk> weightedWalks;
// const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
// for (const Walk& walk : walks) {
// const double prob = eval(walk, desiredHeading, dist_m);
// weightedWalks.add(walk, prob);
// }
// Walk res = weightedWalks.get();
// const Node* dst = res.back();
// return dst;
// }
double evalDistance(const Walk& w, const float desiredDist) const {
const Node* nStart = w.front();
const Node* nEnd = w.back();
const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
}
double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
const Node* nStart = w.front();
const Node* nEnd = w.back();
@@ -83,39 +182,43 @@ public:
return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
}
double eval(const Walk& w, const Heading desiredHeading) const {
double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
return 1.0
* evalHeadingStartEnd(w, desiredHeading)
* evalDistance(w, desiredDistance)
// * evalHeadingChanges(w);
;
}
Walk getRandomWalk(Grid<Node>& grid, const Node* start, const float dist_m) const {
Walk walk;
float dist = 0;
const Node* cur = start;
while(true) {
// Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
walk.push_back(cur);
if (dist > dist_m) {break;}
// Walk walk;
const int numNeighbors = cur->getNumNeighbors();
//std::cout << "neighbors: " << numNeighbors << std::endl;
int idx = rand() % numNeighbors;
const Node* next = &grid.getNeighbor(*cur, idx);
dist += next->inMeter().getDistance(cur->inMeter());
cur = next;
// float dist = 0;
}
// const Node* cur = start;
// while(true) {
return walk;
// walk.push_back(cur);
// if (dist > dist_m) {break;}
}
// const int numNeighbors = cur->getNumNeighbors();
// //std::cout << "neighbors: " << numNeighbors << std::endl;
// int idx = rand() % numNeighbors;
// const Node* next = &grid.getNeighbor(*cur, idx);
// dist += next->inMeter().getDistance(cur->inMeter());
// cur = next;
// }
// return walk;
// }
};