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

worked on synthetic sensors

Browse Source 
worked on grid-walker
minor changes/fixes/improvements
This commit is contained in:
k-a-z-u
2017-10-18 16:54:57 +02:00
parent 72f083d32a
commit 3e31f6da53
8 changed files with 522 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
grid/walk/v3/GridWalker3.h
View File

@@ -37,6 +37,7 @@ public:
struct WalkResult {
Point3 position;
Heading heading = Heading(0);
double probability = 1.0;
};
@@ -79,18 +80,26 @@ public:
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.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 nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
WalkResult res;
res.heading = params.heading;
@@ -104,33 +113,39 @@ public:
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)) {
res.position = p3; // new position
res.heading; // keep as-is
return res;
} else {
std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
}
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 == 0) {
realDist_m *= dDist.draw();
res.heading += dHead.draw();
}
//if (1 == 1) {
res.heading = params.heading + dHead.draw() * cnt;
realDist_m = params.distance_m * dDist.draw();// * cnt;
//}
// reached max retries?
if (++cnt > 10) {
WalkResult res;
res.position = params.start;
res.heading = params.heading;
if (cnt > 8) {
res.position = params.start; // reset
res.heading = params.heading; // reset
res.probability = 1e-50; // unlikely
return res;
} // did not work out....