FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on grid walker

Browse Source
This commit is contained in:
k-a-z-u
2017-11-15 16:41:57 +01:00
parent 08d8292976
commit 7af5131ccf
4 changed files with 190 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
grid/walk/v3/Helper.h
View File

@@ -84,43 +84,67 @@ namespace GW3 {
}; };
/**
* data-structure to track to-be-visited nodes
* push_back, pop_front
* as pop_front is costly, we omit the pop and use a head-index instead
* memory-consumption vs speed
*/
struct ToVisit {
size_t nextIdx = 0;
std::vector<uint32_t> vec;
ToVisit() {vec.reserve(256);}
void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
uint32_t next() {return vec[nextIdx++];}
bool empty() const {return nextIdx >= vec.size();}
};
/** get an iterator over all nodes reachable from the given start */ /** get an iterator over all nodes reachable from the given start */
template <typename Node> class ReachableIteratorUnsorted { template <typename Node, typename Conditions> class ReachableIteratorUnsorted {
const Grid<Node>& grid; const Grid<Node>& grid;
const Node& start; const Node& start;
Node* curNode = nullptr; Node* curNode = nullptr;
std::unordered_set<uint32_t> visited; std::unordered_set<uint32_t> visited;
std::vector<uint32_t> toVisit; ToVisit toVisit;
Conditions cond;
public: public:
ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) { ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) {
toVisit.push_back(start.getIdx()); toVisit.add(start.getIdx());
} }
bool hasNext() const { bool hasNext() const {
return !toVisit.empty(); return !toVisit.empty();
} }
const Node& next(const std::function<bool(const Node&)>& skip) { //const Node& next(const std::function<bool(const Node&)>& skip) {
//template <typename Skip> const Node& next(const Skip skip) {
const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance) const Node& next() {
toVisit.erase(toVisit.begin());
visited.insert(curIdx);
// get the next to-be-visited node
const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance)
const Node& curNode = grid[curIdx]; const Node& curNode = grid[curIdx];
for (int i = 0; i < curNode.getNumNeighbors(); ++i) { // mark as "visited"
visited.insert(curIdx);
const int neighborIdx = curNode.getNeighborIdx(i); // get all neighbors
const int numNeighbors = curNode.getNumNeighbors();
for (int i = 0; i < numNeighbors; ++i) {
const uint32_t neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx]; const Node& neighbor = grid[neighborIdx];
const bool visit = cond.visit(neighbor) ;
// not yet reached -> store distance // not yet reached -> store distance
if (!skip(neighbor)) { if (visit) {
if (visited.find(neighborIdx) == visited.end()) { if (visited.find(neighborIdx) == visited.end()) {
toVisit.push_back(neighborIdx); toVisit.add(neighborIdx);
} }
} }

27
grid/walk/v3/WalkEvaluator.h
View File

@@ -39,7 +39,8 @@ namespace GW3 {
const GridPoint gp = Helper<Node>::p3ToGp(pEnd); const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
const Node& node = grid->getNodeFor(gp); const Node& node = grid->getNodeFor(gp);
const double p = node.getWalkImportance(); const double p = node.getWalkImportance();
return std::pow(p,10); return p;
//return std::pow(p,10);
} }
@@ -50,11 +51,19 @@ namespace GW3 {
/** evaluate the difference between head(start,end) and the requested heading */ /** evaluate the difference between head(start,end) and the requested heading */
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> { template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
const double sigma; const double sigma_rad;
const double kappa;
Distribution::VonMises<double> _dist;
Distribution::LUT<double> dist;
public: public:
WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;} // kappa = 1/var = 1/sigma^2
// https://en.wikipedia.org/wiki/Von_Mises_distribution
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
;
}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
@@ -68,7 +77,8 @@ namespace GW3 {
const Heading head(pStart.xy(), pEnd.xy()); const Heading head(pStart.xy(), pEnd.xy());
const float diff = head.getDiffHalfRAD(params.heading); const float diff = head.getDiffHalfRAD(params.heading);
//const float diff = Heading::getSignedDiff(params.heading, head); //const float diff = Heading::getSignedDiff(params.heading, head);
return Distribution::Normal<double>::getProbability(0, sigma, diff); //return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
} }
@@ -79,14 +89,19 @@ namespace GW3 {
const double sigma; const double sigma;
const Distribution::Normal<double> dist;
public: public:
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;} WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
const float requestedDistance_m = params.distance_m;
const float walkedDistance_m = pStart.getDistance(pEnd); const float walkedDistance_m = pStart.getDistance(pEnd);
return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m); const float diff = walkedDistance_m - requestedDistance_m;
return dist.getProbability(diff);
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
} }

173
grid/walk/v3/Walker.h
View File

@@ -26,18 +26,22 @@ namespace GW3 {
public: public:
/** get a new destination for the given start */ /** get a new destination for the given start */
virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0; virtual const WalkResult getDestination(const WalkParams& params) const = 0;
}; };
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> { template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
//RandomGenerator rnd; Grid<Node>& grid;
std::vector<WalkEvaluator<Node>*> evals; std::vector<WalkEvaluator<Node>*> evals;
public: public:
/** ctor */
WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
;
}
/** make the code a little more readable */ /** make the code a little more readable */
using Helper = GW3::Helper<Node>; using Helper = GW3::Helper<Node>;
using Walk = typename GW3::Walk<Node>; using Walk = typename GW3::Walk<Node>;
@@ -50,7 +54,7 @@ namespace GW3 {
} }
/** perform the walk based on the configured setup */ /** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override { const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.distance_m, "walking distance must be > 0"); Assert::isNot0(params.distance_m, "walking distance must be > 0");
@@ -142,8 +146,20 @@ namespace GW3 {
std::vector<WalkEvaluator<Node>*> evals; std::vector<WalkEvaluator<Node>*> evals;
Grid<Node>& grid;
const float gridSize_m;
mutable std::minstd_rand rndGen;
mutable std::uniform_real_distribution<float> dFinal;
public: public:
/** ctor */
WalkerWeightedRandom(Grid<Node>& grid) :
grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f) {
;
}
/** make the code a little more readable */ /** make the code a little more readable */
using Helper = GW3::Helper<Node>; using Helper = GW3::Helper<Node>;
using Walk = typename GW3::Walk<Node>; using Walk = typename GW3::Walk<Node>;
@@ -156,79 +172,71 @@ namespace GW3 {
} }
/** perform the walk based on the configured setup */ /** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override { const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.distance_m, "walking distance must be > 0"); Assert::isNot0(params.distance_m, "walking distance must be > 0");
static std::minstd_rand rndGen;
const GridPoint gpStart = Helper::p3ToGp(params.start); const GridPoint gpStart = Helper::p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart); const Node* startNode = grid.getNodePtrFor(gpStart);
if (!startNode) {throw Exception("start node not found!");}
// // include one additional grid-cell (increased distance) const float maxDist = params.distance_m + gridSize_m;
// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
// ReachableSettings set; Point3 best; double bestP = 0;
// set.limitDistance = true; //DrawList<Point3> drawer;
// set.limitHeading = true;
// set.dist_m = params.distance_m + secBuffer_m;
// set.heading = params.heading;
// set.maxHeadingDiff_rad = M_PI/2;
// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
Point3 best;
double bestP = 0;
// DrawList<Point3> drawer;
const Point3 start = params.start; const Point3 start = params.start;
// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
//for (int i = 0; i < 500; ++i) {
// const Node* dstNode = reachableNodes[dNode(rndGen)];
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
struct RICond {
ReachableIteratorUnsorted<Node> ri(grid, *startNode); const GridPoint gpStart;
const float maxDist = params.distance_m * 1.25 + gridSize_m; const float maxDist;
RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;}
auto skip = [&] (const Node& n) { bool visit (const Node& n) const {
const float dist_m = n.getDistanceInMeter(gpStart); const float dist_m = n.getDistanceInMeter(gpStart);
return dist_m > maxDist; return dist_m < maxDist;
}
}; };
RICond riCond(gpStart, maxDist);
//for (const Node* dstNode : reachableNodes) { // iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance)
while(ri.hasNext()) { ReachableIteratorUnsorted<Node, RICond> ri(grid, *startNode, riCond);
const Node* dstNode = &ri.next(skip); int numVisitedNodes = 0;
// const float dist_m = dstNode->getDistanceInMeter(gpStart);
// if (dist_m > maxDist) {
// break;
// }
for (int i = 0; i < 25; ++i) { #define MODE 1
#if (MODE == 1)
double bestNodeP = 0;
const Node* bestNode = nullptr;
while(ri.hasNext()) {
const Node* dstNode = &ri.next();
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
double p = 1.0;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, nodeCenter, params);
p *= p1;
}
if (p > bestNodeP) {
bestNodeP = p;
bestNode = dstNode;
}
}
for (int i = 0; i < 10; ++i) {
const Point3 nodeCenter = Helper::gpToP3(*bestNode);
// random position within destination-node // random position within destination-node
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); const float ox = dFinal(rndGen);
const float oy = dFinal(rndGen);
// destination = node-center + offset (within the node's bbox) // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
// sanity check
if (start == end) {continue;}
if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
std::cout << "random destination is not part of the grid" << std::endl;
continue;
}
//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
double p = 1; double p = 1;
for (const WalkEvaluator<Node>* eval : evals) { for (const WalkEvaluator<Node>* eval : evals) {
@@ -237,20 +245,63 @@ namespace GW3 {
} }
if (p > bestP) {bestP = p; best = end;} if (p > bestP) {bestP = p; best = end;}
//drawer.add(end, p);
} }
} #elif (MODE == 2)
//const Point3 end = drawer.get(); // all reachable nodes
while(ri.hasNext()) {
const Node* dstNode = &ri.next();
++numVisitedNodes;
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
// try multiple locations within each reachable node
for (int i = 0; i < 1; ++i) {
// random position within destination-node
const float ox = dFinal(rndGen);
const float oy = dFinal(rndGen);
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
// sanity check
if (start == end) {continue;}
// if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
// std::cout << "random destination is not part of the grid" << std::endl;
// continue;
// }
//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
// drawer.add(end, p);
}
}
#endif
//std::cout << numVisitedNodes << std::endl;
//double drawProb = 0; const Point3 end = drawer.get(drawProb);
const Point3 end = best; const Point3 end = best;
WalkResult res; WalkResult res;
if (start == end) { if (start == end) {
res.probability = 0; res.probability = 0;
} else { } else {
res.heading = Heading(start.xy(), end.xy()); res.heading = Heading(start.xy(), end.xy());
res.probability = bestP; //res.probability = drawProb; // when using DrawList
res.probability = bestP; // when using bestP
} }
res.position = end; res.position = end;
return res; return res;

32
synthetic/SyntheticTurns.h
View File

@@ -7,7 +7,7 @@
#include "../sensors/imu/GyroscopeData.h" #include "../sensors/imu/GyroscopeData.h"
#include "../geo/Heading.h" #include "../geo/Heading.h"
#include "../math/distribution/Normal.h" #include "../math/Distributions.h"
/** /**
* simulates acceleromter and gyroscope data * simulates acceleromter and gyroscope data
@@ -41,6 +41,8 @@ private:
Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25); Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased
Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
std::vector<Listener*> listeners; std::vector<Listener*> listeners;
@@ -68,7 +70,7 @@ protected:
Heading desiredHead = Heading(0); Heading desiredHead = Heading(0);
Heading curHead = Heading(0); Heading curHead = Heading(0);
Point3 lastPos = Point3(NAN, NAN, NAN); Point3 lastPos = Point3(NAN, NAN, NAN);
float change; double change = 0;
inline float clamp(const float val, const float min, const float max) { inline float clamp(const float val, const float min, const float max) {
if (val < min) {return min;} if (val < min) {return min;}
@@ -86,27 +88,33 @@ protected:
if (lastPos.x != lastPos.x) { if (lastPos.x != lastPos.x) {
lastPos = curPos; lastPos = curPos;
} else { } else {
desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();; desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();
lastPos = curPos; lastPos = curPos;
} }
// difference between current-heading and desired-heading // difference between current-heading and desired-heading
const float diffRad = Heading::getSignedDiff(curHead, desiredHead); const double maxChange = dMaxChange.draw();
const double diffRad = Heading::getSignedDiff(curHead, desiredHead);
//change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange);
change = clamp(diffRad / 25, -maxChange, +maxChange);
// slowly change the current heading to match the desired one
const float maxChange = dMaxChange.draw(); // // slowly change the current heading to match the desired one
const float toChange = clamp(diffRad, -maxChange, +maxChange); // //const double maxChange = dMaxChange.draw();
//if (change < toChange) {change += toChange*0.01;} // //const double toChange = clamp(diffRad, -maxChange, +maxChange);
if (change > toChange) {change *= 0.93;} // const double toChange = diffRad;
if (change < toChange) {change += dChange.draw()/10000;} // //if (change < toChange) {change += toChange*0.01;}
//if (change > toChange) {change -= dChange.draw();} // if (change > toChange) {change *= 0.93;}
// //if (change < toChange) {change += dChange.draw()/10000;} // does not work for small changes?!
// if (change < toChange) {change += (toChange-change) * 0.07;}
// //if (change > toChange) {change -= dChange.draw();}
curHead += change; curHead += change;
// convert to gyro's radians-per-second // convert to gyro's radians-per-second
const float radPerSec = change * 1000 / deltaTs.ms();; const double radPerSec = change * 1000 / deltaTs.ms();;
const float accX = 0.00 + dAccX.draw(); const float accX = 0.00 + dAccX.draw();
const float accY = 0.00 + dAccY.draw(); const float accY = 0.00 + dAccY.draw();