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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
toni
2017-10-24 18:42:33 +02:00
parent 5d6d62c2d2 feaa2ea12c
commit 1c0e680b9d
8 changed files with 668 additions and 370 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
geo/Heading.h
View File

@@ -27,6 +27,11 @@ public:
Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
}
/** ctor from(x,y) -> to(x,y) */
Heading(const Point2 from, const Point2 to) : Heading(from.x, from.y, to.x, to.y) {
;
}
/** POSITIVE angular difference [0:PI] */
float getDiffHalfRAD(const Heading other) const {
return Angle::getDiffRAD_2PI_PI(rad, other.rad);

113
grid/walk/v3/GridWalk3Helper.h
View File

@@ -1,113 +0,0 @@
#ifndef GRIDWALK3HELPER_H
#define GRIDWALK3HELPER_H
#include "../../../nav/dijkstra/Dijkstra.h"
#include <set>
template <typename Node> class GridWalk3Helper {
public:
/** one walk along several nodes */
struct Walk3 : public std::vector<const Node*> {
};
struct Walks3 : std::vector<Walk3> {
};
struct Nodes3 : std::vector<const Node*> {
};
/** get all possible walks from start within a given region */
static Walks3 getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
struct Access {
Grid<Node>& grid;
Access(Grid<Node>& grid) : grid(grid) {;}
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
} acc(grid);
const float addDist_m = grid.getGridSize_cm() / 100.0f;
const float maxDist_m = dist_m * 1.1 + addDist_m;
Dijkstra<Node> dijkstra;
dijkstra.build(start, nullptr, acc, maxDist_m);
const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
Walks3 walks;
for (const auto& it : nodes) {
Walk3 walk;
DijkstraNode<Node>* node = it.second;
do {
const Node* gridNode = node->element;
walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted
node = node->previous;
} while (node);
walks.push_back(walk);
}
return walks;
}
/** get all reachable nodes that are within a given range */
static Nodes3 getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
Nodes3 res;
std::unordered_map<uint32_t, float> distances;
std::vector<uint32_t> toVisit;
toVisit.push_back(start->getIdx());
distances[start->getIdx()] = 0.0f;
while (!toVisit.empty()) {
int curIdx = toVisit.front();
toVisit.erase(toVisit.begin());
const Node& curNode = grid[curIdx];
const float curDistance = distances[curIdx];
res.push_back(&curNode); // remember for output
if (curDistance <= dist_m) {
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
const int neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
const float totalDist = curDistance + addDist;
// this is like in dijkstra. keep the smallest distance to reach a node:
// not yet reached -> store distance
if (distances.find(neighborIdx) == distances.end()) {
toVisit.push_back(neighborIdx);
distances[neighborIdx] = totalDist;
// reached earlier but found shorter way
} else {
if (distances[neighborIdx] > totalDist) {
distances[neighborIdx] = totalDist;
}
}
}
}
}
return res;
}
};
#endif // GRIDWALK3HELPER_H

257
grid/walk/v3/GridWalker3.h
View File

@@ -1,257 +0,0 @@
#ifndef GRIDWALKER3_H
#define GRIDWALKER3_H
#include "../../../data/Timestamp.h"
#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "../../../math/Distributions.h"
#include "../../../math/Stats.h"
#include "../../../geo/Heading.h"
#include "../../../math/stats/Variance.h"
#include "GridWalk3Helper.h"
#include "../../../geo/BBox2.h"
/**
* modular grid-walker that takes various sub-components to determine
* p(e) and thus randomly pick edges
*/
template <typename Node> class GridWalker3 {
private:
/** all modules to evaluate */
// std::vector<WalkModule<Node, WalkState>*> modules;
RandomGenerator rnd;
public:
/** paremters for the walk */
struct WalkParams {
Point3 start;
float distance_m;
Heading heading = Heading(0);
};
/** result of the random walk */
struct WalkResult {
Point3 position;
Heading heading = Heading(0);
double probability = 1.0;
};
using Helper = GridWalk3Helper<Node>;
using Walk = typename GridWalk3Helper<Node>::Walk3;
using Walks = typename GridWalk3Helper<Node>::Walks3;
using Nodes = typename GridWalk3Helper<Node>::Nodes3;
GridPoint p3ToGp(const Point3 p) const {
const Point3 p100 = p*100;
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
}
Point3 gpToP3(const GridPoint gp) const {
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
}
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) {
//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
return _drawThenCheck(grid, params);
}
// /** perform the walk based on the configured setup */
// const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {
// //return _getFromPossibleWalks(grid, start, ctrl, dist_m);
// //return _drawThenCheck(grid, start, ctrl, dist_m);
// throw "error";
// }
/** does the given grid-node contain the provided point-in-question? */
const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
const float d = gridSize_m / 2.0f;
const Point2 pMin = n->inMeter().xy() - Point2(d, d);
const Point2 pMax = n->inMeter().xy() + Point2(d, d);
const BBox2 bbox(pMin, pMax);
return bbox.contains(pt);
}
/** does one of the given grid-node scontains the provided point-in-question? */
const Node* contains(const Grid<Node>& grid, const Nodes& nodes, Point2 pt) {
for (const Node* n : nodes) {
if (contains(grid, n, pt)) {return n;}
}
return nullptr;
}
const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
const GridPoint gpStart = p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
static Distribution::Normal<float> dDist(1, 0.02);
static Distribution::Normal<float> dHead(0, 0.01);
// 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;
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 = contains(grid, reachableNodes, dst);
if (n) {
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = 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 // GRIDWALKER3_H

133
grid/walk/v3/Helper.h Normal file
View File

@@ -0,0 +1,133 @@
#ifndef INDOOR_GW3_HELPER_H
#define INDOOR_GW3_HELPER_H
#include "../../../nav/dijkstra/Dijkstra.h"
#include "../../Grid.h"
#include "Structs.h"
#include <set>
namespace GW3 {
template <typename Node> class Helper {
public:
static GridPoint p3ToGp(const Point3 p) {
const Point3 p100 = p*100;
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
}
static Point3 gpToP3(const GridPoint gp) {
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
}
/** does the given grid-node contain the provided point-in-question? */
static bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
const float d = gridSize_m / 2.0f;
const Point2 pMin = n->inMeter().xy() - Point2(d, d);
const Point2 pMax = n->inMeter().xy() + Point2(d, d);
const BBox2 bbox(pMin, pMax);
return bbox.contains(pt);
}
/** does one of the given grid-nodes contains the provided point-in-question? */
static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
for (const Node* n : nodes) {
if (contains(grid, n, pt)) {return n;}
}
return nullptr;
}
/** get all possible walks from start within a given region */
static Walks<Node> getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
struct Access {
Grid<Node>& grid;
Access(Grid<Node>& grid) : grid(grid) {;}
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
} acc(grid);
const float addDist_m = grid.getGridSize_cm() / 100.0f;
const float maxDist_m = dist_m * 1.1 + addDist_m;
Dijkstra<Node> dijkstra;
dijkstra.build(start, nullptr, acc, maxDist_m);
const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
Walks<Node> walks;
for (const auto& it : nodes) {
Walk<Node> walk;
DijkstraNode<Node>* node = it.second;
do {
const Node* gridNode = node->element;
walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted
node = node->previous;
} while (node);
walks.push_back(walk);
}
return walks;
}
/** get all reachable nodes that are within a given range */
static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
Nodes<Node> res;
std::unordered_map<uint32_t, float> distances;
std::vector<uint32_t> toVisit;
toVisit.push_back(start->getIdx());
distances[start->getIdx()] = 0.0f;
while (!toVisit.empty()) {
int curIdx = toVisit.front();
toVisit.erase(toVisit.begin());
const Node& curNode = grid[curIdx];
const float curDistance = distances[curIdx];
res.push_back(&curNode); // remember for output
if (curDistance <= dist_m) {
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
const int neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
const float totalDist = curDistance + addDist;
// this is like in dijkstra. keep the smallest distance to reach a node:
// not yet reached -> store distance
if (distances.find(neighborIdx) == distances.end()) {
toVisit.push_back(neighborIdx);
distances[neighborIdx] = totalDist;
// reached earlier but found shorter way
} else {
if (distances[neighborIdx] > totalDist) {
distances[neighborIdx] = totalDist;
}
}
}
}
}
return res;
}
};
}
#endif // INDOOR_GW3_HELPER_H

43
grid/walk/v3/Structs.h Normal file
View File

@@ -0,0 +1,43 @@
#ifndef INDOOR_GW3_STRUCTS_H
#define INDOOR_GW3_STRUCTS_H
#include "../../../geo/Heading.h"
#include "../../../geo/Point3.h"
#include <vector>
namespace GW3 {
/** paremters for the walk */
struct WalkParams {
Point3 start;
float distance_m;
Heading heading = Heading(0);
};
/** result of the random walk */
struct WalkResult {
Point3 position;
Heading heading = Heading(0);
double probability = 1.0;
};
/** several nodes */
template <typename Node> struct Nodes : public std::vector<const Node*> {
};
/** one walk along several nodes */
template <typename Node> struct Walk : public std::vector<const Node*> {
};
/** several walks */
template <typename Node> struct Walks : public std::vector<Walk<Node>> {
};
}
#endif // INDOOR_GW3_STRUCTS_H

91
grid/walk/v3/WalkEvaluator.h Normal file
View File

@@ -0,0 +1,91 @@
#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 {
/** 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 Point3 pStart, const Point3 pEnd, const WalkParams& params) 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 Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
(void) params;
(void) pStart;
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
const Node& node = grid->getNodeFor(gp);
const double p = node.getWalkImportance();
return std::pow(p,10);
}
};
/** evaluate the difference between head(start,end) and the requested heading */
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
public:
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
(void) params;
if (pStart == 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 float diff = Heading::getSignedDiff(params.heading, head);
return Distribution::Normal<double>::getProbability(0, 0.04, diff);
}
};
/** evaluate the difference between distance(start, end) and the requested distance */
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
const double sigma = 0.1f;
public:
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
const float walkedDistance_m = pStart.getDistance(pEnd);
return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
}
};
}
#endif // INDOOR_GW3_WALKEVALUATOR_H

20
grid/walk/v3/WalkGenerator.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef INDOOR_GRIDWALKER3GENERATOR_H
#define INDOOR_GRIDWALKER3GENERATOR_H
#include "Structs.h"
namespace GW3 {
class GridWalker3Generator {
virtual Walk getPossibleWalks() = 0;
};
}
#endif // INDOOR_GRIDWALKER3GENERATOR_H

376
grid/walk/v3/Walker.h Normal file
View File

@@ -0,0 +1,376 @@
#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