huge commit
- worked on about everything - grid walker using plugable modules - wifi models - new distributions - worked on geometric data-structures - added typesafe timestamps - worked on grid-building - added sensor-classes - added sensor analysis (step-detection, turn-detection) - offline data reader - many test-cases
This commit is contained in:
125
grid/walk/v2/GridWalker.h
Normal file
125
grid/walk/v2/GridWalker.h
Normal file
@@ -0,0 +1,125 @@
|
||||
#ifndef GRIDWALKER_H
|
||||
#define GRIDWALKER_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;
|
||||
|
||||
DrawList<const Node*> drawer;
|
||||
|
||||
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) {
|
||||
|
||||
WalkState startState = _startState;
|
||||
updateBefore(startState);
|
||||
|
||||
|
||||
|
||||
// get the node that corresponds to start;
|
||||
const Node* startNode = grid.getNodePtrFor(startState.startPos);
|
||||
Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
|
||||
|
||||
// currently examined node
|
||||
const Node* curNode = startNode;
|
||||
|
||||
// until distance is reached
|
||||
while (dist_m > 0) {
|
||||
|
||||
drawer.reset();
|
||||
|
||||
// evaluate each neighbor
|
||||
for (const Node& neighbor : grid.neighbors(*curNode)) {
|
||||
const double prob = getProbability(startState, *startNode, *curNode, neighbor);
|
||||
drawer.add(&neighbor, prob);
|
||||
}
|
||||
|
||||
// pick a neighbor
|
||||
const Node* nextNode = drawer.get();
|
||||
|
||||
// inform
|
||||
step(startState, *curNode, *nextNode);
|
||||
|
||||
// update
|
||||
dist_m -= nextNode->getDistanceInMeter(*curNode);
|
||||
curNode = nextNode;
|
||||
|
||||
}
|
||||
|
||||
// output state
|
||||
WalkState nextState = startState;
|
||||
nextState.startPos = *curNode;
|
||||
|
||||
// update
|
||||
updateAfter(nextState, *startNode, *curNode);
|
||||
|
||||
// done
|
||||
return nextState;
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
|
||||
inline void updateBefore(WalkState& state) {
|
||||
|
||||
for (WalkModule<Node, WalkState>* mdl : modules) {
|
||||
mdl->updateBefore(state);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/** 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) {
|
||||
//prob *= mdl->getProbability(state, start, cur, next);
|
||||
prob += std::log( mdl->getProbability(state, start, cur, next) );
|
||||
}
|
||||
|
||||
//return prob;
|
||||
return std::exp(prob);
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // GRIDWALKER_H
|
||||
Reference in New Issue
Block a user