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

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; }
}