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

started testing a new grid-builder

Browse Source 
minor fixes
worked on walkers
This commit is contained in:
k-a-z-u
2017-12-20 17:12:30 +01:00
parent d48b0b8fd4
commit c346b7f222
7 changed files with 709 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
grid/walk/v3/Helper.h
View File

@@ -174,10 +174,11 @@ namespace GW3 {
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 GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
// const Point3 p100 = p*100;
// //return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
// return GridPoint( grid.snapX(p100.x), grid.snapY(p100.y), grid.snapZ(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);

39
grid/walk/v3/Structs.h
View File

@@ -4,18 +4,55 @@
#include "../../../geo/Heading.h"
#include "../../../geo/Point3.h"
#include <vector>
#include "../../../math/Distributions.h"
#include "../../../grid/Grid.h"
namespace GW3 {
struct StepSizes {
float stepSizeFloor_m = NAN;
float stepSizeStair_m = NAN;
bool isValid() const {
return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
}
template <typename Node> float inMeter(const int steps, const Point3 start, const Grid<Node>& grid) const {
Assert::isTrue(isValid(), "invalid step-sizes given");
const GridPoint gp = grid.toGridPoint(start);
const Node& n = grid.getNodeFor(gp);
if (grid.isPlain(n)) {
return stepSizeFloor_m * steps;
} else {
return stepSizeStair_m * steps;
}
}
};
/** paremters for the walk */
struct WalkParams {
//Distribution::Normal<float> dDistFloor;
//Distribution::Normal<float> dDistStair;
Point3 start;
float distance_m;
//float distance_m;
int numSteps;
Heading heading = Heading(0);
float lookFurther_m = 1.5;
StepSizes stepSizes;
template <typename Node> float getDistanceInMeter(const Grid<Node>& grid) const {
return stepSizes.inMeter(numSteps, start, grid);
}
};
/** result of the random walk */

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

@@ -9,6 +9,29 @@
namespace GW3 {
/** describes a potential walk, which can be evaluated */
struct PotentialWalk {
/** initial parameters (requested walk) */
const WalkParams& params;
/** walk started here */
Point3 pStart;
/** walk ended here */
Point3 pEnd;
/** usually the euclidean distance start<->end but not necessarily! */
float walkDist_m;
/** ctor */
PotentialWalk(const WalkParams& params, const Point3 pStart, const Point3 pEnd, const float walkedDistance_m) :
params(params), pStart(pStart), pEnd(pEnd), walkDist_m(walkedDistance_m) {
;
}
};
/** interface for all evaluators that return a probability for a given walk */
template <typename Node> class WalkEvaluator {
@@ -17,7 +40,7 @@ namespace GW3 {
/** 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 float walkedDist_m, const WalkParams& params) const = 0;
virtual double getProbability(const PotentialWalk& walk) const = 0;
};
@@ -31,12 +54,9 @@ namespace GW3 {
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
(void) params;
(void) pStart;
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
const Node& node = grid->getNodeFor(gp);
const double p = node.getWalkImportance();
return p;
@@ -65,17 +85,15 @@ namespace GW3 {
;
}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
(void) params;
if (pStart == pEnd) {
if (walk.pStart == walk.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 Heading head(walk.pStart.xy(), walk.pEnd.xy());
const float diff = head.getDiffHalfRAD(walk.params.heading);
//const float diff = Heading::getSignedDiff(params.heading, head);
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
@@ -87,18 +105,20 @@ namespace GW3 {
/** evaluate the difference between distance(start, end) and the requested distance */
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
const Grid<Node>& grid;
const double sigma;
const Distribution::Normal<double> dist;
public:
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
const float requestedDistance_m = params.distance_m;
const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd);
const float requestedDistance_m = walk.params.getDistanceInMeter(grid);
const float walkedDistance_m = walk.walkDist_m;//pStart.getDistance(pEnd);
const float diff = walkedDistance_m - requestedDistance_m;
return dist.getProbability(diff);
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);

57
grid/walk/v3/Walker.h
View File

@@ -58,18 +58,22 @@ namespace GW3 {
/** perform the walk based on the configured setup */
const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.distance_m, "walking distance must be > 0");
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 = Helper::p3ToGp(params.start);
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(params.start, dst, params.start.getDistance(dst), params);
const double p1 = eval->getProbability(pWalk);
p *= p1;
}
return p;
@@ -97,7 +101,7 @@ namespace GW3 {
return (startNode->getDistanceInMeter(n)) < maxDist_m;
}
};
Cond cond(params.distance_m+secBuffer_m, startNode);
Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
std::vector<const Node*> reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
WalkResult res;
@@ -106,7 +110,7 @@ namespace GW3 {
// 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.distance_m);
const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid));
// is above destination reachable?
const Node* n = Helper::contains(grid, reachableNodes, dst);
@@ -114,14 +118,14 @@ namespace GW3 {
if (n) {
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = Helper::p3ToGp(p3);
const GridPoint gp = grid.toGridPoint(p3);
if (grid.hasNodeFor(gp)) {
res.position = p3; // update position
//res.heading; // keep as-is
res.probability *= 1;//getP(p3); // keep as-is
res.probability *= getP(p3); // keep as-is
return res; // done
} else {
@@ -145,9 +149,11 @@ namespace GW3 {
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(start, end, start.getDistance(end), params);
const double p1 = eval->getProbability(pWalk);
p *= p1;
}
@@ -157,8 +163,29 @@ namespace GW3 {
}
res.heading = Heading(start.xy(), end.xy());
res.probability *= 0.1;//getP(end);
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;
}
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
return res;
}
@@ -204,14 +231,16 @@ namespace GW3 {
/** perform the walk based on the configured setup */
const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.distance_m, "walking distance must be > 0");
const float walkDist_m = params.getDistanceInMeter(grid);
Assert::isNot0(walkDist_m, "walking distance must be > 0");
const GridPoint gpStart = Helper::p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
if (!startNode) {throw Exception("start node not found!");}
const float maxDist = params.distance_m + gridSize_m;
const int depth = std::ceil(params.distance_m / gridSize_m) + 1;
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;
@@ -331,9 +360,11 @@ namespace GW3 {
//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(start, end, walkDist_m, params);
const double p1 = eval->getProbability(pWalk);
p *= p1;
}