added new helper methods
worked on gridWalker v3
This commit is contained in:
113
grid/walk/v3/GridWalk3Helper.h
Normal file
113
grid/walk/v3/GridWalk3Helper.h
Normal file
@@ -0,0 +1,113 @@
|
||||
#ifndef GRIDWALK3HELPER_H
|
||||
#define GRIDWALK3HELPER_H
|
||||
|
||||
#include "../../../nav/dijkstra/Dijkstra.h"
|
||||
#include <set>
|
||||
|
||||
|
||||
|
||||
template <typename Node> class GridWalk3Helper {
|
||||
|
||||
public:
|
||||
|
||||
/** one walk along several nodes */
|
||||
struct Walk3 : public std::vector<const Node*> {
|
||||
|
||||
};
|
||||
|
||||
struct Walks3 : std::vector<Walk3> {
|
||||
|
||||
};
|
||||
|
||||
struct Nodes3 : std::vector<const Node*> {
|
||||
|
||||
};
|
||||
|
||||
/** get all possible walks from start within a given region */
|
||||
static Walks3 getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
|
||||
|
||||
struct Access {
|
||||
Grid<Node>& grid;
|
||||
Access(Grid<Node>& grid) : grid(grid) {;}
|
||||
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
|
||||
Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
|
||||
float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
|
||||
} acc(grid);
|
||||
|
||||
const float addDist_m = grid.getGridSize_cm() / 100.0f;
|
||||
const float maxDist_m = dist_m * 1.1 + addDist_m;
|
||||
Dijkstra<Node> dijkstra;
|
||||
dijkstra.build(start, nullptr, acc, maxDist_m);
|
||||
|
||||
const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
|
||||
|
||||
Walks3 walks;
|
||||
|
||||
for (const auto& it : nodes) {
|
||||
Walk3 walk;
|
||||
DijkstraNode<Node>* node = it.second;
|
||||
do {
|
||||
const Node* gridNode = node->element;
|
||||
walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted
|
||||
node = node->previous;
|
||||
} while (node);
|
||||
walks.push_back(walk);
|
||||
}
|
||||
|
||||
return walks;
|
||||
|
||||
}
|
||||
|
||||
/** get all reachable nodes that are within a given range */
|
||||
static Nodes3 getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
|
||||
|
||||
Nodes3 res;
|
||||
std::unordered_map<uint32_t, float> distances;
|
||||
std::vector<uint32_t> toVisit;
|
||||
|
||||
toVisit.push_back(start->getIdx());
|
||||
distances[start->getIdx()] = 0.0f;
|
||||
|
||||
while (!toVisit.empty()) {
|
||||
|
||||
int curIdx = toVisit.front();
|
||||
toVisit.erase(toVisit.begin());
|
||||
|
||||
const Node& curNode = grid[curIdx];
|
||||
const float curDistance = distances[curIdx];
|
||||
res.push_back(&curNode); // remember for output
|
||||
|
||||
if (curDistance <= dist_m) {
|
||||
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
|
||||
|
||||
const int neighborIdx = curNode.getNeighborIdx(i);
|
||||
const Node& neighbor = grid[neighborIdx];
|
||||
const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
|
||||
const float totalDist = curDistance + addDist;
|
||||
|
||||
// this is like in dijkstra. keep the smallest distance to reach a node:
|
||||
|
||||
// not yet reached -> store distance
|
||||
if (distances.find(neighborIdx) == distances.end()) {
|
||||
toVisit.push_back(neighborIdx);
|
||||
distances[neighborIdx] = totalDist;
|
||||
|
||||
// reached earlier but found shorter way
|
||||
} else {
|
||||
if (distances[neighborIdx] > totalDist) {
|
||||
distances[neighborIdx] = totalDist;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // GRIDWALK3HELPER_H
|
||||
@@ -8,12 +8,14 @@
|
||||
#include "../../../math/Stats.h"
|
||||
#include "../../../geo/Heading.h"
|
||||
#include "../../../math/stats/Variance.h"
|
||||
#include "GridWalk3Helper.h"
|
||||
#include "../../../geo/BBox2.h"
|
||||
|
||||
/**
|
||||
* modular grid-walker that takes various sub-components to determine
|
||||
* p(e) and thus randomly pick edges
|
||||
*/
|
||||
template <typename Node, typename WalkState, typename Control> class GridWalker3 {
|
||||
template <typename Node> class GridWalker3 {
|
||||
|
||||
private:
|
||||
|
||||
@@ -24,38 +26,135 @@ private:
|
||||
|
||||
public:
|
||||
|
||||
|
||||
/** one walk along several nodes */
|
||||
struct Walk : public std::vector<const Node*> {
|
||||
|
||||
struct WalkParams {
|
||||
Point3 start;
|
||||
float distance_m;
|
||||
Heading heading = Heading(0);
|
||||
};
|
||||
|
||||
// struct Walks : std::vector<Walk> {
|
||||
|
||||
// };
|
||||
using Helper = GridWalk3Helper<Node>;
|
||||
using Walk = typename GridWalk3Helper<Node>::Walk3;
|
||||
using Walks = typename GridWalk3Helper<Node>::Walks3;
|
||||
using Nodes = typename GridWalk3Helper<Node>::Nodes3;
|
||||
|
||||
GridPoint p3ToGp(const Point3 p) const {
|
||||
const Point3 p100 = p*100;
|
||||
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
|
||||
}
|
||||
|
||||
Point3 gpToP3(const GridPoint gp) const {
|
||||
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
|
||||
}
|
||||
|
||||
/** perform the walk based on the configured setup */
|
||||
const Node* getDestination(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
|
||||
const Point3 getDestination(Grid<Node>& grid, const WalkParams& params) {
|
||||
//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
|
||||
return _drawThenCheck(grid, params);
|
||||
|
||||
const Node* startNode = grid.getNodePtrFor(start);
|
||||
}
|
||||
|
||||
Heading desiredHeading = ctrl.heading;
|
||||
// /** perform the walk based on the configured setup */
|
||||
// const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {
|
||||
|
||||
DrawList<Walk> walks;
|
||||
// //return _getFromPossibleWalks(grid, start, ctrl, dist_m);
|
||||
// //return _drawThenCheck(grid, start, ctrl, dist_m);
|
||||
// throw "error";
|
||||
// }
|
||||
|
||||
const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
|
||||
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
|
||||
const float d = gridSize_m / 2.0f;
|
||||
const Point2 pMin = n->inMeter().xy() - Point2(d, d);
|
||||
const Point2 pMax = n->inMeter().xy() + Point2(d, d);
|
||||
const BBox2 bbox(pMin, pMax);
|
||||
return bbox.contains(pt);
|
||||
}
|
||||
|
||||
const Point3 _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
|
||||
|
||||
const GridPoint gpStart = p3ToGp(params.start);
|
||||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||||
|
||||
static Distribution::Normal<float> dDist(1, 0.02);
|
||||
static Distribution::Normal<float> dHead(0, 0.02);
|
||||
|
||||
// include one additional grid-cell (increased distance)
|
||||
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
|
||||
const float range_m = params.distance_m + secBuffer_m;
|
||||
const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
||||
|
||||
float realDist_m = params.distance_m;
|
||||
Heading realHead = params.heading;// + dHead.draw();
|
||||
|
||||
int cnt = 0;
|
||||
while(true) {
|
||||
|
||||
const Point2 dir = realHead.asVector();
|
||||
const Point2 dst = params.start.xy() + (dir * realDist_m);
|
||||
|
||||
// is dst reachable?
|
||||
for (const Node* n : nodes) {
|
||||
//const float distToNode = n->inMeter().xy().getDistance(dst);
|
||||
//if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) {
|
||||
if (contains(grid, n, dst)) {
|
||||
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
||||
const GridPoint gp = p3ToGp(p3);
|
||||
if (grid.hasNodeFor(gp)) {
|
||||
return p3;
|
||||
} else {
|
||||
std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// before trying again, modify distance and angle
|
||||
if (1 == 0) {
|
||||
realDist_m *= dDist.draw();
|
||||
realHead += dHead.draw();
|
||||
}
|
||||
|
||||
// reached max retries?
|
||||
if (++cnt > 10) {return params.start;} // did not work out....
|
||||
|
||||
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;
|
||||
throw "error";
|
||||
|
||||
}
|
||||
|
||||
// const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
|
||||
|
||||
// const Node* startNode = grid.getNodePtrFor(start);
|
||||
|
||||
// Heading desiredHeading = ctrl.heading;
|
||||
|
||||
// DrawList<Walk> weightedWalks;
|
||||
|
||||
// const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
|
||||
// for (const Walk& walk : walks) {
|
||||
// const double prob = eval(walk, desiredHeading, dist_m);
|
||||
// weightedWalks.add(walk, prob);
|
||||
// }
|
||||
|
||||
|
||||
// Walk res = weightedWalks.get();
|
||||
// const Node* dst = res.back();
|
||||
// return dst;
|
||||
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
double evalDistance(const Walk& w, const float desiredDist) const {
|
||||
const Node* nStart = w.front();
|
||||
const Node* nEnd = w.back();
|
||||
const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
|
||||
return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
|
||||
}
|
||||
|
||||
double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
|
||||
const Node* nStart = w.front();
|
||||
const Node* nEnd = w.back();
|
||||
@@ -83,39 +182,43 @@ public:
|
||||
return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
|
||||
}
|
||||
|
||||
double eval(const Walk& w, const Heading desiredHeading) const {
|
||||
double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
|
||||
|
||||
return 1.0
|
||||
* evalHeadingStartEnd(w, desiredHeading)
|
||||
* evalDistance(w, desiredDistance)
|
||||
// * evalHeadingChanges(w);
|
||||
;
|
||||
|
||||
}
|
||||
|
||||
Walk getRandomWalk(Grid<Node>& grid, const Node* start, const float dist_m) const {
|
||||
|
||||
Walk walk;
|
||||
|
||||
float dist = 0;
|
||||
|
||||
const Node* cur = start;
|
||||
while(true) {
|
||||
// Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
|
||||
|
||||
walk.push_back(cur);
|
||||
if (dist > dist_m) {break;}
|
||||
// Walk walk;
|
||||
|
||||
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;
|
||||
// float dist = 0;
|
||||
|
||||
}
|
||||
// const Node* cur = start;
|
||||
// while(true) {
|
||||
|
||||
return walk;
|
||||
// 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;
|
||||
|
||||
// }
|
||||
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user