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

worked on grid walker

Browse Source
This commit is contained in:
k-a-z-u
2017-11-15 16:41:57 +01:00
parent 08d8292976
commit 7af5131ccf
4 changed files with 190 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -39,7 +39,8 @@ namespace GW3 {
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
const Node& node = grid->getNodeFor(gp);
const double p = node.getWalkImportance();
return std::pow(p,10);
return p;
//return std::pow(p,10);
}
@@ -50,11 +51,19 @@ namespace GW3 {
/** evaluate the difference between head(start,end) and the requested heading */
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
const double sigma;
const double sigma_rad;
const double kappa;
Distribution::VonMises<double> _dist;
Distribution::LUT<double> dist;
public:
WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
// 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 Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
@@ -68,7 +77,8 @@ namespace GW3 {
const Heading head(pStart.xy(), pEnd.xy());
const float diff = head.getDiffHalfRAD(params.heading);
//const float diff = Heading::getSignedDiff(params.heading, head);
return Distribution::Normal<double>::getProbability(0, sigma, diff);
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
}
@@ -79,14 +89,19 @@ namespace GW3 {
const double sigma;
const Distribution::Normal<double> dist;
public:
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
const float requestedDistance_m = params.distance_m;
const float walkedDistance_m = pStart.getDistance(pEnd);
return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
const float diff = walkedDistance_m - requestedDistance_m;
return dist.getProbability(diff);
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
}