From 382a046df1f1d36d854b8194ad76adc3eac4e473 Mon Sep 17 00:00:00 2001 From: FrankE Date: Fri, 5 Feb 2016 20:18:48 +0100 Subject: [PATCH] new walker (control+path) added new sanity checks fixed minor errors added corresponding test-cases added moving-median --- grid/Grid.h | 13 +- grid/factory/GridFactory.h | 8 +- grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 2 + grid/walk/GridWalkPathControl.h | 161 +++++++++++++++++++ grid/walk/GridWalkSimpleControl.h | 14 +- math/MovingMedian.h | 65 ++++++++ math/TestMovingMedian.h | 6 + tests/grid/Plot.h | 1 + tests/grid/TestGrid.cpp | 24 ++- tests/math/TestMovingMedian.cpp | 22 +++ 10 files changed, 296 insertions(+), 20 deletions(-) create mode 100644 grid/walk/GridWalkPathControl.h create mode 100644 math/MovingMedian.h create mode 100644 math/TestMovingMedian.h create mode 100644 tests/math/TestMovingMedian.cpp diff --git a/grid/Grid.h b/grid/Grid.h index dc4e9fb..f95d74b 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -78,12 +78,13 @@ public: /** add the given (not necessarly aligned) element to the grid */ int addUnaligned(const T& elem) { - const int idx = nodes.size(); // next free index - const UID uid = getUID(elem); // get the UID for this new element - nodes.push_back(elem); // add it to the grid - nodes.back()._idx = idx; - hashes[uid] = idx; // add an UID->index lookup - return idx; // done + const UID uid = getUID(elem); // get the UID for this new element + Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!"); // avoid potential errors + const int idx = nodes.size(); // next free index + nodes.push_back(elem); // add it to the grid + nodes.back()._idx = idx; // let the node know his own index + hashes[uid] = idx; // add an UID->index lookup + return idx; // done } /** connect (uni-dir) i1 -> i2 */ diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h index 74e7257..71dd254 100755 --- a/grid/factory/GridFactory.h +++ b/grid/factory/GridFactory.h @@ -199,7 +199,7 @@ public: void buildStairLine(T& _n1, T& _n2) { // half the grid size = small steps - const int gridSize_cm = grid.getGridSize_cm() / 2; + const int gridSize_cm = grid.getGridSize_cm();// / std::sqrt(2); // local copies, needed for std::swap to work T n1 = _n1; T n2 = _n2; @@ -216,7 +216,7 @@ public: const int idx3 = n2.getIdx(); // final node // move upards in gridSize steps - for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) { + for (int _z = gridSize_cm; _z < zDiff - gridSize_cm; _z+= gridSize_cm) { // calculate the percentage of reached upwards-distance const float percent = _z/zDiff; @@ -227,8 +227,8 @@ public: int z = n1.z_cm + _z; // snap (x,y) to the grid??? - x = std::round(x / gridSize_cm) * gridSize_cm; - y = std::round(y / gridSize_cm) * gridSize_cm; + //x = std::round(x / gridSize_cm) * gridSize_cm; + //y = std::round(y / gridSize_cm) * gridSize_cm; // create a new node add it to the grid, and connect it with the previous one idx2 = grid.addUnaligned(T(x,y,z)); diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index d5b4815..66756d0 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -46,6 +46,8 @@ public: /** ctor with the target you want to reach */ template GridWalkLightAtTheEndOfTheTunnel(Grid& grid, const Access& acc, const T& target) { + gen.seed(1234); + // build all shortest path to reach th target dijkstra.build(target, target, acc); diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h new file mode 100644 index 0000000..4ac88bf --- /dev/null +++ b/grid/walk/GridWalkPathControl.h @@ -0,0 +1,161 @@ +#ifndef GRIDWALKPATHCONTROL_H +#define GRIDWALKPATHCONTROL_H + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/Distributions.h" +#include "../../math/DrawList.h" + +#include "../../nav/dijkstra/Dijkstra.h" + +#include "GridWalkState.h" +#include "GridWalkHelper.h" +#include "GridWalk.h" + +template class GridWalkPathControl : public GridWalk { + + friend class GridWalkHelper; + +private: + + /** 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; + + DrawList drawer; + +public: + + + /** ctor with the target you want to reach */ + template GridWalkPathControl(Grid& grid, const Access& acc, const T& target) { + + gen.seed(1234); + + // build all shortest path to reach th target + dijkstra.build(target, target, acc); + + // attach a corresponding weight-information to each user-grid-node + for (T& node : grid) { + + const DijkstraNode* dn = dijkstra.getNode(node); + + // should never be null as all nodes were evaluated + if (dn != nullptr) { + node.distToTarget = dn->cumWeight/2000; + } + + } + + } + + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + + // 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(15), diff); +// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally + + // nodes own importance + const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.0; + + // 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; + + } + +}; + + + +#endif // GRIDWALKPATHCONTROL_H diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index 636646f..333dbe0 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -44,12 +44,12 @@ public: GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { // proportional change of the heading - static Distribution::Normal dHead(1, 0.10); + static Distribution::Normal dHead(1, 0.01); // proportional change of the to-be-walked distance - static Distribution::Normal dWalk(1, 0.10); + static Distribution::Normal dWalk(1, 0.50); - distance_m = distance_m*dWalk.draw(); // TODO: why *2? + distance_m = distance_m*dWalk.draw()*2; // TODO: why *2? headChange_rad = headChange_rad*dHead.draw(); @@ -62,7 +62,11 @@ 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) {return 0.1;} + 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); @@ -71,7 +75,7 @@ private: const float diff = possibleHead.getDiffHalfRAD(head); // // compare this heading with the requested one - const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(30), diff); + const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(15), diff); // const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally // nodes own importance diff --git a/math/MovingMedian.h b/math/MovingMedian.h new file mode 100644 index 0000000..9ad4cdd --- /dev/null +++ b/math/MovingMedian.h @@ -0,0 +1,65 @@ +#ifndef MOVINGMEDIAN_H +#define MOVINGMEDIAN_H + +#include +#include + +template class MovingMedian { + +private: + + /** up to "size" elements */ + std::vector values; + + /** the number of elements to find the median within */ + int size; + +public: + + /** ctor */ + MovingMedian(const int size) : size(size) {;} + + /** add a new value */ + void add(const T val) { + + // add new value + values.push_back(val); + + // too many values? + if ((int) values.size() > size) { + values.erase(values.begin()); + } + + } + + /** get the current median */ + T get() const { + + // create a sorted copy (slow, but works) + std::vector copy = values; + std::sort(copy.begin(), copy.end()); + + // get the median + if (values.size() % 2 != 0) { + return values[(values.size() + 0) / 2]; + } else { + return (values[values.size() / 2 - 1] + values[values.size() / 2 + 0]) / 2; + } + + } + + + /** get the number of used entries */ + int getNumUsed() const { + return (int) values.size(); + } + + /** get number of entries to get the median for */ + int getSize() const { + return size; + } + + +}; + +#endif // MOVINGMEDIAN_H diff --git a/math/TestMovingMedian.h b/math/TestMovingMedian.h new file mode 100644 index 0000000..27f2d31 --- /dev/null +++ b/math/TestMovingMedian.h @@ -0,0 +1,6 @@ +#ifndef TESTMOVINGMEDIAN_H +#define TESTMOVINGMEDIAN_H + + + +#endif // TESTMOVINGMEDIAN_H diff --git a/tests/grid/Plot.h b/tests/grid/Plot.h index a678d2f..7bdac50 100644 --- a/tests/grid/Plot.h +++ b/tests/grid/Plot.h @@ -17,6 +17,7 @@ public: float imp = 1.0f; float impPath = 1.0f; float distToTarget = 1.0; + int cnt = 0; public: GP() : GridNode(), GridPoint() {;} GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;} diff --git a/tests/grid/TestGrid.cpp b/tests/grid/TestGrid.cpp index 14c14bc..312a57c 100755 --- a/tests/grid/TestGrid.cpp +++ b/tests/grid/TestGrid.cpp @@ -6,23 +6,37 @@ #include "../../grid/GridPoint.h" #include "../../grid/GridNode.h" +TEST(Grid, addTwice) { + + Grid grid(20); + ASSERT_EQ(0, grid.add(GP(0,0,0))); + ASSERT_EQ(1, grid.add(GP(20,20,20))); + + // already present -> error + ASSERT_THROW(grid.add(GP(0,0,0)), std::exception); + ASSERT_THROW(grid.add(GP(20,20,20)), std::exception); + + ASSERT_EQ(2, grid.add(GP(20,20,40))); + + // already present -> error + ASSERT_THROW(grid.add(GP(20,20,40)), std::exception); + +} + TEST(Grid, add) { Grid grid(20); ASSERT_EQ(0, grid.add(GP())); - ASSERT_EQ(1, grid.add(GP())); - ASSERT_EQ(2, grid.add(GP())); - ASSERT_EQ(3, grid.add(GP())); // not aligned? -> error ASSERT_THROW(grid.add(GP(10,10,10)), std::exception); ASSERT_THROW(grid.add(GP(10,20,20)), std::exception); ASSERT_THROW(grid.add(GP(20,10,20)), std::exception); ASSERT_THROW(grid.add(GP(20,20,10)), std::exception); - ASSERT_EQ(4, grid.add(GP(20,20,20))); + ASSERT_EQ(1, grid.add(GP(20,20,20))); // access - ASSERT_EQ(GP(20,20,20), grid[4]); + ASSERT_EQ(GP(20,20,20), grid[1]); } diff --git a/tests/math/TestMovingMedian.cpp b/tests/math/TestMovingMedian.cpp new file mode 100644 index 0000000..d4d72da --- /dev/null +++ b/tests/math/TestMovingMedian.cpp @@ -0,0 +1,22 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/MovingMedian.h" + +TEST(MovingMedian, add) { + + MovingMedian med(3); + + med.add(1); ASSERT_EQ(1, med.get()); + med.add(2); ASSERT_EQ(1.5, med.get()); + med.add(3); ASSERT_EQ(2, med.get()); + + med.add(3); ASSERT_EQ(3, med.get()); + med.add(1); ASSERT_EQ(3, med.get()); + med.add(1); ASSERT_EQ(1, med.get()); + med.add(2); ASSERT_EQ(1, med.get()); + med.add(3); ASSERT_EQ(2, med.get()); + +} + +#endif