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);

1
math/Distributions.h
View File

@@ -5,5 +5,6 @@
#include "distribution/Exponential.h"
#include "distribution/Logistic.h"
#include "distribution/Uniform.h"
#include "distribution/VonMises.h"
#endif // DISTRIBUTIONS_H

20
math/Math.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef K_MATH_MATH_H
#define K_MATH_MATH_H
#include "../Defines.h"
class Math {
public:
/** ensure val stays within [min:max] */
template <typename Scalar> static inline Scalar clamp(const Scalar min, const Scalar max, const Scalar val) {
if (unlikely(val < min)) {return min;}
if (unlikely(val > max)) {return max;}
return val;
}
};
#endif // K_MATH_MATH_H

57
math/distribution/Bessel.h Normal file
View File

@@ -0,0 +1,57 @@
#ifndef BESSEL_H
#define BESSEL_H
namespace Distribution {
/** helper class */
template <typename T> class Bessel {
public:
/**
* calculation for I_v(z)
* https://en.wikipedia.org/wiki/Bessel_function
* http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html
*/
T getModified(const int v, const T z) const {
// running factorials (updated within for-loops)
uint64_t runFac1 = 1;
uint64_t runFac2 = factorial(v); // Delta(k+v+1) = (k+v+1-1)! = (k+v)! [k starts at 0] -> (v)!
// running potential
T runPot = 1;
const T pot = T(0.25) * z * z;
// start for-loop at k=1 instead of k=0. allows for running factorial without using if (k==0)
T sum = 0;
for (int k = 0; k < 16; ) {
sum += runPot / runFac1 / runFac2;
++k;
runFac1 *= k;
runFac2 *= k;
runPot *= pot;
}
// done
return std::pow( (T(0.5) * z), v) * sum;
}
private:
static uint64_t factorial(const int n) {
uint64_t res = 1;
for (int i = 2; i <= n; ++i) {res *= i;}
return res;
}
};
}
#endif // BESSEL_H

58
math/distribution/LUT.h Normal file
View File

@@ -0,0 +1,58 @@
#ifndef LUT_H
#define LUT_H
#include <vector>
#include "../Math.h"
namespace Distribution {
template <typename Scalar> class LUT {
private:
Scalar min;
Scalar max;
Scalar diff;
/** number of samples between min and max */
int numSamples;
/** the look-up-table */
std::vector<Scalar> samples;
public:
/** ctor */
template <typename Distribution> LUT(const Distribution dist, const Scalar min, const Scalar max, const int numSamples) :
min(min), max(max), diff(max-min), numSamples(numSamples) {
buildLUT(dist);
}
/** get the probability of val from the LUT */
Scalar getProbability(const Scalar val) const {
int idx = ((val - min) * numSamples / diff);
idx = Math::clamp(0, numSamples-1, idx);
return samples[idx];
}
private:
/** build the look-up-table */
template <typename Distribution> void buildLUT(const Distribution dist) {
samples.resize(numSamples);
for (int idx = 0; idx < numSamples; ++idx) {
const Scalar val = min + (idx * diff / numSamples);
samples[idx] = dist.getProbability(val);
}
}
};
}
#endif // LUT_H

56
math/distribution/VonMises.h Normal file
View File

@@ -0,0 +1,56 @@
#ifndef K_MATH_DISTRIBUTION_VONMISES_H
#define K_MATH_DISTRIBUTION_VONMISES_H
#include <cmath>
#include <random>
#include "../Math.h"
#include "Bessel.h"
#include "LUT.h"
namespace Distribution {
/** von-mises distribution */
template <typename T> class VonMises {
private:
/** center of the distribution */
const T mu;
/** like 1.0/variance of the distribution */
const T kappa;
/** pre-calcuated look-up-table */
std::vector<T> lut;
/** helper for modified bessel I_0(kappa) */
Bessel<T> bessel;
public:
/** ctor */
VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) {
}
LUT<T> getLUT() const {
return LUT<T>(*this, -M_PI, +M_PI, 10000);
}
/** get probability for the given value */
T getProbability(const T _val) const {
const T val = Math::clamp((T)-M_PI, (T)+M_PI, _val);
return
std::exp(kappa * std::cos(val - mu)) /
(2 * M_PI * bessel.getModified(0, kappa));
}
};
}
#endif // K_MATH_DISTRIBUTION_VONMISES_H