FrankE
d283d9b326
new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping
128 lines
3.1 KiB
C++
128 lines
3.1 KiB
C++
#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) {
|
|
|
|
// 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);
|
|
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(currentState, *startNode, *curNode, neighbor);
|
|
drawer.add(&neighbor, prob);
|
|
}
|
|
|
|
// pick a neighbor
|
|
const Node* nextNode = drawer.get();
|
|
|
|
// inform
|
|
step(currentState, *curNode, *nextNode);
|
|
|
|
// update distance-to-walk and current position
|
|
dist_m -= nextNode->getDistanceInMeter(*curNode);
|
|
curNode = nextNode;
|
|
currentState.position = *curNode;
|
|
|
|
|
|
}
|
|
|
|
// update after
|
|
updateAfter(currentState, *startNode, *curNode);
|
|
|
|
// done
|
|
return currentState;
|
|
|
|
}
|
|
|
|
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) {
|
|
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
|