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:
76
grid/walk/v2/modules/WalkModuleHeading.h
Normal file
76
grid/walk/v2/modules/WalkModuleHeading.h
Normal file
@@ -0,0 +1,76 @@
|
||||
#ifndef WALKMODULEHEADING_H
|
||||
#define WALKMODULEHEADING_H
|
||||
|
||||
#include "WalkModule.h"
|
||||
#include "WalkStateHeading.h"
|
||||
|
||||
#include "../../../../geo/Heading.h"
|
||||
#include "../../../../math/Distributions.h"
|
||||
|
||||
|
||||
/** keep the state's heading */
|
||||
template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
|
||||
|
||||
private:
|
||||
|
||||
/** van-Mises distribution */
|
||||
Distribution::LUT<double> dist;
|
||||
|
||||
/** van-Mises draw list */
|
||||
DrawList<double> draw;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
|
||||
;
|
||||
}
|
||||
|
||||
virtual void updateBefore(WalkState& state) override {
|
||||
|
||||
(void) state;
|
||||
state.startHeading += draw.get();
|
||||
|
||||
}
|
||||
|
||||
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
||||
|
||||
// if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) {
|
||||
// Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm);
|
||||
// state.startHeading = head;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
/** one step (edge) is taken */
|
||||
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
||||
|
||||
// TODO
|
||||
(void) state;
|
||||
(void) curNode;
|
||||
(void) nextNode;
|
||||
|
||||
}
|
||||
|
||||
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
||||
|
||||
(void) startNode;
|
||||
|
||||
// get the heading between curNode and potentialNode
|
||||
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
|
||||
|
||||
// compare the heading against the state's heading
|
||||
const Heading stateHead = state.startHeading;
|
||||
|
||||
// get the difference
|
||||
const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();
|
||||
|
||||
// determine probability
|
||||
return dist.getProbability(angularDiff);
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // WALKMODULEHEADING_H
|
||||
Reference in New Issue
Block a user