FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added missing code changes

Browse Source 
started working on refactoring of sensors
new test-cases
This commit is contained in:
FrankE
2016-03-17 17:28:41 +01:00
parent 6346231a64
commit f8d7f768f1
18 changed files with 524 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
grid/walk/GridWalkPathControl.h
View File

@@ -32,6 +32,10 @@ private:
DrawList<T&> drawer;
public:
float pOther = 0.10;
public:
@@ -65,9 +69,11 @@ public:
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.4; // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.15);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -75,12 +81,12 @@ public:
private:
double getProbability(const T& start, const T& possible, const Heading head) const {
double getProbability(const T& start, const T& prev, const T& possible, const Heading head) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
if (start.z_cm == possible.z_cm) {return 0;} // back to the start
//throw 1;
throw 1;
return 0.5;// stair start/end TODO: fix
}
@@ -95,7 +101,8 @@ private:
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance
const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025;
//const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // from start
const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node
// bring it together
return angleProb * nodeProb;
@@ -123,7 +130,7 @@ private:
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, neighbor, reqHeading);
const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading);
drawer.add(neighbor, prob);
}

82
grid/walk/GridWalkShortestPathControl.h
View File

@@ -17,6 +17,8 @@
#include "GridWalkHelper.h"
#include "GridWalk.h"
#include <KLib/math/statistics/Statistics.h>
template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {
@@ -82,14 +84,17 @@ protected:
Dijkstra<T> dijkstra;
const T& target;
Point3 centerOfMass = Point3(0,0,0);
//Point3 centerOfMass = Point3(0,0,0);
Wrapper wrapper;
DijkstraPath<T>* path = nullptr;
KNN<Wrapper,3>* knn = nullptr;
//KNN<Wrapper,3>* knn = nullptr;
DrawList<T&> drawer;
std::vector<Point3> points;
Point3 centerOfMass;
float stdDevDist;
public:
@@ -109,17 +114,51 @@ public:
int recalc = 0;
int times = 3;
float pOther = 0.10;
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
// update the center-of-mass
centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
if (path == nullptr) {rebuildPath(grid);}
if (++recalc > 100 * 1000) {
// update the center-of-mass
points.push_back( (Point3)*start.node );
//centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
//if (path == nullptr) {rebuildPath(grid);}
if (++recalc >= 7500) {
// center of mass
Point3 sum(0,0,0);
for (const Point3& p : points) {sum += p;}
centerOfMass = sum/points.size();
// deviation from the center of mass
float dSum = 0;
float dSum2 = 0;
for (const Point3& p : points) {
const float d = p.getDistance(centerOfMass);
dSum += d;
dSum2 += d*d;
}
dSum /= points.size();
dSum2 /= points.size();
stdDevDist = std::sqrt( (dSum2) - (dSum * dSum) ) * times;
// restart
points.clear();
recalc = 0;
rebuildPath(grid);
// update
rebuildPath(grid, centerOfMass);
}
// if (knn != nullptr) {
// const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} );
// if (dist > 10000) {
@@ -133,9 +172,11 @@ public:
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.5; // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.10);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -143,12 +184,12 @@ public:
private:
double getProbability(const T& start, const T& possible, const Heading head) const {
double getProbability(Grid<T>& grid, const T& start, const T& prev, const T& possible, const Heading head) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
if (start.z_cm == possible.z_cm) {return 0;} // back to the start
//throw 1;
throw 1;
return 0.5;// stair start/end TODO: fix
}
@@ -165,11 +206,16 @@ private:
// nodes own importance
double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025;
if (knn != nullptr) {
const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
if (path != nullptr) {
//const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
int steps = stdDevDist / grid.getGridSize_cm();
const float pToTarget = possible.getDistanceInMeter(*path->getFromStart(steps).element);
const float sToTarget = prev.getDistanceInMeter(*path->getFromStart(steps).element);
nodeProb = (pToTarget < sToTarget) ? (1.0) : (pOther);
//const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} );
//nodeProb = (pd < sd) ? 1 : 0.0;
nodeProb = Distribution::Exponential<float>::getProbability(0.15, pd_m);
//nodeProb = Distribution::Exponential<float>::getProbability(0.9, pToTarget);
//nodeProb = Distribution::Exponential<float>::getProbability(1.0, tDist_m);
}
// bring it together
@@ -198,7 +244,7 @@ private:
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, neighbor, reqHeading);
const double prob = getProbability(grid, *start.node, *cur.node, neighbor, reqHeading);
drawer.add(neighbor, prob);
}
@@ -230,14 +276,14 @@ private:
}
/** rebuild the path for the given center point */
void rebuildPath(Grid<T>& grid) {
void rebuildPath(Grid<T>& grid, const Point3 centerOfMass) {
// find the grid node nearest to the current center-of-mass
auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); };
const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode);
delete path; path = nullptr;
delete knn; knn = nullptr;
//delete knn; knn = nullptr;
DijkstraNode<T>* dnTarget = dijkstra.getNode(target);
DijkstraNode<T>* dnStart = dijkstra.getNode(currentMass);
@@ -252,9 +298,9 @@ private:
// create k-nn lookup
wrapper = Wrapper(path);
knn = new KNN<Wrapper, 3>(wrapper);
//knn = new KNN<Wrapper, 3>(wrapper);
} catch (...) {
knn = nullptr;
//knn = nullptr;
path = nullptr;
}

10
grid/walk/GridWalkSimpleControl.h
View File

@@ -47,11 +47,13 @@ public:
static Distribution::Normal<float> dHead(1, 0.01);
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.50);
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.4; // TODO: why * X?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.10);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -79,8 +81,8 @@ private:
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance
//const double nodeProb = Distribution::Logistic<float>::getCDF(possible.imp, 1, 0.5);
const double nodeProb = std::pow(possible.imp, 5);
const double nodeProb = Distribution::Logistic<float>::getCDF(possible.imp, 1, 0.9);
//const double nodeProb = std::pow(possible.imp, 2);
//const double nodeProb = 1.0;
// bring it together