145 lines
3.9 KiB
C++
145 lines
3.9 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef INDOOR_GW3_WALKEVALUATOR_H
|
||
#define INDOOR_GW3_WALKEVALUATOR_H
|
||
|
||
#include "Structs.h"
|
||
#include "Helper.h"
|
||
|
||
#include "../../../math/Distributions.h"
|
||
#include "../../../grid/Grid.h"
|
||
|
||
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 {
|
||
|
||
public:
|
||
|
||
/** get the probability for the given walk */
|
||
//virtual double getProbability(const Walk<Node>& walk) const = 0;
|
||
|
||
virtual double getProbability(const PotentialWalk& walk) const = 0;
|
||
|
||
};
|
||
|
||
|
||
/** evaluate the grid-node-importance importance(end) */
|
||
template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {
|
||
|
||
Grid<Node>* grid;
|
||
|
||
public:
|
||
|
||
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
|
||
|
||
virtual double getProbability(const PotentialWalk& walk) const override {
|
||
|
||
const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
|
||
const Node& node = grid->getNodeFor(gp);
|
||
const double p = node.getWalkImportance();
|
||
return p;
|
||
//return std::pow(p,10);
|
||
|
||
}
|
||
|
||
};
|
||
|
||
|
||
|
||
/** evaluate the difference between head(start,end) and the requested heading */
|
||
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
|
||
|
||
const double sigma_rad;
|
||
const double kappa;
|
||
Distribution::VonMises<double> _dist;
|
||
Distribution::LUT<double> dist;
|
||
|
||
public:
|
||
|
||
// kappa = 1/var = 1/sigma^2
|
||
// https://en.wikipedia.org/wiki/Von_Mises_distribution
|
||
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
|
||
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
|
||
;
|
||
}
|
||
|
||
virtual double getProbability(const PotentialWalk& walk) const override {
|
||
|
||
if (walk.pStart == walk.pEnd) {
|
||
std::cout << "warn! start-position == end-positon" << std::endl;
|
||
return 0;
|
||
}
|
||
|
||
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);
|
||
|
||
}
|
||
|
||
};
|
||
|
||
/** 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 Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {
|
||
Assert::isFalse(grid.isEmpty(), "empty grid given");
|
||
}
|
||
|
||
virtual double getProbability(const PotentialWalk& walk) const override {
|
||
|
||
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);
|
||
|
||
}
|
||
|
||
};
|
||
|
||
}
|
||
|
||
#endif // INDOOR_GW3_WALKEVALUATOR_H
|