From f77a28735b6a7c4502bea9d69903500b369b7f6f Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 21 Apr 2016 08:59:05 +0200 Subject: [PATCH] added von mises distributionb quick&dirty: added activity to the grid-walkers --- grid/walk/GridWalk.h | 11 +++- grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 2 +- grid/walk/GridWalkPathControl.h | 16 ++++-- grid/walk/GridWalkRandomHeadingUpdate.h | 2 +- grid/walk/GridWalkShortestPathControl.h | 2 +- grid/walk/GridWalkSimpleControl.h | 2 +- math/Distributions.h | 1 + math/Math.h | 20 +++++++ math/distribution/Bessel.h | 57 +++++++++++++++++++ math/distribution/LUT.h | 58 ++++++++++++++++++++ math/distribution/VonMises.h | 56 +++++++++++++++++++ 11 files changed, 216 insertions(+), 11 deletions(-) create mode 100644 math/Math.h create mode 100644 math/distribution/Bessel.h create mode 100644 math/distribution/LUT.h create mode 100644 math/distribution/VonMises.h diff --git a/grid/walk/GridWalk.h b/grid/walk/GridWalk.h index fc5336f..31c0ea9 100644 --- a/grid/walk/GridWalk.h +++ b/grid/walk/GridWalk.h @@ -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 class GridWalk { public: - virtual GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) = 0; + virtual GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) = 0; }; diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index eccff20..1dc8b58 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -65,7 +65,7 @@ public: } - GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad); diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index 4980780..87926ee 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -61,7 +61,7 @@ public: } - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // proportional change of the heading static Distribution::Normal dHead(1, 0.01); @@ -75,13 +75,13 @@ public: static Distribution::Normal 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 walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) { + GridWalkState walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) { // try-again distribution //static Distribution::Normal 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); } diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 523c418..ce5209e 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -47,7 +47,7 @@ public: - GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1); diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h index 32512a3..d69c6a1 100644 --- a/grid/walk/GridWalkShortestPathControl.h +++ b/grid/walk/GridWalkShortestPathControl.h @@ -117,7 +117,7 @@ public: int times = 3; float pOther = 0.10; - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // update the center-of-mass diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index 7ea6cef..d5911ab 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -41,7 +41,7 @@ public: - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // proportional change of the heading static Distribution::Normal dHead(1, 0.01); diff --git a/math/Distributions.h b/math/Distributions.h index 0559b24..f4cec9f 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -5,5 +5,6 @@ #include "distribution/Exponential.h" #include "distribution/Logistic.h" #include "distribution/Uniform.h" +#include "distribution/VonMises.h" #endif // DISTRIBUTIONS_H diff --git a/math/Math.h b/math/Math.h new file mode 100644 index 0000000..4d12949 --- /dev/null +++ b/math/Math.h @@ -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 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 diff --git a/math/distribution/Bessel.h b/math/distribution/Bessel.h new file mode 100644 index 0000000..d3f60b0 --- /dev/null +++ b/math/distribution/Bessel.h @@ -0,0 +1,57 @@ +#ifndef BESSEL_H +#define BESSEL_H + +namespace Distribution { + + /** helper class */ + template 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 diff --git a/math/distribution/LUT.h b/math/distribution/LUT.h new file mode 100644 index 0000000..ef46015 --- /dev/null +++ b/math/distribution/LUT.h @@ -0,0 +1,58 @@ +#ifndef LUT_H +#define LUT_H + +#include +#include "../Math.h" + +namespace Distribution { + + template class LUT { + + private: + + Scalar min; + Scalar max; + Scalar diff; + + /** number of samples between min and max */ + int numSamples; + + /** the look-up-table */ + std::vector samples; + + + public: + + /** ctor */ + template 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 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 diff --git a/math/distribution/VonMises.h b/math/distribution/VonMises.h new file mode 100644 index 0000000..789e0c8 --- /dev/null +++ b/math/distribution/VonMises.h @@ -0,0 +1,56 @@ +#ifndef K_MATH_DISTRIBUTION_VONMISES_H +#define K_MATH_DISTRIBUTION_VONMISES_H + +#include +#include + +#include "../Math.h" +#include "Bessel.h" +#include "LUT.h" + +namespace Distribution { + + /** von-mises distribution */ + template 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 lut; + + /** helper for modified bessel I_0(kappa) */ + Bessel bessel; + + public: + + /** ctor */ + VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) { + + } + + LUT getLUT() const { + return LUT(*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