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

started testing a new grid-builder

Browse Source 
minor fixes
worked on walkers
This commit is contained in:
k-a-z-u
2017-12-20 17:12:30 +01:00
parent d48b0b8fd4
commit c346b7f222
7 changed files with 709 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,6 +9,29 @@
namespace GW3 {
/** describes a potential walk, which can be evaluated */
struct PotentialWalk {
/** initial parameters (requested walk) */
const WalkParams& params;
/** walk started here */
Point3 pStart;
/** walk ended here */
Point3 pEnd;
/** usually the euclidean distance start<->end but not necessarily! */
float walkDist_m;
/** ctor */
PotentialWalk(const WalkParams& params, const Point3 pStart, const Point3 pEnd, const float walkedDistance_m) :
params(params), pStart(pStart), pEnd(pEnd), walkDist_m(walkedDistance_m) {
;
}
};
/** interface for all evaluators that return a probability for a given walk */
template <typename Node> class WalkEvaluator {
@@ -17,7 +40,7 @@ namespace GW3 {
/** get the probability for the given walk */
//virtual double getProbability(const Walk<Node>& walk) const = 0;
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0;
virtual double getProbability(const PotentialWalk& walk) const = 0;
};
@@ -31,12 +54,9 @@ namespace GW3 {
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
(void) params;
(void) pStart;
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
const Node& node = grid->getNodeFor(gp);
const double p = node.getWalkImportance();
return p;
@@ -65,17 +85,15 @@ namespace GW3 {
;
}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
(void) params;
if (pStart == pEnd) {
if (walk.pStart == walk.pEnd) {
std::cout << "warn! start-position == end-positon" << std::endl;
return 0;
}
const Heading head(pStart.xy(), pEnd.xy());
const float diff = head.getDiffHalfRAD(params.heading);
const Heading head(walk.pStart.xy(), walk.pEnd.xy());
const float diff = head.getDiffHalfRAD(walk.params.heading);
//const float diff = Heading::getSignedDiff(params.heading, head);
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
@@ -87,18 +105,20 @@ namespace GW3 {
/** evaluate the difference between distance(start, end) and the requested distance */
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
const Grid<Node>& grid;
const double sigma;
const Distribution::Normal<double> dist;
public:
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
const float requestedDistance_m = params.distance_m;
const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd);
const float requestedDistance_m = walk.params.getDistanceInMeter(grid);
const float walkedDistance_m = walk.walkDist_m;//pStart.getDistance(pEnd);
const float diff = walkedDistance_m - requestedDistance_m;
return dist.getProbability(diff);
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);