added new data-structures
added new test-cases added flexible dijkstra calculation added debugging log modified: plotting, grid-generation, grid-importance, refactoring
This commit is contained in:
@@ -4,16 +4,31 @@
|
||||
#include "../Grid.h"
|
||||
#include "GridFactory.h"
|
||||
#include "../../misc/KNN.h"
|
||||
#include "../../math/MiniMat2.h"
|
||||
|
||||
#include "../../misc/Debug.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:
|
||||
* - nodes that belong to a door or narrow path are more important
|
||||
* - nodes directly located at walls are less important
|
||||
*/
|
||||
class GridImportance {
|
||||
|
||||
private:
|
||||
|
||||
static constexpr const char* name = "GridImp";
|
||||
|
||||
public:
|
||||
|
||||
/** attach importance-factors to the grid */
|
||||
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, 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);
|
||||
@@ -22,29 +37,28 @@ public:
|
||||
// construct KNN search
|
||||
KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
|
||||
|
||||
// the number of neighbors to use
|
||||
static constexpr int numNeighbors = 8;
|
||||
|
||||
for (int idx = 0; idx < g.getNumNodes(); ++idx) {
|
||||
|
||||
// process each point
|
||||
T& n1 = (T&) g[idx];
|
||||
|
||||
// // get its nearest neighbor
|
||||
// size_t idxNear;
|
||||
// float distSquared;
|
||||
// float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
|
||||
// knn.getNearest(point, idxNear, distSquared);
|
||||
|
||||
// // calculate importante
|
||||
// const float imp = importance( Units::cmToM(std::sqrt(distSquared)) );
|
||||
// n1.imp = imp;
|
||||
|
||||
size_t indices[10];
|
||||
float squaredDist[10];
|
||||
// get the 10 nearest neighbors and their distance
|
||||
size_t indices[numNeighbors];
|
||||
float squaredDist[numNeighbors];
|
||||
float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
|
||||
knn.get(point, 10, indices, squaredDist);
|
||||
knn.get(point, numNeighbors, indices, squaredDist);
|
||||
|
||||
const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
|
||||
const float imp2 = door( indices );
|
||||
n1.imp = (imp1 + imp2)/2;
|
||||
// get the neighbors
|
||||
std::vector<T*> neighbors;
|
||||
for (int i = 0; i < numNeighbors; ++i) {
|
||||
neighbors.push_back(&inv[indices[i]]);
|
||||
}
|
||||
|
||||
addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
|
||||
addDoor(n1, neighbors);
|
||||
|
||||
}
|
||||
|
||||
@@ -52,31 +66,67 @@ public:
|
||||
|
||||
}
|
||||
|
||||
float door( size_t* indices ) {
|
||||
|
||||
// build covariance
|
||||
/** add importance to nSrc if it is part of a door */
|
||||
template <typename T> void addDoor( T& nSrc, std::vector<T*> neighbors ) {
|
||||
|
||||
MiniMat2 m;
|
||||
Point3 center = nSrc;
|
||||
|
||||
// calculate the centroid of the nSrc's nearest-neighbors
|
||||
Point3 centroid(0,0,0);
|
||||
for (const T* n : neighbors) {
|
||||
centroid = centroid + (Point3)*n;
|
||||
}
|
||||
centroid /= neighbors.size();
|
||||
|
||||
// if nSrc is too far from the centroid, this does not make sense
|
||||
if ((centroid-center).length() > 60) {return;}
|
||||
|
||||
// build covariance of the nearest-neighbors
|
||||
int used = 0;
|
||||
for (const T* n : neighbors) {
|
||||
Point3 d = (Point3)*n - center;
|
||||
if (d.length() > 100) {continue;} // radius search
|
||||
m.addSquared(d.x, d.y);
|
||||
++used;
|
||||
}
|
||||
|
||||
// we need at least two points for the covariance
|
||||
if (used < 2) {return;}
|
||||
|
||||
// check eigenvalues
|
||||
MiniMat2::EV ev = m.getEigenvalues();
|
||||
|
||||
// ensure e1 > e2
|
||||
if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
|
||||
|
||||
// door?
|
||||
if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.2; }
|
||||
|
||||
//if (dist1_m > 1.0) {return 1;}
|
||||
//return 1.0 - std::abs(dist1_m - dist2_m);
|
||||
return 1;
|
||||
}
|
||||
|
||||
float importance(float dist_m) {
|
||||
/** get the importance of the given node depending on its nearest wall */
|
||||
template <typename T> void addImportance(T& nSrc, float dist_m) {
|
||||
|
||||
static K::NormalDistribution d1(0.0, 0.5);
|
||||
//if (dist_m > 1.5) {dist_m = 1.5;}
|
||||
return 1.0 - d1.getProbability(dist_m) * 0.5;
|
||||
// avoid sticking too close to walls (unlikely)
|
||||
static K::NormalDistribution avoidWalls(0.0, 0.3);
|
||||
|
||||
// static K::NormalDistribution d1(1.0, 0.75);
|
||||
// //static K::NormalDistribution d2(3.0, 0.75);
|
||||
// favour walking near walls (likely)
|
||||
static K::NormalDistribution sticToWalls(0.9, 0.5);
|
||||
|
||||
// favour walking far away (likely)
|
||||
static K::NormalDistribution farAway(2.2, 0.5);
|
||||
if (dist_m > 2.0) {dist_m = 2.0;}
|
||||
|
||||
// overall importance
|
||||
nSrc.imp *= 1.0
|
||||
- avoidWalls.getProbability(dist_m) * 0.35 // avoid walls
|
||||
+ sticToWalls.getProbability(dist_m) * 0.15 // walk near walls
|
||||
+ farAway.getProbability(dist_m) * 0.20 // walk in the middle
|
||||
;
|
||||
|
||||
// if (dist_m > 3.0) {dist_m = 3.0;}
|
||||
// return 0.8 + d1.getProbability(dist_m);// + d2.getProbability(dist_m);
|
||||
|
||||
// if (dist_m < 0.5) {return 0.8;}
|
||||
// if (dist_m < 1.5) {return 1.2;}
|
||||
// if (dist_m < 2.5) {return 0.8;}
|
||||
// else {return 1.2;}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user