kazu
4f511d907e
needed interface changes [new options] logger for android wifi-ap-optimization new test-cases
156 lines
4.4 KiB
C++
156 lines
4.4 KiB
C++
#ifndef GRIDWALKER_H
|
|
#define GRIDWALKER_H
|
|
|
|
#include "../../../data/Timestamp.h"
|
|
#include "../../Grid.h"
|
|
#include "../../../math/DrawList.h"
|
|
#include "modules/WalkModule.h"
|
|
|
|
/**
|
|
* modular grid-walker that takes various sub-components to determine
|
|
* p(e) and thus randomly pick edges
|
|
*/
|
|
template <typename Node, typename WalkState> class GridWalker {
|
|
|
|
private:
|
|
|
|
/** all modules to evaluate */
|
|
std::vector<WalkModule<Node, WalkState>*> modules;
|
|
|
|
RandomGenerator rnd;
|
|
|
|
public:
|
|
|
|
/** add the given module */
|
|
void addModule(WalkModule<Node, WalkState>* module) {
|
|
modules.push_back(module);
|
|
}
|
|
|
|
/** perform the walk based on the configured setup */
|
|
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;
|
|
|
|
// get the node that corresponds to start;
|
|
const Node* startNode = grid.getNodePtrFor(currentState.position);
|
|
Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
|
|
|
|
// 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) {
|
|
|
|
// 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)) {
|
|
const double prob = getProbability(currentState, *startNode, *curNode, neighbor);
|
|
drawer.add(&neighbor, prob);
|
|
}
|
|
|
|
// 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);
|
|
|
|
// update distance-to-walk and current position
|
|
dist_m -= nextNode->getDistanceInMeter(*curNode);
|
|
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;
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
|
|
inline void updateBefore(WalkState& state, const Node& startNode) {
|
|
|
|
for (WalkModule<Node, WalkState>* mdl : modules) {
|
|
mdl->updateBefore(state, startNode);
|
|
}
|
|
|
|
}
|
|
|
|
/** update the WalkState after the random walk (if needed) */
|
|
inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {
|
|
|
|
for (WalkModule<Node, WalkState>* mdl : modules) {
|
|
mdl->updateAfter(state, startNode, endNode);
|
|
}
|
|
|
|
}
|
|
|
|
/** one step taken. inform modules */
|
|
inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {
|
|
|
|
for (WalkModule<Node, WalkState>* mdl : modules) {
|
|
mdl->step(state, curNode, nextNode);
|
|
}
|
|
|
|
}
|
|
|
|
/** get the probability for the given random walk (one edge) */
|
|
inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {
|
|
|
|
double prob = 1.0;
|
|
//double prob = 0;
|
|
|
|
for (const WalkModule<Node, WalkState>* mdl : modules) {
|
|
const double subProb = mdl->getProbability(state, start, cur, next);
|
|
Assert::isTrue(subProb >= 0, "probability must not be negative!");
|
|
prob *= subProb;
|
|
//prob += std::log( mdl->getProbability(state, start, cur, next) );
|
|
}
|
|
|
|
return prob;
|
|
//return std::exp(prob);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // GRIDWALKER_H
|