377 lines
10 KiB
C++
377 lines
10 KiB
C++
#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"
|
|
|
|
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(Grid<Node>& grid, const WalkParams& params) const = 0;
|
|
|
|
};
|
|
|
|
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
|
|
|
|
//RandomGenerator rnd;
|
|
|
|
std::vector<WalkEvaluator<Node>*> evals;
|
|
|
|
public:
|
|
|
|
/** 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(Grid<Node>& grid, const WalkParams& params) const override {
|
|
|
|
static std::mt19937 rndGen;
|
|
|
|
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
|
const Node* startNode = grid.getNodePtrFor(gpStart);
|
|
|
|
// 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 reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
|
|
|
WalkResult res;
|
|
res.heading = params.heading;
|
|
res.position = params.start;
|
|
float realDist_m = params.distance_m;
|
|
|
|
|
|
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";
|
|
}
|
|
|
|
}
|
|
|
|
// 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;
|
|
|
|
double p = 1;
|
|
for (const WalkEvaluator<Node>* eval : evals) {
|
|
const double p1 = eval->getProbability(start, end, params);
|
|
p *= p1;
|
|
}
|
|
|
|
if (start == end) {
|
|
res.probability = 0;
|
|
return res;
|
|
}
|
|
|
|
res.heading = Heading(start.xy(), end.xy());
|
|
res.probability = p;
|
|
res.position = end;
|
|
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;
|
|
|
|
public:
|
|
|
|
/** 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(Grid<Node>& grid, const WalkParams& params) const override {
|
|
|
|
static std::minstd_rand rndGen;
|
|
|
|
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
|
const Node* startNode = grid.getNodePtrFor(gpStart);
|
|
|
|
// 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 reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
|
|
|
// 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);
|
|
|
|
|
|
Point3 best;
|
|
double bestP = 0;
|
|
|
|
const Point3 start = params.start;
|
|
|
|
// try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles")
|
|
for (int i = 0; i < 25; ++i) {
|
|
|
|
const Node* dstNode = reachableNodes[dNode(rndGen)];
|
|
|
|
// random position within destination-node
|
|
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f);
|
|
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
|
|
|
|
// destination = node-center + offset (within the node's bbox)
|
|
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
|
|
|
|
// sanity check
|
|
Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
|
|
|
|
if (start == end) {continue;}
|
|
|
|
double p = 1;
|
|
for (const WalkEvaluator<Node>* eval : evals) {
|
|
const double p1 = eval->getProbability(start, end, params);
|
|
p *= p1;
|
|
}
|
|
|
|
if (p > bestP) {bestP = p; best = end;}
|
|
|
|
}
|
|
|
|
//const Point3 end = lst.get();
|
|
const Point3 end = best;
|
|
WalkResult res;
|
|
if (start == end) {
|
|
res.probability = 0;
|
|
} else {
|
|
res.heading = Heading(start.xy(), end.xy());
|
|
res.probability = bestP; // 1
|
|
}
|
|
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
|