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

some fixes [multithreading,..]

Browse Source 
needed interface changes [new options]
logger for android
wifi-ap-optimization
new test-cases
This commit is contained in:
FrankE
2016-09-28 12:19:14 +02:00
parent 91e3367372
commit 4f511d907e
62 changed files with 1418 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
grid/walk/v2/GridWalker.h
View File

@@ -1,6 +1,7 @@
#ifndef GRIDWALKER_H
#define GRIDWALKER_H
#include "../../../data/Timestamp.h"
#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "modules/WalkModule.h"
@@ -16,7 +17,7 @@ private:
/** all modules to evaluate */
std::vector<WalkModule<Node, WalkState>*> modules;
DrawList<const Node*> drawer;
RandomGenerator rnd;
public:
@@ -26,14 +27,16 @@ public:
}
/** perform the walk based on the configured setup */
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m, double& probability) {
Assert::isTrue(dist_m >= 0, "walk distance must not be negative!");
Assert::isTrue(dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!");
// keep the starting state for reference
//const WalkState startState = _startState;
// the current state that is modified for each step
WalkState currentState = _startState;
updateBefore(currentState);
// get the node that corresponds to start;
const Node* startNode = grid.getNodePtrFor(currentState.position);
@@ -42,10 +45,26 @@ public:
// currently examined node
const Node* curNode = startNode;
// perform initial update
updateBefore(currentState, *curNode);
// add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced)
dist_m += _startState.distance.error_m;
probability = 1.0;
int cnt = 0;
// until distance is reached
while (dist_m > 0) {
drawer.reset();
// only needed for a global (no-thread-safe) drawer
//drawer.reset();
// the rnd-generator is shared among several threads which should be fine
// the draw-list, however, is per-thread, which is mandatory!
// alternative to the generator would be seeding the interal one which prooved very unstable!
DrawList<const Node*> drawer(rnd); drawer.reserve(10);
// evaluate each neighbor
for (const Node& neighbor : grid.neighbors(*curNode)) {
@@ -53,8 +72,12 @@ public:
drawer.add(&neighbor, prob);
}
// pick a neighbor
const Node* nextNode = drawer.get();
// pick a neighbor and get its probability
double nodeProbability;
const Node* nextNode = drawer.get(nodeProbability);
if ( nodeProbability < probability ) {probability = nodeProbability;} // keep the smallest one
//probability += nodeProbability;
//++cnt;
// inform
step(currentState, *curNode, *nextNode);
@@ -64,12 +87,17 @@ public:
curNode = nextNode;
currentState.position = *curNode;
}
//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
probability = curNode->getWalkImportance();
// update after
updateAfter(currentState, *startNode, *curNode);
// calculate the walked-distance-error (is usually negative, as we walked a little too far)
currentState.distance.error_m = dist_m;
// done
return currentState;
@@ -78,10 +106,10 @@ public:
private:
/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
inline void updateBefore(WalkState& state) {
inline void updateBefore(WalkState& state, const Node& startNode) {
for (WalkModule<Node, WalkState>* mdl : modules) {
mdl->updateBefore(state);
mdl->updateBefore(state, startNode);
}
}

18
grid/walk/v2/modules/WalkModule.h
View File

@@ -9,6 +9,22 @@ struct WalkState {
/** current position within the grid (-> in cm!) */
GridPoint position;
/** nested struct to prevent name clashes */
struct Distance {
/**
* for every walk, the walker is given a desired distance
* however, the walking distance depends on the grid size and can
* therefore never be reached exactly. therefor we track the
* error between desired and walked distance to ensure "in average"
* the walked distance is correct
*/
float error_m;
Distance() : error_m(0) {;}
} distance;
/** ctor */
explicit WalkState(const GridPoint& position) : position(position) {;}
@@ -23,7 +39,7 @@ template <typename Node, typename WalkState> class WalkModule {
public:
/** update the given WalkState before starting the walk. e.g. based on sensor readings */
virtual void updateBefore(WalkState& state) = 0;
virtual void updateBefore(WalkState& state, const Node& startNode) = 0;
/** get the probability p(e) from curNode to potentialNode */
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;

111
grid/walk/v2/modules/WalkModuleActivityControl.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef WALKMODULEACTIVITYCONTROL_H
#define WALKMODULEACTIVITYCONTROL_H
#include "WalkModule.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressure.h"
/**
* use the currently detected activity (stay-on-floor, walk-up, walk-down, ..)
* from the system's control data to favor edges that resemble this activity
*/
template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControl : public WalkModule<Node, WalkState> {
private:
const Control* ctrl;
public:
/** ctor */
WalkModuleActivityControl(const Control* ctrl) : ctrl(ctrl) {
;
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
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;
}
// static double getProbability(const Node& lastNode, const Node& nextNode, const ActivityButterPressure::Activity activity) {
// switch (activity) {
// case ActivityButterPressure::Activity::DOWN:
// if (deltaZ_cm < 0) {return 0.85;}
// if (deltaZ_cm == 0) {return 0.10;}
// {return 0.05;}
// case ActivityButterPressure::Activity::UP:
// if (deltaZ_cm > 0) {return 0.85;}
// if (deltaZ_cm == 0) {return 0.10;}
// {return 0.05;}
// case ActivityButterPressure::Activity::STAY:
// if (deltaZ_cm == 0) {return 0.85;}
// {return 0.15;}
// default:
// throw Exception("not yet implemented");
// }
// return 1.0;
// }
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;
// TODO: general activity enum and activity-detector based on barometer and accelerometer?
const ActivityButterPressure::Activity activity = ctrl->activity;
// const float kappa = 0.75;
switch (activity) {
case ActivityButterPressure::Activity::DOWN:
if (deltaZ_cm < 0) {return 0.60;}
if (deltaZ_cm == 0) {return 0.25;}
{return 0.15;}
case ActivityButterPressure::Activity::UP:
if (deltaZ_cm > 0) {return 0.60;}
if (deltaZ_cm == 0) {return 0.25;}
{return 0.15;}
case ActivityButterPressure::Activity::STAY:
if (deltaZ_cm == 0) {return 0.60;}
{return 0.40;}
// case ActivityButterPressure::Activity::DOWN:
// case ActivityButterPressure::Activity::UP:
// if (potentialNode.getType() == GridNode::TYPE_STAIR) {return kappa;}
// if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
// {return 1-kappa;}
// case ActivityButterPressure::Activity::STAY:
// if (potentialNode.getType() == GridNode::TYPE_DOOR) {return kappa;}
// if (potentialNode.getType() == GridNode::TYPE_FLOOR) {return kappa;}
// {return 1-kappa;}
default:
throw Exception("not yet implemented");
}
}
};
#endif // WALKMODULEACTIVITYCONTROL_H

32
grid/walk/v2/modules/WalkModuleButterActivity.h
View File

@@ -2,12 +2,30 @@
#define WALKMODULEBUTTERACTIVITY_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressure.h"
DEPREACTED
SEE WalkModuleActivityControl
struct WalkStateBarometerActivity {
/** innser-struct to prevent name-clashes */
struct Barometer {
/** activity currently detected from the baromter */
ActivityButterPressure::Activity activity;
Barometer() : activity(ActivityButterPressure::Activity::STAY) {;}
} barometer;
/** ctor */
WalkStateBarometerActivity() : barometer() {;}
};
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleButterActivity : public WalkModule<Node, WalkState> {
@@ -16,11 +34,15 @@ public:
/** ctor */
WalkModuleButterActivity() {
;
// ensure templates WalkState inherits from 'WalkStateBarometerActivity'
StaticAssert::AinheritsB<WalkState, WalkStateBarometerActivity>();
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -43,11 +65,11 @@ public:
const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
if(state.act == ActivityButterPressure::Activity::DOWN){
if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){
if (deltaZ_cm < 0) {return 0;}
if (deltaZ_cm == 0) {return 0.1;}
return 0.9;
} else if (state.act == ActivityButterPressure::Activity::UP){
} else if (state.barometer.activity == ActivityButterPressure::Activity::UP){
if (deltaZ_cm > 0) {return 0;}
if (deltaZ_cm == 0) {return 0.1;}
return 0.9;

15
grid/walk/v2/modules/WalkModuleFavorZ.h
View File

@@ -36,7 +36,7 @@ template <typename Node, typename WalkState> class WalkModuleFavorZ : public Wal
private:
// force states to walk into the same z-direction for 30 edges
const int keepForXEdges = 12;
const int keepForXEdges = 8;
public:
@@ -49,8 +49,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -93,15 +94,15 @@ public:
const int diff = potentialNode.z_cm - curNode.z_cm;
// tendence available + tendence match? -> high score!
if (tendence > 0 && diff > 0) {return 0.95;}
if (tendence < 0 && diff < 0) {return 0.95;}
if (tendence > 0 && diff >= 0) {return 0.90;}
if (tendence < 0 && diff <= 0) {return 0.90;}
// tendence available + tendence mismatch? -> very low score!
if (tendence > 0 && diff < 0) {return 0.05;}
if (tendence < 0 && diff > 0) {return 0.05;}
if (tendence > 0 && diff < 0) {return 0.10;}
if (tendence < 0 && diff > 0) {return 0.10;}
// no tendence available -> just favor z-transitions over non-z-transitions
return (diff != 0) ? (0.7) : (0.3);
return (diff != 0) ? (0.75) : (0.25);
}

78
grid/walk/v2/modules/WalkModuleFollowDestination.h
View File

@@ -2,39 +2,77 @@
#define WALKMODULEFOLLOWDESTINATION_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../nav/dijkstra/Dijkstra.h"
#include "../../../../nav/dijkstra/DijkstraPath.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/**
* favour edges p(e) that approach the destination
* favor nodes that approach a known destination
*/
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
private:
const Grid<Node>& grid;
Dijkstra<Node> dijkstra;
const DijkstraNode<Node>* dnDest;
struct DijkstraMapper {
struct DijkstraAccess {
const Grid<Node>& grid;
DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
DijkstraAccess(const Grid<Node>& grid) : grid(grid) {;}
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
const 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.getDistanceInCM(n2) * n2.navImportance;
return n1.getDistanceInMeter(n2) / n2.getNavImportance();
}
};
public:
/** ctor */
WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
/** ctor WITHOUT known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {
// shortest path calculation
dijkstra.build(&destination, DijkstraMapper(grid));
// ensure the template WalkState inherits from 'WalkStateFavorZ'
//StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
/** ctor WITH known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
setDestination(destination);
}
/** set the desired destination node */
void setDestination(const Node& dest) {
DijkstraAccess acc(grid);
dijkstra.build(&dest, acc);
dnDest = dijkstra.getNode(dest);
}
/** get the shortest path from the given start to the configured destination */
DijkstraPath<Node> getShortestPath(const Node& start) {
const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
const DijkstraPath<Node> path(dnStart, dnDest);
return path;
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
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 {
@@ -43,26 +81,28 @@ public:
(void) nextNode;
}
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
const float kappa = 0.8;
const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);
// probability
return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
if (dnCur == nullptr) {return 1.0;}
if (dnPot == nullptr) {return 1.0;}
constexpr double kappa = 0.70;
const float curDistToTarget = dnCur->cumWeight;
const float potDistToTarget = dnPot->cumWeight;
return (potDistToTarget < curDistToTarget) ? (kappa) : (1.0-kappa);
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
};
#endif // WALKMODULEFOLLOWDESTINATION_H

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

@@ -36,7 +36,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) startNode;
// add noise
state.heading.direction += draw.get();

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

@@ -19,14 +19,12 @@ private:
/** random noise */
Distribution::Normal<float> distNoise;
Control* ctrl;
//std::unordered_map<WalkState*, float> errorTracker;
const Control* ctrl;
public:
/** ctor 3.0 should be OK! */
WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) {
@@ -37,17 +35,20 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
// reset this one after every transition!
Assert::isBetween(ctrl->turnAngle, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
// sensor noise
const float var = distNoise.draw();
float var = distNoise.draw();
// stair? -> increase variance
if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
// adjust the state's heading using the control-data
state.heading.direction += ctrl->turnAngle + var;
state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
}
@@ -61,8 +62,13 @@ public:
(void) state;
// ignore for stairs?
//if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
// for elevator edges [same (x,y) but different z] do not adjust anything
if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
if (nextNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
//if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
// get the heading denoted by the way from curNode to nextNode
const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
@@ -86,9 +92,13 @@ public:
(void) startNode;
// ignore for stairs?
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
// for elevator edges [same (x,y) but different z] just return 1
if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
// get the heading between curNode and potentialNode
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
@@ -99,16 +109,18 @@ public:
// get the difference
const float angularDiff = head.getDiffHalfRAD(stateHead);
if (angularDiff > Angle::degToRad(135)) {return 0.01;}
if (angularDiff > Angle::degToRad(90)) {return 0.02;}
if (angularDiff > Angle::degToRad(45)) {return 0.07;}
{return 0.90;}
// add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE"
// determine probability
const float prob = dist.getProbability(angularDiff);
return prob;
// const float prob = dist.getProbability(angularDiff);
// return prob;
}

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

@@ -22,8 +22,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -44,7 +45,12 @@ public:
(void) startNode;
(void) curNode;
const double prob = potentialNode.getNavImportance();
//const double prob = potentialNode.getWalkImportance();
const float i1 = curNode.getWalkImportance();
const float i2 = potentialNode.getWalkImportance();
const double prob = (i2 > i1) ? (0.9) : (0.1);
return prob;
}

3
grid/walk/v2/modules/WalkModulePreventVisited.h
View File

@@ -45,8 +45,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

3
grid/walk/v2/modules/WalkModuleRelativePressureControl.h
View File

@@ -46,8 +46,9 @@ public:
;
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

3
grid/walk/v2/modules/WalkModuleSpread.h
View File

@@ -45,8 +45,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {