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

removed gridSize from the template arguments

Browse Source 
- not needed and code much cleaner
some minor changes
new test-cases
This commit is contained in:
FrankE
2016-01-25 15:53:12 +01:00
parent 9947dced15
commit 5aedce47f1
16 changed files with 167 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
grid/factory/GridImportance.h
View File

@@ -31,25 +31,26 @@ private:
public:
/** attach importance-factors to the grid */
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
template <typename T> void addImportance(Grid<T>& g, const float z_cm) {
Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
// get an inverted version of the grid
Grid<gridSize_cm, T> inv;
GridFactory<gridSize_cm, T> fac(inv);
Grid<T> inv(g.getGridSize_cm());
GridFactory<T> fac(inv);
fac.addInverted(g, z_cm);
// construct KNN search
KNN<Grid<gridSize_cm, T>, 3> knn(inv);
KNN<Grid<T>, 3> knn(inv);
// the number of neighbors to use
static constexpr int numNeighbors = 8;
for (int idx = 0; idx < g.getNumNodes(); ++idx) {
// create list of all doors
std::vector<T> doors;
// process each point
T& n1 = (T&) g[idx];
// process each node
for (T& n1 : g) {
// get the 10 nearest neighbors and their distance
size_t indices[numNeighbors];
@@ -64,14 +65,51 @@ public:
}
addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
addDoor(n1, neighbors);
//addDoor(n1, neighbors);
// is the current node a door?
if (isDoor(n1, neighbors)) {doors.push_back(n1);}
// favor stairs just like doors
if (isStaircase(g, n1)) {doors.push_back(n1);}
}
KNNArray<std::vector<T>> knnArrDoors(doors);
KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
// process each node again
for (T& n1 : g) {
static K::NormalDistribution favorDoors(0.0, 0.6);
// get the distance to the nearest door
const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
// importance for this node (based on the distance from the next door)
const float imp = 1.0 + favorDoors.getProbability(dist_m) * 0.35;
// adjust
n1.imp *= imp;
}
}
/** is the given node connected to a staircase? */
template <typename T> bool isStaircase(Grid<T>& g, T& node) {
// if this node has a neighbor with a different z, this is a stair
for (T& neighbor : g.neighbors(node)) {
if (neighbor.z_cm != node.z_cm) {return true;}
}
return false;
}
/** attach importance-factors to the grid */
template <int gridSize_cm, typename T> void addDistanceToTarget(Grid<gridSize_cm, T>& g, Dijkstra<T>& d) {
template <typename T> void addDistanceToTarget(Grid<T>& g, Dijkstra<T>& d) {
//Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
@@ -87,7 +125,7 @@ public:
}
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
template <typename T> void addImportance(Grid<T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
// routing path
DijkstraPath<T> path(end, start);
@@ -112,8 +150,8 @@ public:
}
/** add importance to nSrc if it is part of a door */
template <typename T> void addDoor( T& nSrc, std::vector<T*> neighbors ) {
/** is the given node (and its inverted neighbors) a door? */
template <typename T> bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
MiniMat2 m;
Point3 center = nSrc;
@@ -126,7 +164,7 @@ public:
centroid /= neighbors.size();
// if nSrc is too far from the centroid, this does not make sense
if ((centroid-center).length() > 60) {return;}
if ((centroid-center).length() > 20) {return false;}
// build covariance of the nearest-neighbors
int used = 0;
@@ -138,7 +176,7 @@ public:
}
// we need at least two points for the covariance
if (used < 2) {return;}
if (used < 2) {return false;}
// check eigenvalues
MiniMat2::EV ev = m.getEigenvalues();
@@ -147,7 +185,7 @@ public:
if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
// door?
if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; }
return ((ev.e2/ev.e1) < 0.15) ;
}