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

18
geo/Angle.h
View File

@@ -3,6 +3,7 @@
#include <cmath>
#include "../Assertions.h"
#include "Point2.h"
struct Angle {
@@ -16,11 +17,17 @@ public:
return (tmp < 0) ? (tmp + 2*M_PI) : (tmp);
}
/** get the radians from (0,0) to (p.x,p.y) between 0 (to-the-right) and <2_PI */
static float getRAD_2PI(const Point2& p) {
return getRAD_2PI(0, 0, p.x, p.y);
}
/** get the degrees from (x1,y1) to (x2,y2) between 0 (to-the-right) and <360 */
static float getDEG_360(const float x1, const float y1, const float x2, const float y2) {
return radToDeg(getRAD_2PI(x1,y1,x2,y2));
}
/**
* gets the angular difference between
* - the given radians [0:2PI]
@@ -35,12 +42,19 @@ public:
/** convert degrees to radians */
static constexpr float degToRad(const float deg) {
return deg / 180 * M_PI;
return deg / 180.0f * M_PI;
}
/** convert radians to degrees */
static float radToDeg(const float rad) {
return rad * 180 / M_PI;
return rad * 180.0f / M_PI;
}
/** get a pointer vector (length 1) pointing to the given angle (in radians) */
static Point2 getPointer(const float rad) {
const float x = cos(rad);
const float y = sin(rad);
return Point2(x,y);
}
};

9
geo/Heading.h
View File

@@ -46,6 +46,10 @@ public:
return (Heading(*this) += _rad);
}
/** compare two headings */
bool operator == (const Heading other) const {return rad == other.rad;}
bool operator != (const Heading other) const {return rad != other.rad;}
/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
Heading getInverted() const {
Heading out(rad);
@@ -57,9 +61,10 @@ public:
/** get a random heading */
static Heading rnd() {
static std::minstd_rand gen; gen.seed(1234);
static std::minstd_rand gen(1234);
static std::uniform_real_distribution<float> dist(0, _2PI);
return Heading(dist(gen));
const float rnd = dist(gen);
return Heading(rnd);
}
#undef _2PI

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

1
math/Distributions.h
View File

@@ -4,5 +4,6 @@
#include "distribution/Normal.h"
#include "distribution/Exponential.h"
#include "distribution/Logistic.h"
#include "distribution/Uniform.h"
#endif // DISTRIBUTIONS_H

38
math/distribution/Uniform.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef UNIFORM_H
#define UNIFORM_H
#include <cmath>
#include <random>
namespace Distribution {
/** uniform distribution */
template <typename T> class Uniform {
private:
std::minstd_rand gen;
std::uniform_real_distribution<T> dist;
public:
/** ctor */
Uniform(const T min, const T max) :
gen(1234), dist(min, max) {
}
/** get a uniformaly distributed random number */
T draw() {
return dist(gen);
}
/** set the seed to use */
void setSeed(const long seed) {
gen.seed(seed);
}
};
}
#endif // UNIFORM_H

10
tests/geo/TestAngle.cpp
View File

@@ -4,6 +4,16 @@
#include "../../geo/Angle.h"
TEST(Angle, dir) {
// angle -> pointer -> angle
ASSERT_NEAR(0, Angle::getRAD_2PI(Angle::getPointer(0)), 0.0001);
ASSERT_NEAR(1, Angle::getRAD_2PI(Angle::getPointer(1)), 0.0001);
ASSERT_NEAR(2, Angle::getRAD_2PI(Angle::getPointer(2)), 0.0001);
ASSERT_NEAR(3, Angle::getRAD_2PI(Angle::getPointer(3)), 0.0001);
}
TEST(Angle, calc) {
ASSERT_EQ(0, Angle::getDEG_360(0,0, +1,0)); // to the right

35
tests/geo/TestHeading.cpp
View File

@@ -26,4 +26,39 @@ TEST(Heading, diff) {
}
TEST(Heading, ctor) {
// OK
Heading(0);
Heading(1);
Heading(2);
Heading(3);
Heading(4);
Heading(5);
Heading(6);
Heading(2*M_PI-0.0001);
// out of range
ASSERT_THROW(Heading(-0.0001), std::exception);
ASSERT_THROW(Heading(2*M_PI+0.0001), std::exception);
}
TEST(Heading, eq) {
ASSERT_EQ(Heading(0), Heading(0));
ASSERT_EQ(Heading(1), Heading(1));
ASSERT_EQ(Heading(2), Heading(2));
}
TEST(Heading, random) {
// two random values must not be equal
ASSERT_NE(Heading::rnd(), Heading::rnd());
ASSERT_NE(Heading::rnd(), Heading::rnd());
ASSERT_NE(Heading::rnd(), Heading::rnd());
}
#endif

5
tests/grid/TestWalk.cpp
View File

@@ -97,7 +97,8 @@ TEST(Walk, DISABLED_plot) {
GridWalkLightAtTheEndOfTheTunnel<GP> walk(g, tmp, *end);
std::minstd_rand gen;
std::normal_distribution<float> dist(0.3, 0.3);
std::normal_distribution<float> dWalk(0.3, 0.3);
std::normal_distribution<float> dTurn(0.3, 0.3);
K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000");
@@ -129,7 +130,7 @@ TEST(Walk, DISABLED_plot) {
for (int i = 0; i < 5000; ++i) {
pStates.clear();
for (GridWalkState<GP>& state : states) {
state = walk.getDestination(g, state, std::abs(dist(gen)));
state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen));
pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10));
}
p.gp.draw(p.splot);