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

added several grid-walks

Browse Source 
added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...
This commit is contained in:
FrankE
2016-01-24 18:59:06 +01:00
parent cdf97322f8
commit 9947dced15
30 changed files with 1406 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
grid/Grid.h
View File

@@ -15,7 +15,12 @@
/**
* grid of the given grid-size, storing some value which
* extends GridPoint
* extends GridPoint and GridNode
*
* Usage:
* for (Node& n : grid) {...}
* for (Node& n2 : grid.neighbors(n)) {...}
*
*/
template <int gridSize_cm, typename T> class Grid {
@@ -47,6 +52,12 @@ public:
/** no-assign */
void operator = (const Grid& o) = delete;
/** allows for-each iteration over all included nodes */
decltype(nodes.begin()) begin() {return nodes.begin();}
/** allows for-each iteration over all included nodes */
decltype(nodes.end()) end() {return nodes.end();}
/**
* add the given element to the grid.
@@ -235,14 +246,14 @@ public:
Log::add(name, "running grid cleanup");
// check every single node
for (int i = (int)nodes.size() - 1; i >= 0; --i) {
for (size_t i = 0; i < nodes.size(); ++i) {
// is this node marked as "deleted"? (idx == -1)
if (nodes[i]._idx == -1) {
// remove this node
deleteNode(i);
++i;
--i;
}
}
@@ -261,6 +272,8 @@ private:
/** hard-delete the given node */
void deleteNode(const int idx) {
_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
// remove him from the node list (reclaim its memory and its index)

8
grid/GridNeighborIterator.h
View File

@@ -8,10 +8,10 @@ class NeighborIter : std::iterator<std::input_iterator_tag, int> {
private:
/** the grid the src-node belongs to */
const Grid<gridSize_cm, T>& grid;
Grid<gridSize_cm, T>* grid;
/** index of the source-node within its grid */
const int srcNodeIdx;
int srcNodeIdx;
/** index of the current neighbor [0:10] */
int nIdx;
@@ -20,7 +20,7 @@ public:
/** ctor */
NeighborIter(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx, const int nIdx) :
grid(grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
grid((Grid<gridSize_cm, T>*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
/** next neighbor */
NeighborIter& operator++() {++nIdx; return *this;}
@@ -35,7 +35,7 @@ public:
bool operator!=(const NeighborIter& rhs) {return srcNodeIdx != rhs.srcNodeIdx || nIdx != rhs.nIdx;}
/** get the neighbor the iterator currently points to */
T& operator*() {return (T&) grid.getNeighbor(srcNodeIdx, nIdx);}
T& operator*() {return (T&) grid->getNeighbor(srcNodeIdx, nIdx);}
};

9
grid/GridPoint.h
View File

@@ -4,6 +4,8 @@
#include <cmath>
#include "../geo/Point3.h"
#include <KLib/Assertions.h>
struct GridPoint {
/** x-position (in centimeter) */
@@ -42,6 +44,13 @@ struct GridPoint {
/** cast to string */
operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
/** read-only array access */
float operator [] (const int idx) const {
_assertBetween(idx, 0, 2, "index out of bounds");
if (0 == idx) {return x_cm;}
if (1 == idx) {return y_cm;}
{return z_cm;}
}
};

4
grid/factory/GridFactory.h
View File

@@ -143,8 +143,8 @@ public:
int y = n1.y_cm + yDiff * percent;
// snap (x,y) to the grid???
//x = std::round(x / gridSize_cm) * gridSize_cm;
//y = std::round(y / gridSize_cm) * gridSize_cm;
x = std::round(x / gridSize_cm) * gridSize_cm;
y = std::round(y / gridSize_cm) * gridSize_cm;
// create a new node add it to the grid, and connect it with the previous one
idx2 = grid.addUnaligned(T(x,y,z));

49
grid/factory/GridImportance.h
View File

@@ -4,12 +4,18 @@
#include "../Grid.h"
#include "GridFactory.h"
#include "../../misc/KNN.h"
#include "../../misc/KNNArray.h"
#include "../../math/MiniMat2.h"
#include "../../misc/Debug.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../nav/dijkstra/DijkstraPath.h"
#include <KLib/math/distribution/Normal.h>
/**
* add an importance factor to each node within the grid.
* the importance is calculated based on several facts:
@@ -35,7 +41,7 @@ public:
fac.addInverted(g, z_cm);
// construct KNN search
KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
KNN<Grid<gridSize_cm, T>, 3> knn(inv);
// the number of neighbors to use
static constexpr int numNeighbors = 8;
@@ -62,6 +68,45 @@ public:
}
}
/** attach importance-factors to the grid */
template <int gridSize_cm, typename T> void addDistanceToTarget(Grid<gridSize_cm, T>& g, Dijkstra<T>& d) {
//Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
for (T& node : g) {
DijkstraNode<T>* dn = d.getNode(node);
if (dn != nullptr) {
node.distToTarget = dn->cumWeight / 2000;
}
}
}
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
// routing path
DijkstraPath<T> path(end, start);
// knn search within the path
KNN<DijkstraPath<T>, 3> knn(path);
// update each node from the grid using its distance to the path
for (T& n : g) {
//const int idx = knn.getNearestIndex( {n.x_cm, n.y_cm, n.z_cm} );
//T& node = g[idx];
const float dist_cm = knn.getNearestDistance( {n.x_cm, n.y_cm, n.z_cm} );
const float dist_m = Units::cmToM(dist_cm);
n.impPath = 1.0 + K::NormalDistribution::getProbability(0, 1.0, dist_m) * 0.8;
}
}
@@ -102,7 +147,7 @@ public:
if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
// door?
if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.2; }
if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; }
}

17
grid/walk/GridWalkHelper.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef GRIDWALKHELPER_H
#define GRIDWALKHELPER_H
#include "../../geo/Heading.h"
class GridWalkHelper {
public:
/** get the heading-change between the two given locations */
template <typename T> static Heading getHeading(const T& from, const T& to) {
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
}
};
#endif // GRIDWALKHELPER_H

160
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h Normal file
View File

@@ -0,0 +1,160 @@
#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> class GridWalkLightAtTheEndOfTheTunnel {
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
/** per-edge: allowed heading difference */
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
/** allows drawing elements according to their probability */
DrawList<T&> drawer;
/** 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);
Dijkstra<T> dijkstra;
public:
/** ctor with the target you want to reach */
template <int gridSize_cm, typename Access> GridWalkLightAtTheEndOfTheTunnel(Grid<gridSize_cm, T>& grid, const Access& acc, const T& target) {
// build all shortest path to reach th target
dijkstra.build(target, target, acc);
// attach a corresponding weight-information to each user-grid-node
for (T& node : grid) {
const DijkstraNode<T>* dn = dijkstra.getNode(node);
// should never be null as all nodes were evaluated
if (dn != nullptr) {
node.distToTarget = dn->cumWeight/2000;
}
}
}
template <int gridSize_cm> GridWalkState<T> getDestination(Grid<gridSize_cm, T>& grid, GridWalkState<T> start, float distance_m) {
int retries = 2;
GridWalkState<T> res;
// 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 = walk(grid, start, distance_m);
} while (res.node == nullptr && --retries);
// still reaching a dead end?
// -> try a walk in the opposite direction instead
if (res.node == nullptr) {
res = walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> keep the start as-is
return (res.node == nullptr) ? (start) : (res);
}
private:
template <int gridSize_cm> GridWalkState<T> walk(Grid<gridSize_cm, T>& grid, GridWalkState<T> cur, float distRest_m) {
drawer.reset();;
// calculate the weight for all possible destinations from "cur"
for (T& neighbor : grid.neighbors(*cur.node)) {
// heading when walking from cur to neighbor
const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
// angular difference
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
// probability for this direction change?
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
// perfer locations reaching the target
const double shortening = cur.node->distToTarget - neighbor.distToTarget;
if (shortening > 0) {prob *= 30;} // << importance factor!!
drawer.add(neighbor, prob);
}
GridWalkState<T> next(nullptr, cur.heading);
// pick a random destination
T& nDir = drawer.get();
const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
next.heading = hDir;
next.heading += headingChangeDist(gen);
// compare two neighbors according to their implied heading change
auto compp = [&] (const T& n1, const T& n2) {
Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
const float d1 = next.heading.getDiffHalfRAD(h1);
const float d2 = next.heading.getDiffHalfRAD(h2);
// same heading -> prefer nodes nearer to the target. needed for stairs!!!
// BAD: leads to straight lines in some palces. see solution B (below)
//return (d1 < d2) && (n1.distToTarget < n2.distToTarget);
// VERY IMPORTANT!
// pick the node with the smallest heading change.
// if the heading change is the same for two nodes, pick a random one!
return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);
};
// pick the neighbor best matching the new heading
auto it = grid.neighbors(*cur.node);
T& nn = *std::min_element(it.begin(), it.end(), compp);
next.node = &nn;
// // pervent dramatic heading changes. instead: try again
// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
// return State(nullptr, 0);
// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H

21
grid/walk/GridWalkState.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef GRIDWALKSTATE_H
#define GRIDWALKSTATE_H
#include "../../geo/Heading.h"
template <typename T> struct GridWalkState {
/** the user-node this state resides at */
const T* node;
/** the current heading */
Heading heading;
/** empty ctor */
GridWalkState() : node(nullptr), heading(0) {;}
/** ctor with user-node and heading */
GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;}
};
#endif // GRIDWALKSTATE_H

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

@@ -0,0 +1,141 @@
#ifndef GRIDWALKWEIGHTED_H
#define GRIDWALKWEIGHTED_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> class GridWalkWeighted {
public:
struct State {
const T* node;
Heading heading;
State() : node(nullptr), heading(0) {;}
State(const T* node, Heading heading) : node(node), heading(heading) {;}
};
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
/** per-edge: allowed heading difference */
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
/** allows drawing elements according to their probability */
DrawList<T&> drawer;
/** 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);
public:
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
int retries = 2;
State res;
// 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 = walk(grid, start, distance_m);
} while (res.node == nullptr && --retries);
// still reaching a dead end?
// -> try a walk in the opposite direction instead
if (res.node == nullptr) {
res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> keep the start as-is
return (res.node == nullptr) ? (start) : (res);
}
private:
static Heading getHeading(const T& from, const T& to) {
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
}
template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
drawer.reset();;
// calculate the weight for all possible destinations from "cur"
for (T& neighbor : grid.neighbors(*cur.node)) {
// heading when walking from cur to neighbor
const Heading potentialHeading = getHeading(*cur.node, neighbor);
// angular difference
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
// probability for this direction change?
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
// perfer locations reaching the target
const double shortening = cur.node->distToTarget - neighbor.distToTarget;
if (shortening > 0) {prob *= 5;}
drawer.add(neighbor, prob);
}
State next(nullptr, cur.heading);
// pick a random destination
T& nDir = drawer.get();
const Heading hDir = getHeading(*cur.node, nDir);
//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
next.heading = hDir;
next.heading += headingChangeDist(gen);
// compare two neighbors according to their implied heading change
auto compp = [&] (const T& n1, const T& n2) {
Heading h1 = getHeading(*cur.node, n1);
Heading h2 = getHeading(*cur.node, n2);
const float d1 = next.heading.getDiffHalfRAD(h1);
const float d2 = next.heading.getDiffHalfRAD(h2);
return d1 < d2;
};
// pick the neighbor best matching the new heading
auto it = grid.neighbors(*cur.node);
T& nn = *std::min_element(it.begin(), it.end(), compp);
next.node = &nn;
// // pervent dramatic heading changes. instead: try again
// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
// return State(nullptr, 0);
// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKWEIGHTED_H

127
grid/walk/GridWalkWeighted2.h Normal file
View File

@@ -0,0 +1,127 @@
#ifndef GRIDWALKWEIGHTED2_H
#define GRIDWALKWEIGHTED2_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> class GridWalkWeighted {
public:
struct State {
const T* node;
Heading heading;
State() : node(nullptr), heading(0) {;}
State(const T* node, Heading heading) : node(node), heading(heading) {;}
};
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
/** per-edge: allowed heading difference */
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(20);
/** allows drawing elements according to their probability */
DrawList<T&> drawer;
/** 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);
public:
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
int retries = 2;
State res;
// 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 = walk(grid, start, distance_m);
} while (res.node == nullptr && --retries);
// still reaching a dead end?
// -> try a walk in the opposite direction instead
if (res.node == nullptr) {
res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> keep the start as-is
return (res.node == nullptr) ? (start) : (res);
}
private:
static Heading getHeading(const T& from, const T& to) {
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
}
template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
// make the draw-list empty (faster than allocating a new one every time?)
drawer.reset();
// calculate probabilites for the neighboring nodes
for (T& neighbor : grid.neighbors(*cur.node)) {
// heading when walking from cur to neighbor
Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, neighbor.x_cm, neighbor.y_cm);
// angular difference
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
// probability for this change?
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
// adjust by the neighbors importance factor
//prob += std::pow(neighbor.impPath, 2);
prob *= neighbor.imp;
//prob *= K::NormalDistribution::getProbability(1.4, 0.2, neighbor.imp);
drawer.add(neighbor, prob);
}
// if there is no likely neighbor at all, we reached a dead end
// -> start over!
if (drawer.getCumProbability() < 0.01) {return State(nullptr, 0);}
// get the next node
T* nn = &drawer.get();
//Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, nn->x_cm, nn->y_cm);
//State next(nn, potentialHeading);
State next(&nn, cur.heading);
next.heading += headingChangeDist(gen);
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKWEIGHTED2_H

139
grid/walk/TestWalkWeighted3.h Normal file
View File

@@ -0,0 +1,139 @@
#ifndef GRIDWALKWEIGHTED3_H
#define GRIDWALKWEIGHTED3_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> class GridWalkWeighted {
public:
struct State {
const T* node;
Heading heading;
State() : node(nullptr), heading(0) {;}
State(const T* node, Heading heading) : node(node), heading(heading) {;}
};
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
/** per-edge: allowed heading difference */
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
/** allows drawing elements according to their probability */
DrawList<T&> drawer;
/** 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);
public:
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
int retries = 2;
State res;
// 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 = walk(grid, start, distance_m);
} while (res.node == nullptr && --retries);
// still reaching a dead end?
// -> try a walk in the opposite direction instead
if (res.node == nullptr) {
res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> keep the start as-is
return (res.node == nullptr) ? (start) : (res);
}
private:
static Heading getHeading(const T& from, const T& to) {
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
}
template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
drawer.reset();;
// calculate the weight for all possible destinations from "cur"
for (T& neighbor : grid.neighbors(*cur.node)) {
// heading when walking from cur to neighbor
const Heading potentialHeading = getHeading(*cur.node, neighbor);
// angular difference
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
// probability for this change?
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
prob *= std::pow(neighbor.impPath, 5);
drawer.add(neighbor, prob);
}
State next(nullptr, cur.heading);
// pick a random destination
T& nDir = drawer.get();
const Heading hDir = getHeading(*cur.node, nDir);
//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.95;
next.heading = hDir;
next.heading += headingChangeDist(gen);
// compare two neighbors according to their implied heading change
auto compp = [&] (const T& n1, const T& n2) {
Heading h1 = getHeading(*cur.node, n1);
Heading h2 = getHeading(*cur.node, n2);
const float d1 = next.heading.getDiffHalfRAD(h1);
const float d2 = next.heading.getDiffHalfRAD(h2);
return d1 < d2;
};
// pick the neighbor best matching the new heading
auto it = grid.neighbors(*cur.node);
T& nn = *std::min_element(it.begin(), it.end(), compp);
next.node = &nn;
// // pervent dramatic heading changes. instead: try again
// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
// return State(nullptr, 0);
// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKWEIGHTED3_H