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

new test cases

Browse Source 
worked on all walkers
new helper methods
new distributions
some bugfixes
This commit is contained in:
FrankE
2016-02-02 21:43:15 +01:00
parent ec86b07c43
commit 2e2c1a3004
18 changed files with 363 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
grid/Grid.h
View File

@@ -235,6 +235,11 @@ public:
remove(nodes[idx]);
}
/**
* mark the given node for deletion
* see: cleanup()
*/
void remove(T& node) {
// disconnect from all neighbors
@@ -250,8 +255,11 @@ public:
}
/** remove all nodes marked for deletion */
void cleanup() {
debugMemoryUsage();
Log::add(name, "running grid cleanup", false);
Log::tick();
@@ -293,6 +301,8 @@ public:
rebuildHashes();
debugMemoryUsage();
}
/** rebuild the UID-hash-list */
@@ -310,6 +320,18 @@ public:
}
/** debug-print the grid's current memory usage */
void debugMemoryUsage() {
const uint32_t bytes = nodes.size() * sizeof(T);
const uint32_t numNodes = nodes.size();
uint32_t numNeighbors = 0;
//uint32_t numNeighborsUnused = 0;
for (T& n : nodes) {
numNeighbors += n._numNeighbors;
}
Log::add(name, "memory: " + std::to_string(bytes/1024.0f/1024.0f) + " MB in " + std::to_string(numNodes) + " nodes");
}
// /**
// * remove all nodes, marked for deletion.
// * BEWARE: this will invalidate all indices used externally!

3
grid/factory/GridFactory.h
View File

@@ -198,7 +198,8 @@ public:
/** build a stair (z-transition) from n1 to n2 */
void buildStairLine(T& _n1, T& _n2) {
const int gridSize_cm = grid.getGridSize_cm();
// half the grid size = small steps
const int gridSize_cm = grid.getGridSize_cm() / 2;
// local copies, needed for std::swap to work
T n1 = _n1; T n2 = _n2;

4
grid/factory/GridImportance.h
View File

@@ -197,7 +197,7 @@ public:
static Distribution::Normal<float> avoidWalls(0.0, 0.4);
// favour walking near walls (likely)
static Distribution::Normal<float> sticToWalls(0.9, 0.5);
static Distribution::Normal<float> stickToWalls(0.9, 0.5);
// favour walking far away (likely)
static Distribution::Normal<float> farAway(2.2, 0.5);
@@ -205,7 +205,7 @@ public:
// overall importance
return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
+ sticToWalls.getProbability(dist_m) * 0.15 // walk near walls
+ stickToWalls.getProbability(dist_m) * 0.15 // walk near walls
+ farAway.getProbability(dist_m) * 0.15 // walk in the middle
;

2
grid/walk/GridWalk.h
View File

@@ -8,7 +8,7 @@ template <typename T> class GridWalk {
public:
virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) = 0;
virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) = 0;
};

15
grid/walk/GridWalkHelper.h
View File

@@ -22,7 +22,13 @@ public:
const float d1 = h.getDiffHalfRAD(h1);
const float d2 = h.getDiffHalfRAD(h2);
//return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2));
return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); // same heading? > random decision
//return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); // same heading? > random decision
//return ((from.z_cm != n1.z_cm) && (from.z_cm == n2.z_cm)) ||
// (d1 < d2) ||
//(rand() < RAND_MAX/2);
if (d1 < d2) {return true;}
else if (d1 == d2) {return n1.z_cm != n2.z_cm;}
return false;
};
auto neighbors = grid.neighbors(from);
@@ -39,7 +45,7 @@ public:
* if this also fails
* - add some randomness and try again
*/
template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m) {
template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m, const float headChange_rad) {
Assert::isTrue(distance_m >= 0, "distance must not be negative!");
Assert::isNotNull(start.node, "starting node must not be null");
@@ -53,13 +59,14 @@ public:
// try to walk the given distance from the start
// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
do {
res = w.walk(grid, start, distance_m);
res = w.walk(grid, start, distance_m, headChange_rad);
} while (res.node == nullptr && --retries);
// still reaching a dead end?
// -> try a walk in the opposite direction instead
if (res.node == nullptr) {
res = w.walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
GridWalkState<T> newState(start.node, start.heading.getInverted());
res = w.walk(grid, newState, distance_m, headChange_rad);
}
// still nothing found? -> modify and try again

25
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
View File

@@ -63,15 +63,15 @@ public:
}
GridWalkState<T> getDestination(Grid<T>& grid, GridWalkState<T> start, float distance_m) override {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad);
}
private:
GridWalkState<T> walk(Grid<T>& grid, GridWalkState<T> cur, float distRest_m) {
GridWalkState<T> walk(Grid<T>& grid, GridWalkState<T>& cur, float distRest_m, const float headChange_rad) {
drawer.reset();
@@ -105,7 +105,10 @@ private:
}
GridWalkState<T> next(nullptr, cur.heading);
//GridWalkState<T> next(nullptr, cur.heading);
// create a copy (to keep heading and walked distance)
GridWalkState<T> next = cur;
// pick a random destination
T& nDir = drawer.get();
@@ -116,6 +119,7 @@ private:
next.heading += headingChangeDist(gen);
next.node = &nDir;
//// // compare two neighbors according to their implied heading change
//// auto compp = [&] (const T& n1, const T& n2) {
//// Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
@@ -145,13 +149,22 @@ private:
//// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
const float walked_m = next.node->getDistanceInMeter(*cur.node);
distRest_m -= walked_m;
// update the heading-change and walked-distance
next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD();
next.distanceWalked_m += walked_m;
if (next.node->z_cm != cur.node->z_cm) {
int i = 0;
}
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
return walk(grid, next, distRest_m, headChange_rad);
}

14
grid/walk/GridWalkPushForward.h
View File

@@ -43,7 +43,7 @@ public:
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) override {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
@@ -54,7 +54,7 @@ private:
// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
DrawList<T*> drawer;
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& cur, float distRest_m) {
drawer.reset();
@@ -93,7 +93,8 @@ private:
// all neighbors are unlikely? -> start over
if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
GridWalkState<T> next;
// create a copy (to keep heading and walked distance)
GridWalkState<T> next = cur;
// pick the neighbor best matching this new heading
next.node = drawer.get();
@@ -103,7 +104,12 @@ private:
next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1);
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
const float walked_m = next.node->getDistanceInMeter(*cur.node);
distRest_m -= walked_m;
// update the heading-change and walked-distance
next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD();
next.distanceWalked_m += walked_m;
// done?
if (distRest_m <= 0) {return next;}

26
grid/walk/GridWalkRandomHeadingUpdate.h
View File

@@ -45,20 +45,27 @@ public:
gen.seed(1234);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1);
}
private:
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& cur, float distRest_m, float headChange = -1) {
GridWalkState<T> next;
// create a copy (to keep heading and walked distance)
GridWalkState<T> next = cur;
// change the heading at the beginning of each walk
if (headChange == -1) {headChange = headingChangeDist(gen);}
// get a new random heading
next.heading = cur.heading + headingChangeDist(gen);
next.heading = cur.heading + headChange;
// pick the neighbor best matching this new heading
next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
@@ -71,13 +78,18 @@ private:
}
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
const float walked_m = next.node->getDistanceInMeter(*cur.node);
distRest_m -= walked_m;
// update the heading-change and walked-distance
next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD();
next.distanceWalked_m += walked_m;
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
return walk(grid, next, distRest_m, headChange);
}

26
grid/walk/GridWalkRandomHeadingUpdateAdv.h
View File

@@ -46,7 +46,7 @@ public:
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) override {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
@@ -60,13 +60,18 @@ private:
// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
DrawList<T*> drawer;
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& cur, float distRest_m, float headChange = -1) {
drawer.reset();
GridWalkState<T> next;
// create a copy (to keep heading and walked distance)
GridWalkState<T> next = cur;
// change the heading at the beginning of each walk
if (headChange == -1) {headChange = headingChangeDist(gen);}
// get a new random heading
next.heading = cur.heading + headingChangeDist(gen);
next.heading = cur.heading + headChange;
// weight all neighbors based on this heading
for (T& neighbor : grid.neighbors(*cur.node)) {
@@ -81,8 +86,8 @@ private:
const float prob1 = Distribution::Normal<float>::getProbability(0, Angle::degToRad(40), diffRad);
// add the node's importance factor into the calculation
const float prob2 = Distribution::Logistic<float>::getCDF(neighbor.imp, 1.0, 0.05);
//const float prob2 = std::pow(neighbor.imp, 10);
//const float prob2 = Distribution::Logistic<float>::getCDF(neighbor.imp, 1.0, 0.05);
const float prob2 = std::pow(neighbor.imp, 10);
// final importance
const float prob = prob1 * prob2;
@@ -106,13 +111,18 @@ private:
// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
const float walked_m = next.node->getDistanceInMeter(*cur.node);
distRest_m -= walked_m;
// update the heading-change and walked-distance
next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD();
next.distanceWalked_m += walked_m;
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
return walk(grid, next, distRest_m, headChange);
}

141
grid/walk/GridWalkSimpleControl.h Normal file
View File

@@ -0,0 +1,141 @@
#ifndef GRIDWALKSIMPLECONTROL_H
#define GRIDWALKSIMPLECONTROL_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/Distributions.h"
#include "../../math/DrawList.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
#include "GridWalk.h"
template <typename T> class GridWalkSimpleControl : public GridWalk<T> {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
DrawList<T&> drawer;
public:
/** ctor */
GridWalkSimpleControl() {
gen.seed(1234);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
// proportional change of the heading
static Distribution::Normal<float> dHead(1, 0.10);
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw(); // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
return walk(grid, start, distance_m, headChange_rad);
}
private:
double getProbability(const T& start, const T& possible, const Heading head) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {return 0.1;}
// get the angle between START and the possible next node
const Heading possibleHead = GridWalkHelper::getHeading(start, possible);
// calculate the difference
const float diff = possibleHead.getDiffHalfRAD(head);
// // compare this heading with the requested one
const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(30), diff);
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance
//const double nodeProb = Distribution::Logistic<float>::getCDF(possible.imp, 1, 0.5);
const double nodeProb = std::pow(possible.imp, 5);
//const double nodeProb = 1.0;
// bring it together
return angleProb * nodeProb;
}
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) {
// try-again distribution
//static Distribution::Normal<float> dHead(0, Angle::degToRad(10));
//static Distribution::Normal<float> dUpdate(0, Angle::degToRad(3));
static Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));
int retries = 5;
float walked_m = 0;
GridWalkState<T> cur = start;
// the desired heading
Heading reqHeading = start.heading + (headChange_rad);
// walk until done
while(walked_m < distance_m) {
// evaluate all neighbors
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, neighbor, reqHeading);
drawer.add(neighbor, prob);
}
// too bad? start over!
if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) {
walked_m = 0;
cur = start;
WHAT THE HELL
if (retries == 0) { reqHeading = dChange.draw(); }
continue;
}
// get the neighbor
const T& neighbor = drawer.get();
// update
walked_m += neighbor.getDistanceInMeter(*cur.node);
cur.node = &neighbor;
}
cur.distanceWalked_m = NAN;
cur.headingChange_rad = NAN;
cur.heading = reqHeading;
return cur;
}
};
#endif // GRIDWALKSIMPLECONTROL_H

10
grid/walk/GridWalkState.h
View File

@@ -12,13 +12,19 @@ template <typename T> struct GridWalkState {
/** the current heading */
Heading heading;
/** cumulative heading change */
float headingChange_rad;
/** cumulative distance change */
float distanceWalked_m;
/** empty ctor */
GridWalkState() : node(nullptr), heading(0) {;}
GridWalkState() : node(nullptr), heading(0), headingChange_rad(0), distanceWalked_m(0) {;}
Point3 avg = Point3(0,0,0);
/** ctor with user-node and heading */
GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;}
GridWalkState(const T* node, const Heading heading) : node(node), heading(heading), headingChange_rad(0), distanceWalked_m(0) {;}
};
#endif // GRIDWALKSTATE_H