From 6346231a647d6aed6ce94923a813a362982367d7 Mon Sep 17 00:00:00 2001 From: FrankE Date: Sun, 7 Feb 2016 13:32:19 +0100 Subject: [PATCH] added exponential distrbution + tests new shortest-path walker (still todo) modified dijkstra-path (new helper methods) --- grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 6 +- grid/walk/GridWalkShortestPathControl.h | 266 +++++++++++++++++++ math/distribution/Exponential.h | 48 ++++ nav/dijkstra/DijkstraPath.h | 6 +- tests/math/TestDistribution.cpp | 18 ++ 5 files changed, 339 insertions(+), 5 deletions(-) create mode 100644 grid/walk/GridWalkShortestPathControl.h create mode 100644 tests/math/TestDistribution.cpp diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index 66756d0..703ead9 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -159,9 +159,9 @@ private: next.distanceWalked_m += walked_m; ++((T*)next.node)->cnt; // TODO: eval only - if (next.node->z_cm != cur.node->z_cm) { - int i = 0; - } +// if (next.node->z_cm != cur.node->z_cm) { +// int i = 0; +// } diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h new file mode 100644 index 0000000..a2f196b --- /dev/null +++ b/grid/walk/GridWalkShortestPathControl.h @@ -0,0 +1,266 @@ +#ifndef GRIDWALKSHORTESTPATHCONTROL_H +#define GRIDWALKSHORTESTPATHCONTROL_H + + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/Distributions.h" +#include "../../math/DrawList.h" + +#include "../../nav/dijkstra/Dijkstra.h" +#include "../../nav/dijkstra/DijkstraPath.h" +#include "../../misc/KNN.h" +#include "../../misc/KNNArray.h" + +#include "GridWalkState.h" +#include "GridWalkHelper.h" +#include "GridWalk.h" + +template class GridWalkShortestPathControl : public GridWalk { + + +protected: + + /** put dijkstra-nodes into the KNN */ + class Wrapper { + + private: + + DijkstraPath* path; + + public: + + Wrapper() : path(nullptr) {;} + + /** ctor with the underlying data structure */ + Wrapper(DijkstraPath* path) : path(path) { + ; + } + + /** get the number of elements to search throrugh */ + inline int kdtree_get_point_count() const { + return path->size(); + } + + /** use nanoflanns default bbox */ + template inline bool kdtree_get_bbox(BBOX& bb) const { + (void) bb; return false; + } + + /** get the idx-th element's dim-th coordinate */ + inline float kdtree_get_pt(const size_t idx, const int dim) const { + return (*((*path)[idx]).element)[dim]; + } + + /** get the SQUARED distance between the given coordinates and the provided element */ + inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const { + const float d0 = p1[0] - (*((*path)[idx_p2]).element)[0]; + const float d1 = p1[1] - (*((*path)[idx_p2]).element)[1]; + const float d2 = p1[2] - (*((*path)[idx_p2]).element)[2]; + return (d0*d0) + (d1*d1) + (d2*d2); + } + + + }; + + + + friend class GridWalkHelper; + +protected: + + /** per-edge: change heading with this sigma */ + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); + + /** fast random-number-generator */ + std::minstd_rand gen; + + /** 0-mean normal distribution */ + std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); + + Dijkstra dijkstra; + const T& target; + + Point3 centerOfMass = Point3(0,0,0); + Wrapper wrapper; + DijkstraPath* path = nullptr; + KNN* knn = nullptr; + + + DrawList drawer; + + +public: + + + + /** ctor with the target you want to reach */ + template GridWalkShortestPathControl(Grid& grid, const Access& acc, const T& target) : target(target) { + + (void) grid; + + gen.seed(1234); + + // build all shortest path to reach th target + dijkstra.build(target, target, acc); + + } + + int recalc = 0; + + GridWalkState getDestination(Grid& grid, const GridWalkState& 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) { + recalc = 0; + rebuildPath(grid); + } + +// 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) { +// rebuildPath(grid); +// } +// } + + // proportional change of the heading + static Distribution::Normal dHead(1, 0.01); + + // proportional change of the to-be-walked distance + static Distribution::Normal dWalk(1, 0.10); + + distance_m = distance_m*dWalk.draw()*2; // TODO: why *2? + headChange_rad = headChange_rad*dHead.draw(); + + + return walk(grid, start, distance_m, headChange_rad); + + } + +private: + + double getProbability(const T& start, 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; + return 0.5;// stair start/end TODO: fix + } + + // get the angle between START and the possible next node + const Heading possibleHead = GridWalkHelper::getHeading(start, possible); + + // calculate the difference + const float diff = possibleHead.getDiffHalfRAD(head); + +// // compare this heading with the requested one + const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(25), diff); +// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally + + // 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; + //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::getProbability(0.15, pd_m); + } + + // bring it together + return angleProb * nodeProb; + + } + + GridWalkState walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) { + + // try-again distribution + //static Distribution::Normal dHead(0, Angle::degToRad(10)); + //static Distribution::Normal dUpdate(0, Angle::degToRad(3)); + static Distribution::Uniform dChange(Angle::degToRad(0), +Angle::degToRad(359)); + + int retries = 5; + float walked_m = 0; + GridWalkState cur = start; + + // the desired heading + Heading reqHeading = start.heading + (headChange_rad); + + // walk until done + while(walked_m < distance_m) { + + // evaluate all neighbors + drawer.reset(); + for (T& neighbor : grid.neighbors(*cur.node)) { + + const double prob = getProbability(*start.node, neighbor, reqHeading); + drawer.add(neighbor, prob); + + } + + // too bad? start over! + if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) { + walked_m = 0; + cur = start; + //WHAT THE HELL + if (retries == 0) { reqHeading = dChange.draw(); } + continue; + } + + // get the neighbor + const T& neighbor = drawer.get(); + + // update + walked_m += neighbor.getDistanceInMeter(*cur.node); + cur.node = &neighbor; + + } + + cur.distanceWalked_m = NAN; + cur.headingChange_rad = NAN; + cur.heading = reqHeading; + + return cur; + + } + + /** rebuild the path for the given center point */ + void rebuildPath(Grid& grid) { + + // 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; + + DijkstraNode* dnTarget = dijkstra.getNode(target); + DijkstraNode* dnStart = dijkstra.getNode(currentMass); + + if (dnStart == nullptr) { + return; + } + + // calculate the path from there to the target + try { + path = new DijkstraPath(dnStart, dnTarget); + + // create k-nn lookup + wrapper = Wrapper(path); + knn = new KNN(wrapper); + } catch (...) { + knn = nullptr; + path = nullptr; + } + + } + + +}; + +#endif // GRIDWALKSHORTESTPATHCONTROL_H diff --git a/math/distribution/Exponential.h b/math/distribution/Exponential.h index 8af34d6..f00f410 100644 --- a/math/distribution/Exponential.h +++ b/math/distribution/Exponential.h @@ -1,4 +1,52 @@ #ifndef EXPONENTIAL_H #define EXPONENTIAL_H +#include +#include + + +namespace Distribution { + + /** exponential distribution */ + template class Exponential { + + private: + + const T lambda; + + std::minstd_rand gen; + std::exponential_distribution dist; + + public: + + /** ctor */ + Exponential(const T lambda) : lambda(lambda), dist(lambda) { + ; + } + + /** get probability for the given value */ + T getProbability(const T val) const { + return getProbability(lambda, val); + } + + /** get an exponentially distributed random number */ + T draw() { + return dist(gen); + } + + /** set the seed to use */ + void setSeed(const long seed) { + gen.seed(seed); + } + + + /** get the probability for the given value */ + static T getProbability(const T lambda, const T val) { + return (val < 0) ? (0) : (lambda * std::exp( -lambda * val )); + } + + }; + +} + #endif // EXPONENTIAL_H diff --git a/nav/dijkstra/DijkstraPath.h b/nav/dijkstra/DijkstraPath.h index fbcb34f..1e92823 100644 --- a/nav/dijkstra/DijkstraPath.h +++ b/nav/dijkstra/DijkstraPath.h @@ -43,8 +43,10 @@ public: } /** allow iteration */ - decltype(path.begin()) begin() {return path.begin();} - decltype(path.end()) end() {return path.end();} + decltype(path.begin()) begin() {return path.begin();} + decltype(path.end()) end() {return path.end();} + const DijkstraNode& operator [] (const int idx) const {return *(path[idx]);} + size_t size() const {return path.size();} /** NANOFLANN: number of elements in the path */ diff --git a/tests/math/TestDistribution.cpp b/tests/math/TestDistribution.cpp new file mode 100644 index 0000000..5498f2f --- /dev/null +++ b/tests/math/TestDistribution.cpp @@ -0,0 +1,18 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/Distributions.h" + +TEST(Distribution, Exp) { + + ASSERT_NEAR(1.5, Distribution::Exponential::getProbability(1.5, 0.0), 0.001); + ASSERT_NEAR(1.0, Distribution::Exponential::getProbability(1.0, 0.0), 0.001); + ASSERT_NEAR(0.5, Distribution::Exponential::getProbability(0.5, 0.0), 0.001); + + ASSERT_NEAR(0.35, Distribution::Exponential::getProbability(1.5, 1.0), 0.05); + ASSERT_NEAR(0.35, Distribution::Exponential::getProbability(1.0, 1.0), 0.05); + ASSERT_NEAR(0.35, Distribution::Exponential::getProbability(0.5, 1.0), 0.05); + +} + +#endif