258 lines
6.9 KiB
C++
258 lines
6.9 KiB
C++
#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"
|
|
#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> class GridWalker3 {
|
|
|
|
private:
|
|
|
|
/** all modules to evaluate */
|
|
// std::vector<WalkModule<Node, WalkState>*> modules;
|
|
|
|
RandomGenerator rnd;
|
|
|
|
public:
|
|
|
|
/** paremters for the walk */
|
|
struct WalkParams {
|
|
Point3 start;
|
|
float distance_m;
|
|
Heading heading = Heading(0);
|
|
};
|
|
|
|
/** result of the random walk */
|
|
struct WalkResult {
|
|
Point3 position;
|
|
Heading heading = Heading(0);
|
|
double probability = 1.0;
|
|
};
|
|
|
|
|
|
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 WalkResult 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);
|
|
|
|
}
|
|
|
|
// /** perform the walk based on the configured setup */
|
|
// const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {
|
|
|
|
// //return _getFromPossibleWalks(grid, start, ctrl, dist_m);
|
|
// //return _drawThenCheck(grid, start, ctrl, dist_m);
|
|
// throw "error";
|
|
// }
|
|
|
|
/** does the given grid-node contain the provided point-in-question? */
|
|
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);
|
|
}
|
|
|
|
/** does one of the given grid-node scontains the provided point-in-question? */
|
|
const Node* contains(const Grid<Node>& grid, const Nodes& nodes, Point2 pt) {
|
|
for (const Node* n : nodes) {
|
|
if (contains(grid, n, pt)) {return n;}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
const WalkResult _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.01);
|
|
|
|
// 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 reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
|
|
|
WalkResult res;
|
|
res.heading = params.heading;
|
|
res.position = params.start;
|
|
float realDist_m = params.distance_m;
|
|
|
|
int cnt = 0;
|
|
while(true) {
|
|
|
|
const Point2 dir = res.heading.asVector();
|
|
const Point2 dst = params.start.xy() + (dir * realDist_m);
|
|
|
|
// is dst reachable?
|
|
const Node* n = contains(grid, reachableNodes, dst);
|
|
if (n) {
|
|
|
|
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
|
const GridPoint gp = p3ToGp(p3);
|
|
|
|
if (grid.hasNodeFor(gp)) {
|
|
res.position = p3; // update position
|
|
res.heading; // keep as-is
|
|
res.probability; // keep as-is
|
|
return res; // done
|
|
} else {
|
|
std::cout << "WARN dst not found" << std::endl;
|
|
//throw "should not happen";
|
|
}
|
|
|
|
}
|
|
|
|
// reduce probability with every new run
|
|
++cnt;
|
|
res.probability /= 2;
|
|
|
|
// before trying again, modify distance and angle
|
|
//if (1 == 1) {
|
|
res.heading = params.heading + dHead.draw() * cnt;
|
|
realDist_m = params.distance_m * dDist.draw();// * cnt;
|
|
//}
|
|
|
|
// reached max retries?
|
|
if (cnt > 8) {
|
|
res.position = params.start; // reset
|
|
res.heading = params.heading; // reset
|
|
res.probability = 1e-50; // unlikely
|
|
return res;
|
|
} // did not work out....
|
|
|
|
}
|
|
|
|
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();
|
|
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<double>::getProbability(0, 0.3, diff);
|
|
}
|
|
|
|
double evalHeadingChanges(const Walk& w) const {
|
|
Stats::Variance<float> 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<double>::getProbability(0, 0.3, totalVar);
|
|
}
|
|
|
|
double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
|
|
|
|
return 1.0
|
|
* evalHeadingStartEnd(w, desiredHeading)
|
|
* evalDistance(w, desiredDistance)
|
|
// * evalHeadingChanges(w);
|
|
;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Walk getRandomWalk2(Grid<Node>& 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
|