596 lines
17 KiB
C++
596 lines
17 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_WALKER_H
|
||
#define INDOOR_GW3_WALKER_H
|
||
|
||
#include "../../../data/Timestamp.h"
|
||
#include "../../../math/DrawList.h"
|
||
#include "../../../math/Distributions.h"
|
||
#include "../../../math/Stats.h"
|
||
#include "../../../geo/Heading.h"
|
||
#include "../../../math/stats/Variance.h"
|
||
#include "../../../geo/BBox2.h"
|
||
|
||
#include "../../Grid.h"
|
||
|
||
#include "Helper.h"
|
||
#include "Structs.h"
|
||
#include "WalkEvaluator.h"
|
||
#include "Reachable.h"
|
||
#include "ReachableSampler.h"
|
||
|
||
namespace GW3 {
|
||
|
||
/**
|
||
* modular grid-walker that takes various sub-components to determine
|
||
* p(e) and thus randomly pick edges
|
||
*/
|
||
template <typename Node> class WalkerBase {
|
||
|
||
public:
|
||
|
||
/** get a new destination for the given start */
|
||
virtual const WalkResult getDestination(const WalkParams& params) const = 0;
|
||
|
||
};
|
||
|
||
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
|
||
|
||
Grid<Node>& grid;
|
||
std::vector<WalkEvaluator<Node>*> evals;
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
|
||
;
|
||
}
|
||
|
||
/** make the code a little more readable */
|
||
using Helper = GW3::Helper<Node>;
|
||
using Walk = typename GW3::Walk<Node>;
|
||
using Walks = typename GW3::Walks<Node>;
|
||
using Nodes = typename GW3::Nodes<Node>;
|
||
|
||
/** add a new evaluator to the walker */
|
||
void addEvaluator(WalkEvaluator<Node>* eval) {
|
||
this->evals.push_back(eval);
|
||
}
|
||
|
||
/** perform the walk based on the configured setup */
|
||
const WalkResult getDestination(const WalkParams& params) const override {
|
||
|
||
Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
|
||
|
||
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
|
||
|
||
|
||
static std::mt19937 rndGen;
|
||
|
||
const GridPoint gpStart = grid.toGridPoint(params.start);
|
||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||
|
||
// calculate a walk's probability
|
||
auto getP = [&] (const Point3 dst) {
|
||
double p = 1;
|
||
const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst));
|
||
for (const WalkEvaluator<Node>* eval : evals) {
|
||
const double p1 = eval->getProbability(pWalk);
|
||
p *= p1;
|
||
}
|
||
return p;
|
||
};
|
||
|
||
// include one additional grid-cell (increased distance)
|
||
//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.getDistanceInMeter(grid) + 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.getDistanceInMeter(grid));
|
||
|
||
// 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 = grid.toGridPoint(p3);
|
||
|
||
|
||
|
||
if (grid.hasNodeFor(gp)) {
|
||
res.position = p3; // update position
|
||
//res.heading; // keep as-is
|
||
res.probability *= getP(p3); // keep as-is
|
||
return res; // done
|
||
|
||
} else {
|
||
std::cout << "WARN dst not found" << std::endl;
|
||
//throw "should not happen";
|
||
|
||
}
|
||
|
||
}
|
||
|
||
// not found -> try random pick among all reachable nodes
|
||
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
|
||
std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
|
||
|
||
const Node* dstNode = reachableNodes[dNode(rndGen)];
|
||
|
||
// random position within destination-node
|
||
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
|
||
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
|
||
|
||
const Point3 start = params.start;
|
||
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
|
||
|
||
const PotentialWalk pWalk(params, start, end, start.getDistance(end));
|
||
|
||
double p = 1;
|
||
for (const WalkEvaluator<Node>* eval : evals) {
|
||
const double p1 = eval->getProbability(pWalk);
|
||
p *= p1;
|
||
}
|
||
|
||
if (start == end) {
|
||
res.probability = 0;
|
||
return res;
|
||
}
|
||
|
||
res.heading = Heading(start.xy(), end.xy());
|
||
res.probability *= getP(end);
|
||
res.position = end;
|
||
|
||
if (!grid.hasNodeFor(grid.toGridPoint(res.position))) {
|
||
|
||
std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl;
|
||
|
||
std::cout << "issue:" << res.position.asString() << std::endl;
|
||
|
||
for (int i = -80; i <= +80; i+=10) {
|
||
Point3 pos = res.position + Point3(0,0,i/100.0f);
|
||
std::cout << pos.asString() << " ----- " << res.position.asString() << std::endl;
|
||
std::cout << (grid.toGridPoint(pos)).asString() << std::endl;
|
||
std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl;
|
||
std::cout << std::endl;
|
||
}
|
||
|
||
int i = 0; (void) i;
|
||
|
||
}
|
||
|
||
#if (GRID_MODE == GM_BOX)
|
||
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
|
||
#endif
|
||
|
||
return res;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
|
||
/**
|
||
* 1) get all reachable nodes from "start" (within a certain distance)
|
||
* 2) randomly pick one of em
|
||
* 3) scatter positon within the node's square -> end position
|
||
* 4) evaluate the probability of walking from start -> end
|
||
*/
|
||
template <typename Node> class WalkerWeightedRandom : public WalkerBase<Node> {
|
||
|
||
std::vector<WalkEvaluator<Node>*> evals;
|
||
|
||
Grid<Node>& grid;
|
||
const float gridSize_m;
|
||
|
||
mutable std::minstd_rand rndGen;
|
||
mutable std::uniform_real_distribution<float> dFinal;
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
WalkerWeightedRandom(Grid<Node>& grid) :
|
||
grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
|
||
;
|
||
}
|
||
|
||
/** make the code a little more readable */
|
||
using Helper = GW3::Helper<Node>;
|
||
using Walk = typename GW3::Walk<Node>;
|
||
using Walks = typename GW3::Walks<Node>;
|
||
using Nodes = typename GW3::Nodes<Node>;
|
||
|
||
/** add a new evaluator to the walker */
|
||
void addEvaluator(WalkEvaluator<Node>* eval) {
|
||
this->evals.push_back(eval);
|
||
}
|
||
|
||
/** perform the walk based on the configured setup */
|
||
const WalkResult getDestination(const WalkParams& params) const override {
|
||
|
||
const float walkDist_m = params.getDistanceInMeter(grid);
|
||
|
||
Assert::isNot0(walkDist_m, "walking distance must be > 0");
|
||
|
||
const GridPoint gpStart = grid.toGridPoint(params.start);
|
||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||
if (!startNode) {throw Exception("start node not found!");}
|
||
|
||
const float maxDist = walkDist_m + gridSize_m;
|
||
const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
|
||
|
||
Point3 best; double bestP = 0;
|
||
//DrawList<Point3> drawer;
|
||
|
||
const Point3 start = params.start;
|
||
|
||
|
||
struct RICond {
|
||
const GridPoint gpStart;
|
||
const float maxDist;
|
||
RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;}
|
||
bool visit (const Node& n) const {
|
||
const float dist_m = n.getDistanceInMeter(gpStart);
|
||
return dist_m < maxDist;
|
||
}
|
||
};
|
||
RICond riCond(gpStart, maxDist);
|
||
|
||
// iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance)
|
||
ReachableIteratorUnsorted<Node, RICond> ri(grid, *startNode, riCond);
|
||
|
||
int numVisitedNodes = 0;
|
||
|
||
|
||
#define MODE 2
|
||
|
||
#if (MODE == 1)
|
||
|
||
double bestNodeP = 0;
|
||
const Node* bestNode = nullptr;
|
||
|
||
ReachableByDepthUnsorted<Node> reach(grid);
|
||
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
|
||
|
||
for (const Node* dstNode : nodes) {
|
||
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
|
||
const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
|
||
double p = 1.0;
|
||
for (const WalkEvaluator<Node>* eval : evals) {
|
||
const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
|
||
p *= p1;
|
||
}
|
||
if (p > bestNodeP) {
|
||
bestNodeP = p;
|
||
bestNode = dstNode;
|
||
}
|
||
}
|
||
|
||
// while(ri.hasNext()) {
|
||
// const Node* dstNode = &ri.next();
|
||
// const Point3 nodeCenter = Helper::gpToP3(*dstNode);
|
||
// double p = 1.0;
|
||
// for (const WalkEvaluator<Node>* eval : evals) {
|
||
// const double p1 = eval->getProbability(start, nodeCenter, params);
|
||
// p *= p1;
|
||
// }
|
||
// if (p > bestNodeP) {
|
||
// bestNodeP = p;
|
||
// bestNode = dstNode;
|
||
// }
|
||
// }
|
||
|
||
for (int i = 0; i < 10; ++i) {
|
||
|
||
const Point3 nodeCenter = Helper::gpToP3(*bestNode);
|
||
|
||
// random position within destination-node
|
||
const float ox = dFinal(rndGen);
|
||
const float oy = dFinal(rndGen);
|
||
|
||
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
|
||
const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
|
||
const float walkDist_m = end.getDistance(start);//*1.05;
|
||
|
||
double p = 1;
|
||
for (const WalkEvaluator<Node>* eval : evals) {
|
||
const double p1 = eval->getProbability(start, end, walkDist_m, params);
|
||
p *= p1;
|
||
}
|
||
|
||
if (p > bestP) {bestP = p; best = end;}
|
||
|
||
}
|
||
|
||
#elif (MODE == 2)
|
||
|
||
ReachableByDepthUnsorted<Node> reach(grid);
|
||
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
|
||
|
||
// all reachable nodes
|
||
//while(ri.hasNext()) {
|
||
//const Node* dstNode = &ri.next();
|
||
|
||
for (const Node* dstNode : nodes) {
|
||
|
||
++numVisitedNodes;
|
||
|
||
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
|
||
|
||
// try multiple locations within each reachable node
|
||
for (int i = 0; i < 3; ++i) {
|
||
|
||
// random position within destination-node
|
||
const float ox = dFinal(rndGen);
|
||
const float oy = dFinal(rndGen);
|
||
|
||
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
|
||
const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
|
||
|
||
// sanity check
|
||
if (start == end) {continue;}
|
||
// if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
|
||
// std::cout << "random destination is not part of the grid" << std::endl;
|
||
// continue;
|
||
// }
|
||
//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
|
||
const float walkDist_m = end.getDistance(start);//*1.05;
|
||
|
||
const PotentialWalk pWalk(params, start, end, walkDist_m);
|
||
|
||
double p = 1;
|
||
for (const WalkEvaluator<Node>* eval : evals) {
|
||
const double p1 = eval->getProbability(pWalk);
|
||
p *= p1;
|
||
}
|
||
|
||
if (p > bestP) {bestP = p; best = end;}
|
||
// drawer.add(end, p);
|
||
|
||
}
|
||
|
||
}
|
||
|
||
#elif (MODE == 3)
|
||
|
||
using Reachable = ReachableByDepthWithDistanceSorted<Node>;
|
||
using ReachableNode = typename Reachable::Entry;
|
||
Reachable reach(grid);
|
||
std::vector<ReachableNode> reachableNodes = reach.get(*startNode, depth);
|
||
|
||
using Sampler = ReachableSamplerByDepth<Node>;
|
||
using SamplerResult = typename Sampler::SampleResult;
|
||
Sampler sampler(grid, reachableNodes);
|
||
|
||
for (int i = 0; i < 1500; ++i) {
|
||
|
||
const SamplerResult sample = sampler.sample();
|
||
|
||
double p = 1;
|
||
for (const WalkEvaluator<Node>* eval : evals) {
|
||
const double p1 = eval->getProbability(start, sample.pos, sample.walkDistToStart_m*0.94, params);
|
||
p *= p1;
|
||
}
|
||
|
||
if (p > bestP) {bestP = p; best = sample.pos;}
|
||
|
||
}
|
||
|
||
#endif
|
||
|
||
//std::cout << numVisitedNodes << std::endl;
|
||
|
||
//double drawProb = 0; const Point3 end = drawer.get(drawProb);
|
||
const Point3 end = best;
|
||
WalkResult res;
|
||
if (start == end) {
|
||
res.probability = 0;
|
||
} else {
|
||
res.heading = Heading(start.xy(), end.xy());
|
||
//res.probability = drawProb; // when using DrawList
|
||
res.probability = bestP; // when using bestP
|
||
}
|
||
res.position = end;
|
||
return res;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
|
||
|
||
|
||
//const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
|
||
|
||
|
||
|
||
|
||
// Distribution::Normal<float> dDist(1, 0.02);
|
||
// Distribution::Normal<float> dHead(0, 0.01);
|
||
|
||
// int cnt = 0;
|
||
// while(true) {
|
||
|
||
// const Point2 dir = res.heading.asVector();
|
||
// const Point2 dst = params.start.xy() + (dir * realDist_m);
|
||
|
||
// // is dst reachable?
|
||
// const Node* n = Helper::contains(grid, reachableNodes, 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
|
||
// return res; // done
|
||
// } else {
|
||
// std::cout << "WARN dst not found" << std::endl;
|
||
// //throw "should not happen";
|
||
// }
|
||
|
||
// }
|
||
|
||
// // reduce probability with every new run
|
||
// ++cnt;
|
||
// res.probability /= 2;
|
||
|
||
// // before trying again, modify distance and angle
|
||
// //if (1 == 1) {
|
||
// res.heading = params.heading + dHead.draw() * cnt;
|
||
// realDist_m = params.distance_m * dDist.draw();// * cnt;
|
||
// //}
|
||
|
||
// // reached max retries?
|
||
// if (cnt > 8) {
|
||
// res.position = params.start; // reset
|
||
// res.heading = params.heading; // reset
|
||
// res.probability = 1e-50; // unlikely
|
||
// return res;
|
||
// } // did not work out....
|
||
|
||
// }
|
||
|
||
// 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();
|
||
// if (nStart == nEnd) {
|
||
// std::cout << "warn! start == end" << std::endl;
|
||
// return 0;
|
||
// }
|
||
// Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
|
||
// const float diff = head.getDiffHalfRAD(desiredHeading);
|
||
// return Distribution::Normal<double>::getProbability(0, 0.3, diff);
|
||
// }
|
||
|
||
// double evalHeadingChanges(const Walk& w) const {
|
||
// Stats::Variance<float> var;
|
||
// for (int i = 0; i < w.size()-2; ++i) {
|
||
// const Node* n0 = w[i+0];
|
||
// const Node* n1 = w[i+1];
|
||
// const Node* n2 = w[i+2];
|
||
// Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
|
||
// Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
|
||
// const float diff = h1.getDiffHalfRAD(h2);
|
||
// var.add(diff);
|
||
// }
|
||
// const float totalVar = var.get();
|
||
// return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
|
||
// }
|
||
|
||
// double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
|
||
|
||
// return 1.0
|
||
// * evalHeadingStartEnd(w, desiredHeading)
|
||
// * evalDistance(w, desiredDistance)
|
||
// // * evalHeadingChanges(w);
|
||
// ;
|
||
|
||
// }
|
||
|
||
|
||
|
||
|
||
// Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
|
||
|
||
// Walk walk;
|
||
|
||
// float dist = 0;
|
||
|
||
// const Node* cur = start;
|
||
// while(true) {
|
||
|
||
// 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;
|
||
|
||
// }
|
||
|
||
}
|
||
|
||
#endif // INDOOR_GW3_WALKER_H
|