diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h new file mode 100644 index 0000000..e42c91b --- /dev/null +++ b/grid/walk/v3/GridWalker3.h @@ -0,0 +1,122 @@ +#ifndef GRIDWALKER3_H +#define GRIDWALKER3_H + +#include "../../../data/Timestamp.h" +#include "../../Grid.h" +#include "../../../math/DrawList.h" +#include "../../../math/Distributions.h" +#include "../../../math/Stats.h" +#include "../../../geo/Heading.h" +#include "../../../math/stats/Variance.h" + +/** + * modular grid-walker that takes various sub-components to determine + * p(e) and thus randomly pick edges + */ +template class GridWalker3 { + +private: + + /** all modules to evaluate */ +// std::vector*> modules; + + RandomGenerator rnd; + +public: + + + /** one walk along several nodes */ + struct Walk : public std::vector { + + }; + +// struct Walks : std::vector { + +// }; + + + /** perform the walk based on the configured setup */ + const Node* getDestination(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + + const Node* startNode = grid.getNodePtrFor(start); + + Heading desiredHeading = ctrl.heading; + + DrawList walks; + + for (int i = 0; i < 100; ++i) { + const Walk walk = getRandomWalk(grid, startNode, dist_m); + const double prob = eval(walk, desiredHeading); + walks.add(walk, prob); + } + + Walk res = walks.get(); + const Node* dst = res.back(); + return dst; + + } + + double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { + const Node* nStart = w.front(); + const Node* nEnd = w.back(); + if (nStart == nEnd) { + std::cout << "warn! start == end" << std::endl; + return 0; + } + Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm); + const float diff = head.getDiffHalfRAD(desiredHeading); + return Distribution::Normal::getProbability(0, 0.3, diff); + } + + double evalHeadingChanges(const Walk& w) const { + Stats::Variance var; + for (int i = 0; i < w.size()-2; ++i) { + const Node* n0 = w[i+0]; + const Node* n1 = w[i+1]; + const Node* n2 = w[i+2]; + Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm); + Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm); + const float diff = h1.getDiffHalfRAD(h2); + var.add(diff); + } + const float totalVar = var.get(); + return Distribution::Normal::getProbability(0, 0.3, totalVar); + } + + double eval(const Walk& w, const Heading desiredHeading) const { + + return 1.0 + * evalHeadingStartEnd(w, desiredHeading) + // * evalHeadingChanges(w); + ; + + } + + Walk getRandomWalk(Grid& grid, const Node* start, const float dist_m) const { + + Walk walk; + + float dist = 0; + + const Node* cur = start; + while(true) { + + walk.push_back(cur); + if (dist > dist_m) {break;} + + const int numNeighbors = cur->getNumNeighbors(); + //std::cout << "neighbors: " << numNeighbors << std::endl; + int idx = rand() % numNeighbors; + const Node* next = &grid.getNeighbor(*cur, idx); + dist += next->inMeter().getDistance(cur->inMeter()); + cur = next; + + } + + return walk; + + } + +}; + +#endif // GRIDWALKER3_H diff --git a/math/stats/Variance.h b/math/stats/Variance.h new file mode 100644 index 0000000..334d2f0 --- /dev/null +++ b/math/stats/Variance.h @@ -0,0 +1,48 @@ +#ifndef STATS_VARIANCE_H +#define STATS_VARIANCE_H + +#include "../../Assertions.h" +#include "Average.h" + +namespace Stats { + + template class Variance { + + Average avg; + Average avg2; + + public: + + /** ctor */ + Variance() { + ; + } + + /** contains valid data? */ + bool isValid() const { + return avg.isValid(); + } + + /** add a new value */ + void add(const Scalar val) { + avg.add(val); + avg2.add(val*val); + } + + /** get the current variance */ + Scalar get() const { + Assert::isTrue(avg.isValid(), "add() values first!"); + return avg2.get() - (avg.get()*avg.get()); + } + + /** get the current stadard-deviation */ + Scalar getStdDev() const { + return std::sqrt(get); + } + + + }; + +} + +#endif // STATS_VARIANCE_H