FrankE
d283d9b326
new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping
145 lines
3.4 KiB
C++
145 lines
3.4 KiB
C++
#ifndef IMPORTANCE_H
|
|
#define IMPORTANCE_H
|
|
|
|
|
|
|
|
#include "../../../geo/Units.h"
|
|
#include "../../Grid.h"
|
|
#include "GridFactoryListener.h"
|
|
|
|
#include "../../../misc/KNN.h"
|
|
#include "../../../misc/KNNArray.h"
|
|
|
|
#include "../../../math/MiniMat2.h"
|
|
#include "../../../math/Distributions.h"
|
|
|
|
|
|
|
|
|
|
class Importance {
|
|
|
|
private:
|
|
|
|
static constexpr const char* name = "GridImp";
|
|
|
|
public:
|
|
|
|
|
|
template <typename T> static void addOutlineNodes(Grid<T>& dst, Grid<T>& src) {
|
|
|
|
for (const T& n : src) {
|
|
if (n.getNumNeighbors() < 8) {
|
|
if (!dst.hasNodeFor(n)) {
|
|
dst.add(n);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
/** attach importance-factors to the grid */
|
|
template <typename T> static void addImportance(Grid<T>& g, GridFactoryListener* l = nullptr) {
|
|
|
|
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());
|
|
addOutlineNodes(inv, g);
|
|
//GridFactory<T> fac(inv);
|
|
//fac.addInverted(g, z_cm);
|
|
|
|
// sanity check
|
|
Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!");
|
|
|
|
// construct KNN search
|
|
KNN<Grid<T>, 3> knn(inv);
|
|
|
|
// 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) {
|
|
|
|
switch(n1.getType()) {
|
|
case GridNode::TYPE_DOOR: doors.push_back(n1); break;
|
|
case GridNode::TYPE_STAIR: stairs.push_back(n1); break;
|
|
}
|
|
|
|
}
|
|
|
|
// KNN for doors
|
|
KNNArray<std::vector<T>> knnArrDoors(doors);
|
|
KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
|
|
|
|
// KNN for stairs
|
|
KNNArray<std::vector<T>> knnArrStairs(stairs);
|
|
KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);
|
|
|
|
// probability adjustments
|
|
Distribution::Normal<float> avoidWalls(0.0, 0.35);
|
|
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 (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} ));
|
|
|
|
// get the distance to the nearest door
|
|
const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
|
|
|
|
// get the distance to the nearest stair
|
|
const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
|
|
|
|
const bool useNormal = (distToWall_m < distToDoor_m && distToWall_m < distToStair_m);
|
|
|
|
// final probability
|
|
n1.navImportance = 1.0f;
|
|
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
|
|
n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f;
|
|
|
|
// use wall avoidance
|
|
if (useNormal) {
|
|
n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
|
|
}
|
|
|
|
// sanity check
|
|
Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // IMPORTANCE_H
|