huge commit
- worked on about everything - grid walker using plugable modules - wifi models - new distributions - worked on geometric data-structures - added typesafe timestamps - worked on grid-building - added sensor-classes - added sensor analysis (step-detection, turn-detection) - offline data reader - many test-cases
This commit is contained in:
18
grid/factory/v2/GridNodeImportance.h
Normal file
18
grid/factory/v2/GridNodeImportance.h
Normal file
@@ -0,0 +1,18 @@
|
||||
#ifndef GRIDNODEIMPORTANCE_H
|
||||
#define GRIDNODEIMPORTANCE_H
|
||||
|
||||
|
||||
struct GridNodeImportance {
|
||||
|
||||
/** importance-weight for dijkstra calculation */
|
||||
float navImportance;
|
||||
|
||||
/** get the node's nav importance */
|
||||
float getNavImportance() const {return navImportance;}
|
||||
|
||||
/** ctor */
|
||||
GridNodeImportance() : navImportance(1.0f) {;}
|
||||
|
||||
};
|
||||
|
||||
#endif // GRIDNODEIMPORTANCE_H
|
||||
@@ -1,6 +1,7 @@
|
||||
#ifndef IMPORTANCE_H
|
||||
#define IMPORTANCE_H
|
||||
|
||||
#include "../../../geo/Units.h"
|
||||
#include "../../Grid.h"
|
||||
|
||||
#include "../../../misc/KNN.h"
|
||||
@@ -10,6 +11,8 @@
|
||||
#include "../../../math/Distributions.h"
|
||||
|
||||
|
||||
|
||||
|
||||
class Importance {
|
||||
|
||||
private:
|
||||
@@ -32,9 +35,9 @@ public:
|
||||
}
|
||||
|
||||
/** attach importance-factors to the grid */
|
||||
template <typename T> static void addImportance(Grid<T>& g, const float z_cm) {
|
||||
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));
|
||||
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());
|
||||
@@ -70,7 +73,7 @@ public:
|
||||
KNNArray<std::vector<T>> knnArrDoors(doors);
|
||||
KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
|
||||
|
||||
Distribution::Normal<float> favorDoors(0.0f, 0.6f);
|
||||
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
|
||||
|
||||
// process each node again
|
||||
for (T& n1 : g) {
|
||||
@@ -90,7 +93,7 @@ public:
|
||||
neighbors.push_back(&inv[indices[i]]);
|
||||
}
|
||||
|
||||
n1.imp = 1.0f;
|
||||
n1.navImportance = 1.0f;
|
||||
|
||||
//if (n1.getType() == GridNode::TYPE_FLOOR) {
|
||||
|
||||
@@ -100,20 +103,20 @@ public:
|
||||
// get the distance to the nearest door
|
||||
const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
|
||||
|
||||
n1.imp =
|
||||
n1.navImportance =
|
||||
1 +
|
||||
getWallImportance( distToWall_m ) +
|
||||
favorDoors.getProbability(distToDoor_m);
|
||||
favorDoors.getProbability(distToDoor_m) * 1.5f;
|
||||
|
||||
|
||||
//}
|
||||
//addDoor(n1, neighbors);
|
||||
|
||||
// importance for this node (based on the distance from the next door)
|
||||
//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
|
||||
//n1.navImportance += favorDoors.getProbability(dist_m) * 0.30;
|
||||
|
||||
|
||||
//n1.imp = (dist_m < 0.2) ? (1) : (0.5);
|
||||
//n1.navImportance = (dist_m < 0.2) ? (1) : (0.5);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -131,73 +134,73 @@ public:
|
||||
|
||||
}
|
||||
|
||||
/** is the given node (and its inverted neighbors) a door? */
|
||||
template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
|
||||
// /** is the given node (and its inverted neighbors) a door? */
|
||||
// template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
|
||||
|
||||
if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
|
||||
// if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
|
||||
|
||||
MiniMat2 m1;
|
||||
// MiniMat2 m2;
|
||||
Point3 center = nSrc;
|
||||
// MiniMat2 m1;
|
||||
//// MiniMat2 m2;
|
||||
// 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();
|
||||
// // 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() > 40) {return false;}
|
||||
// // if nSrc is too far from the centroid, this does not make sense
|
||||
// if ((centroid-center).length() > 40) {return false;}
|
||||
|
||||
// build covariance of the nearest-neighbors
|
||||
int used = 0;
|
||||
for (const T* n : neighbors) {
|
||||
// // build covariance of the nearest-neighbors
|
||||
// int used = 0;
|
||||
// for (const T* n : neighbors) {
|
||||
|
||||
const Point3 d1 = (Point3)*n - centroid;
|
||||
if (d1.length() > 100) {continue;} // radius search
|
||||
m1.addSquared(d1.x, d1.y);
|
||||
// const Point3 d1 = (Point3)*n - centroid;
|
||||
// if (d1.length() > 100) {continue;} // radius search
|
||||
// m1.addSquared(d1.x, d1.y);
|
||||
|
||||
// const Point3 d2 = (Point3)*n - center;
|
||||
// if (d2.length() > 100) {continue;} // radius search
|
||||
// m2.addSquared(d2.x, d2.y);
|
||||
//// const Point3 d2 = (Point3)*n - center;
|
||||
//// if (d2.length() > 100) {continue;} // radius search
|
||||
//// m2.addSquared(d2.x, d2.y);
|
||||
|
||||
++used;
|
||||
// ++used;
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
// we need at least two points for the covariance
|
||||
if (used < 6) {return false;}
|
||||
// // we need at least two points for the covariance
|
||||
// if (used < 6) {return false;}
|
||||
|
||||
// check eigenvalues
|
||||
MiniMat2::EV ev1 = m1.getEigenvalues();
|
||||
// MiniMat2::EV ev2 = m2.getEigenvalues();
|
||||
// // check eigenvalues
|
||||
// MiniMat2::EV ev1 = m1.getEigenvalues();
|
||||
//// MiniMat2::EV ev2 = m2.getEigenvalues();
|
||||
|
||||
// ensure e1 > e2
|
||||
if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
|
||||
// if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}
|
||||
// // ensure e1 > e2
|
||||
// if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
|
||||
//// if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}
|
||||
|
||||
// door?
|
||||
const float ratio1 = (ev1.e2/ev1.e1);
|
||||
// const float ratio2 = (ev2.e2/ev2.e1);
|
||||
// const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);
|
||||
// // door?
|
||||
// const float ratio1 = (ev1.e2/ev1.e1);
|
||||
//// const float ratio2 = (ev2.e2/ev2.e1);
|
||||
//// const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);
|
||||
|
||||
return (ratio1 < 0.30 && ratio1 > 0.05) ;
|
||||
// return (ratio1 < 0.30 && ratio1 > 0.05) ;
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
/** get the importance of the given node depending on its nearest wall */
|
||||
static float getWallImportance(float dist_m) {
|
||||
|
||||
// avoid sticking too close to walls (unlikely)
|
||||
static Distribution::Normal<float> avoidWalls(0.0, 0.5);
|
||||
static Distribution::Normal<float> avoidWalls(0.0, 0.35);
|
||||
|
||||
// favour walking near walls (likely)
|
||||
static Distribution::Normal<float> stickToWalls(0.9, 0.5);
|
||||
//static Distribution::Normal<float> stickToWalls(0.9, 0.7);
|
||||
|
||||
// favour walking far away (likely)
|
||||
static Distribution::Normal<float> farAway(2.2, 0.5);
|
||||
if (dist_m > 2.0) {dist_m = 2.0;}
|
||||
//static Distribution::Normal<float> farAway(2.2, 0.5);
|
||||
//if (dist_m > 2.0) {dist_m = 2.0;}
|
||||
|
||||
// overall importance
|
||||
// return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
|
||||
|
||||
@@ -233,7 +233,8 @@ public:
|
||||
|
||||
// no matching node found -> add a new one to the grid
|
||||
if (iNode.idx == -1) {
|
||||
iNode.idx = grid.add(iNode.node);
|
||||
const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, iNode.node.z_cm));
|
||||
iNode.idx = (n2) ? (n2->getIdx()) : (grid.add(iNode.node));
|
||||
}
|
||||
|
||||
// add semantic information
|
||||
|
||||
Reference in New Issue
Block a user