Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor
This commit is contained in:
toni
@@ -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
|
||||
@@ -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
133
grid/walk/v3/Helper.h
Normal 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
43
grid/walk/v3/Structs.h
Normal 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
91
grid/walk/v3/WalkEvaluator.h
Normal 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
20
grid/walk/v3/WalkGenerator.h
Normal 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
376
grid/walk/v3/Walker.h
Normal 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
|
||||
Reference in New Issue
Block a user