FrankE
a203305628
- changed the wifi-estimation api - adjusted test-cases - worked on grid-bulding and grid-importance - new walking modules - fixed some minor issues
118 lines
2.8 KiB
C++
118 lines
2.8 KiB
C++
#ifndef IMPORTANCE_H
|
|
#define IMPORTANCE_H
|
|
|
|
#include "../../../geo/Units.h"
|
|
#include "../../Grid.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) {
|
|
|
|
Log::add(name, "adding importance information to all nodes");// at height " + std::to_string(z_cm));
|
|
|
|
// 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);
|
|
|
|
// 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) {
|
|
|
|
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);
|
|
|
|
|
|
// process each node again
|
|
for (T& n1 : g) {
|
|
|
|
// 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) * 3.5f;
|
|
|
|
if (useNormal) {
|
|
n1.navImportance -= avoidWalls.getProbability(distToWall_m);
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // IMPORTANCE_H
|