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

added von mises distributionb

Browse Source 
quick&dirty: added activity to the grid-walkers
This commit is contained in:
FrankE
2016-04-21 08:59:05 +02:00
parent a00f2cf80a
commit f77a28735b
11 changed files with 216 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
grid/walk/GridWalk.h
View File

@@ -4,11 +4,20 @@
#include "GridWalkState.h"
#include "../Grid.h"
/** all supported acitivites lukas can detect */
enum class Activity {
UNKNOWN,
STANDING,
WALKING,
STAIRS,
ELEVATOR,
};
template <typename T> class GridWalk {
public:
virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) = 0;
virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) = 0;
};

2
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
View File

@@ -65,7 +65,7 @@ public:
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) override {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad);

16
grid/walk/GridWalkPathControl.h
View File

@@ -61,7 +61,7 @@ public:
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
// proportional change of the heading
static Distribution::Normal<float> dHead(1, 0.01);
@@ -75,13 +75,13 @@ public:
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);
return walk(grid, start, distance_m, headChange_rad, act);
}
private:
double getProbability(const T& start, const T& prev, const T& possible, const Heading head) const {
double getProbability(const T& start, const T& prev, const T& possible, const Heading head, Activity act) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
@@ -104,12 +104,16 @@ private:
//const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // from start
const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node
double actProb = 1.0;
if (act == Activity::STAIRS) {actProb = (prev.z_cm != possible.z_cm) ? (0.8) : (0.2);}
if (act == Activity::WALKING) {actProb = (prev.z_cm == possible.z_cm) ? (0.8) : (0.2);}
// bring it together
return angleProb * nodeProb;
return angleProb * nodeProb * actProb;
}
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) {
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) {
// try-again distribution
//static Distribution::Normal<float> dHead(0, Angle::degToRad(10));
@@ -130,7 +134,7 @@ private:
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading);
const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading, act);
drawer.add(neighbor, prob);
}

2
grid/walk/GridWalkRandomHeadingUpdate.h
View File

@@ -47,7 +47,7 @@ public:
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) override {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1);

2
grid/walk/GridWalkShortestPathControl.h
View File

@@ -117,7 +117,7 @@ public:
int times = 3;
float pOther = 0.10;
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
// update the center-of-mass

2
grid/walk/GridWalkSimpleControl.h
View File

@@ -41,7 +41,7 @@ public:
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
// proportional change of the heading
static Distribution::Normal<float> dHead(1, 0.01);