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

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
grid/factory/v2/Importance.h
View File

@@ -1,8 +1,11 @@
#ifndef IMPORTANCE_H
#define IMPORTANCE_H
#include "../../../geo/Units.h"
#include "../../Grid.h"
#include "GridFactoryListener.h"
#include "../../../misc/KNN.h"
#include "../../../misc/KNNArray.h"
@@ -35,9 +38,14 @@ public:
}
/** attach importance-factors to the grid */
template <typename T> static void addImportance(Grid<T>& g) {
template <typename T> static void addImportance(Grid<T>& g, GridFactoryListener* l = nullptr) {
Log::add(name, "adding importance information to all nodes");// at height " + std::to_string(z_cm));
Log::add(name, "adding importance information to all nodes");
if (l) {
l->onGridBuildUpdateMajor(2, 0);
l->onGridBuildUpdateMajor("adding importance information");
l->onGridBuildUpdateMinor("performing initial setups");
}
// get an inverted version of the grid
Grid<T> inv(g.getGridSize_cm());
@@ -51,15 +59,13 @@ public:
// construct KNN search
KNN<Grid<T>, 3> knn(inv);
// the number of neighbors to use
static constexpr int numNeighbors = 12;
// create list of all door-nodes
std::vector<T> doors;
// create list of all stair-nodes
std::vector<T> stairs;
// process each node
for (T& n1 : g) {
@@ -83,9 +89,26 @@ public:
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Normal<float> favorStairs(0.0f, 3.5f);
if (l) {
l->onGridBuildUpdateMajor(2, 1);
l->onGridBuildUpdateMinor("calculating importance for each node");
}
std::cout << "dunno why, but the KNN for stairs searches extremely slow!" << std::endl;
// process each node again
for (T& n1 : g) {
for (int i = 0; i < g.getNumNodes(); ++i) {
// log
if (i % (g.getNumNodes() / 20) == 0) {
if (l) {
l->onGridBuildUpdateMinor(g.getNumNodes(), i);
}
}
// get the node
T& n1 = g[i];
// get the distance to the nearest wall
const float distToWall_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
@@ -101,12 +124,16 @@ public:
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 3.5f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f;
// use wall avoidance
if (useNormal) {
n1.navImportance -= avoidWalls.getProbability(distToWall_m);
n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
}
// sanity check
Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");
}
}