85 lines
1.9 KiB
C++
85 lines
1.9 KiB
C++
#ifndef GRIDIMPORTANCE_H
|
|
#define GRIDIMPORTANCE_H
|
|
|
|
#include "../Grid.h"
|
|
#include "GridFactory.h"
|
|
#include "../../misc/KNN.h"
|
|
|
|
#include <KLib/math/distribution/Normal.h>
|
|
|
|
class GridImportance {
|
|
|
|
public:
|
|
|
|
/** attach importance-factors to the grid */
|
|
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
|
|
|
|
// get an inverted version of the grid
|
|
Grid<gridSize_cm, T> inv;
|
|
GridFactory<gridSize_cm, T> fac(inv);
|
|
fac.addInverted(g, z_cm);
|
|
|
|
// construct KNN search
|
|
KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
|
|
|
|
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];
|
|
float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
|
|
knn.get(point, 10, indices, squaredDist);
|
|
|
|
const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
|
|
const float imp2 = door( indices );
|
|
n1.imp = (imp1 + imp2)/2;
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
float door( size_t* indices ) {
|
|
|
|
// build covariance
|
|
|
|
//if (dist1_m > 1.0) {return 1;}
|
|
//return 1.0 - std::abs(dist1_m - dist2_m);
|
|
return 1;
|
|
}
|
|
|
|
float importance(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;
|
|
|
|
// static K::NormalDistribution d1(1.0, 0.75);
|
|
// //static K::NormalDistribution d2(3.0, 0.75);
|
|
|
|
// 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;}
|
|
}
|
|
|
|
};
|
|
|
|
#endif // GRIDIMPORTANCE_H
|