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

many changes and updates

Browse Source 
- changed the wifi-estimation api
- adjusted test-cases
- worked on grid-bulding and grid-importance
- new walking modules
- fixed some minor issues
This commit is contained in:
FrankE
2016-08-29 19:02:32 +02:00
parent a2c9e575a2
commit a203305628
23 changed files with 505 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
grid/Grid.h
View File

@@ -221,13 +221,14 @@ public:
Assert::isBetween((int32_t)p.y_cm, -max, +max, "y out of bounds");
Assert::isBetween((int32_t)p.z_cm, -max, +max, "z out of bounds");
// shift by half of the allowed width of 20 bit to allow negative regions
// shift by half of the allowed width of 20 bit to allow negative regions:
// -> 19 bit positive and 19 bit negative
const uint64_t center = 1 << 19;
// build
const uint64_t x = std::round((p.x_cm+center) / (float)gridSize_cm);
const uint64_t y = std::round((p.y_cm+center) / (float)gridSize_cm);
const uint64_t z = std::round((p.z_cm+center) / (float)gridSize_cm * 5); // z is usually much lower and not always aligned -> allow more room for hashes
const uint64_t x = center + (int64_t) std::round((p.x_cm) / (float)gridSize_cm);
const uint64_t y = center + (int64_t) std::round((p.y_cm) / (float)gridSize_cm);
const uint64_t z = center + (int64_t) std::round((p.z_cm) / (float)gridSize_cm * 5); // z is usually much lower and not always aligned -> allow more room for hashes
return (z << 40) | (y << 20) | (x << 0);
@@ -256,6 +257,11 @@ public:
disconnectUniDir(n2, n1);
}
/** remove the connection from n1 to n2 (not the other way round!) */
void disconnectUniDir(const int idx1, const int idx2) {
disconnectUniDir(nodes[idx1], nodes[idx2]);
}
/** remove the connection from n1 to n2 (not the other way round!) */
void disconnectUniDir(T& n1, T& n2) {
for (int n = 0; n < n1._numNeighbors; ++n) {
@@ -291,7 +297,11 @@ public:
// disconnect from all neighbors
while (node._numNeighbors) {
// by removing one neighbor, all others are shifted to close the gap within the array
// its therefor ok to always delete array index [0]
disconnectBiDir(node._idx, node._neighbors[0]);
}
// remove from hash-list

161
grid/factory/v2/Importance.h
View File

@@ -54,166 +54,63 @@ public:
// the number of neighbors to use
static constexpr int numNeighbors = 12;
// create list of all doors
// create list of all door-nodes
std::vector<T> doors;
// create list of all stair-nodes
std::vector<T> stairs;
// process each node
for (T& n1 : g) {
// is the current node a door?
//if (isDoor(n1, neighbors)) {doors.push_back(n1);} // OLD
if (n1.getType() == GridNode::TYPE_DOOR) {doors.push_back(n1);} // NEW!
// favor stairs just like doors
//if (isStaircase(g, n1)) {doors.push_back(n1);} // OLD
if (n1.getType() == GridNode::TYPE_STAIR) {doors.push_back(n1);} // NEW
switch(n1.getType()) {
case GridNode::TYPE_DOOR: doors.push_back(n1); break;
case GridNode::TYPE_STAIR: stairs.push_back(n1); break;
}
}
// KNN for doors
KNNArray<std::vector<T>> knnArrDoors(doors);
KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
// KNN for stairs
KNNArray<std::vector<T>> knnArrStairs(stairs);
KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);
// probability adjustments
Distribution::Normal<float> avoidWalls(0.0, 0.35);
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Normal<float> favorStairs(0.0f, 3.5f);
// process each node again
for (T& n1 : g) {
// skip nodes on other than the requested floor-level
//if (n1.z_cm != z_cm) {continue;}
// get the 10 nearest neighbors and their distance
size_t indices[numNeighbors];
float squaredDist[numNeighbors];
float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
knn.get(point, numNeighbors, indices, squaredDist);
// get the neighbors
std::vector<T*> neighbors;
for (int i = 0; i < numNeighbors; ++i) {
neighbors.push_back(&inv[indices[i]]);
}
n1.navImportance = 1.0f;
//if (n1.getType() == GridNode::TYPE_FLOOR) {
// get the distance to the nearest door
const float distToWall_m = Units::cmToM(std::sqrt(squaredDist[0]) + g.getGridSize_cm());
// get the distance to the nearest wall
const float distToWall_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
// get the distance to the nearest door
const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
n1.navImportance =
1 +
getWallImportance( distToWall_m ) +
favorDoors.getProbability(distToDoor_m) * 1.5f;
// get the distance to the nearest stair
const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
const bool useNormal = (distToWall_m < distToDoor_m && distToWall_m < distToStair_m);
//}
//addDoor(n1, neighbors);
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 3.5f;
// importance for this node (based on the distance from the next door)
//n1.navImportance += favorDoors.getProbability(dist_m) * 0.30;
if (useNormal) {
n1.navImportance -= avoidWalls.getProbability(distToWall_m);
}
//n1.navImportance = (dist_m < 0.2) ? (1) : (0.5);
}
}
/** is the given node connected to a staircase? */
template <typename T> static bool isStaircase(Grid<T>& g, T& node) {
return node.getType() == GridNode::TYPE_STAIR;
// // if this node has a neighbor with a different z, this is a stair
// for (T& neighbor : g.neighbors(node)) {
// if (neighbor.z_cm != node.z_cm) {return true;}
// }
// return false;
}
// /** is the given node (and its inverted neighbors) a door? */
// template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
// if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
// MiniMat2 m1;
//// MiniMat2 m2;
// Point3 center = nSrc;
// // calculate the centroid of the nSrc's nearest-neighbors
// Point3 centroid(0,0,0);
// for (const T* n : neighbors) {
// centroid = centroid + (Point3)*n;
// }
// centroid /= neighbors.size();
// // if nSrc is too far from the centroid, this does not make sense
// if ((centroid-center).length() > 40) {return false;}
// // build covariance of the nearest-neighbors
// int used = 0;
// for (const T* n : neighbors) {
// const Point3 d1 = (Point3)*n - centroid;
// if (d1.length() > 100) {continue;} // radius search
// m1.addSquared(d1.x, d1.y);
//// const Point3 d2 = (Point3)*n - center;
//// if (d2.length() > 100) {continue;} // radius search
//// m2.addSquared(d2.x, d2.y);
// ++used;
// }
// // we need at least two points for the covariance
// if (used < 6) {return false;}
// // check eigenvalues
// MiniMat2::EV ev1 = m1.getEigenvalues();
//// MiniMat2::EV ev2 = m2.getEigenvalues();
// // ensure e1 > e2
// if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
//// if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}
// // door?
// const float ratio1 = (ev1.e2/ev1.e1);
//// const float ratio2 = (ev2.e2/ev2.e1);
//// const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);
// return (ratio1 < 0.30 && ratio1 > 0.05) ;
// }
/** get the importance of the given node depending on its nearest wall */
static float getWallImportance(float dist_m) {
// avoid sticking too close to walls (unlikely)
static Distribution::Normal<float> avoidWalls(0.0, 0.35);
// favour walking near walls (likely)
//static Distribution::Normal<float> stickToWalls(0.9, 0.7);
// favour walking far away (likely)
//static Distribution::Normal<float> farAway(2.2, 0.5);
//if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance
// return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
// + stickToWalls.getProbability(dist_m) * 0.15 // walk near walls
// + farAway.getProbability(dist_m) * 0.15 // walk in the middle
return - avoidWalls.getProbability(dist_m) // avoid walls
//+ stickToWalls.getProbability(dist_m) // walk near walls
//+ farAway.getProbability(dist_m) // walk in the middle
;
}
};

38
grid/factory/v2/Stairs.h
View File

@@ -8,7 +8,7 @@
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
#include <fstream>
@@ -23,6 +23,10 @@ private:
Helper<T> helper;
std::ofstream outStairs;
std::ofstream outDelete;
private:
// struct Entry {
@@ -68,10 +72,14 @@ public:
/** ctor */
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
;
outStairs.open("/tmp/quads.dat");
outDelete.open("/tmp/delete.dat");
}
~Stairs() {
outStairs.close();
outDelete.close();
}
@@ -87,6 +95,7 @@ public:
const std::vector<Floorplan::StairPart> parts = stair->getParts();
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);
for (int i = 0; i < (int)parts.size(); ++i) {
//const Floorplan::StairPart& part = parts[i];
@@ -97,6 +106,13 @@ public:
const Point3 p3 = quad.p3;
const Point3 p4 = quad.p4;
outStairs << p1.x << " " << p1.y << " " << p1.z << "\n";
outStairs << p2.x << " " << p2.y << " " << p2.z << "\n";
outStairs << p3.x << " " << p3.y << " " << p3.z << "\n";
outStairs << p4.x << " " << p4.y << " " << p4.z << "\n";
outStairs << p1.x << " " << p1.y << " " << p1.z << "\n\n\n";
HelperPoly poly; poly.add(p1.xy()); poly.add(p2.xy()); poly.add(p3.xy()); poly.add(p4.xy());
const int x1 = helper.align(poly.bbox_cm.getMin().x); // ALIGNF?
@@ -111,6 +127,9 @@ public:
for (int y = y1; y <= y2; y+=gs_cm) {
if (!poly.contains( Point2(x,y) )) {continue;}
points.push_back(XYZ(x,y,-1));
if (y < 500) {
int i = 0; (void) i;
}
}
}
@@ -185,6 +204,7 @@ public:
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
// add all stair nodes or replace them with already existing ones
for (Intermediate& iNode : stairNodes) {
@@ -192,7 +212,9 @@ public:
const float minDiff = gs_cm * 0.49;
// nearer than maxDiff? -> delete as this one is unreachable below the stair
const float maxDiff = 150;
// to prevent errors (e.g. a platform at 1.0 meter) we use 0.98 meter as deleting threshold
// otherwise we get problems in some places
const float maxDiff = 98;
// distance between the stair node and the floor above / below
const float zDiff1 = std::abs(iNode.node.z_cm - 100*floor->getStartingZ());
@@ -223,11 +245,19 @@ public:
// remove nodes directly below the stair
} else if (zDiff1 < maxDiff) {
T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
outDelete << n2->x_cm << " " << n2->y_cm << " " << n2->z_cm << "\n";
if (n2->y_cm < 500) {
int i = 0; (void) i;
}
if (n2) {toDelete.push_back(n2);}
// remove nodes directly above the stair
} else if (zDiff2 < maxDiff) {
T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
outDelete << n2->x_cm << " " << n2->y_cm << " " << n2->z_cm << "\n";
if (n2->y_cm < 500) {
int i = 0; (void) i;
}
if (n2) {toDelete.push_back(n2);}
}

54
grid/walk/v2/modules/WalkModuleFavorZ.h Normal file
View File

@@ -0,0 +1,54 @@
#ifndef WALKMODULEFAVORZ_H
#define WALKMODULEFAVORZ_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleFavorZ : public WalkModule<Node, WalkState> {
public:
/** ctor */
WalkModuleFavorZ() {
;
}
virtual void updateBefore(WalkState& state) override {
(void) state;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
if (curNode.z_cm != potentialNode.z_cm) {
return 8;
} else {
return 1;
}
}
};
#endif // WALKMODULEFAVORZ_H

4
grid/walk/v2/modules/WalkModuleHeadingControl.h
View File

@@ -26,14 +26,14 @@ private:
public:
/** ctor 3.0 should be OK! */
WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 1.5).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
;
}
virtual void updateBefore(WalkState& state) override {
const float var = draw.get() * 0.15;//0.05;
const float var = draw.get() * 0.20;//0.05;
//const float var = 0;
state.startHeading += ctrl->turnAngle + var;

4
grid/walk/v2/modules/WalkModuleNodeImportance.h
View File

@@ -44,8 +44,8 @@ public:
(void) curNode;
const double prob = potentialNode.getNavImportance();
return std::pow(prob, 10);
//return prob;
//return std::pow(prob, 10);
return prob;
}