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

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -28,13 +28,15 @@ public:
/** perform the walk based on the configured setup */
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
WalkState startState = _startState;
updateBefore(startState);
// 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(startState.startPos);
const Node* startNode = grid.getNodePtrFor(currentState.position);
Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
// currently examined node
@@ -47,7 +49,7 @@ public:
// evaluate each neighbor
for (const Node& neighbor : grid.neighbors(*curNode)) {
const double prob = getProbability(startState, *startNode, *curNode, neighbor);
const double prob = getProbability(currentState, *startNode, *curNode, neighbor);
drawer.add(&neighbor, prob);
}
@@ -55,23 +57,21 @@ public:
const Node* nextNode = drawer.get();
// inform
step(startState, *curNode, *nextNode);
step(currentState, *curNode, *nextNode);
// update
// update distance-to-walk and current position
dist_m -= nextNode->getDistanceInMeter(*curNode);
curNode = nextNode;
currentState.position = *curNode;
}
// output state
WalkState nextState = startState;
nextState.startPos = *curNode;
// update
updateAfter(nextState, *startNode, *curNode);
// update after
updateAfter(currentState, *startNode, *curNode);
// done
return nextState;
return currentState;
}
@@ -107,16 +107,18 @@ private:
/** 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;
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) );
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);
return prob;
//return std::exp(prob);
}

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

@@ -6,11 +6,11 @@
/** base-class for all WalkStates */
struct WalkState {
/** position where the walk starts */
GridPoint startPos;
/** current position within the grid (-> in cm!) */
GridPoint position;
/** ctor */
WalkState(const GridPoint& startPos) : startPos(startPos) {;}
explicit WalkState(const GridPoint& position) : position(position) {;}
};

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

@@ -41,7 +41,7 @@ public:
(void) startNode;
if (curNode.z_cm != potentialNode.z_cm) {
return 8;
return 40;
} else {
return 1;
}

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

@@ -28,13 +28,16 @@ public:
virtual void updateBefore(WalkState& state) override {
(void) state;
state.startHeading += draw.get();
// add noise
state.heading.direction += draw.get();
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
// 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;
@@ -60,7 +63,7 @@ public:
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;
const Heading stateHead = state.heading.direction;
// get the difference
const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();

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

@@ -13,11 +13,11 @@ template <typename Node, typename WalkState, typename Control> class WalkModuleH
private:
/** van-Mises distribution */
/** CURRENTLY NOT USED van-Mises distribution */
Distribution::LUT<double> dist;
/** van-Mises draw list */
DrawList<double> draw;
/** random noise */
Distribution::Normal<float> distNoise;
Control* ctrl;
@@ -26,16 +26,25 @@ private:
public:
/** ctor 3.0 should be OK! */
WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) {
;
}
virtual void updateBefore(WalkState& state) override {
const float var = draw.get() * 0.20;//0.05;
//const float var = 0;
state.startHeading += ctrl->turnAngle + var;
// 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?");
// sensor noise
const float var = distNoise.draw();
// adjust the state's heading using the control-data
state.heading.direction += ctrl->turnAngle + var;
}
@@ -46,9 +55,24 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
// 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);
// get the heading requested by the state
const Heading stateHead = state.heading.direction;
// get the error (signed difference) between both
const float angularDiff = stateHead.getSignedDiff(head);
// adjust the error.
// note: the error may get > +/- 2PI but this is not an issue!
// when the error is added to the current heading within getProbability(),
// it is ensured their sum is within [0:2pi]
state.heading.error += angularDiff;
}
@@ -60,11 +84,18 @@ public:
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
// compare the heading against the state's heading - the last error
const Heading stateHead = state.startHeading;
const Heading stateHead = state.heading.direction + state.heading.error;
// 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;

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

@@ -24,7 +24,7 @@ public:
virtual void updateBefore(WalkState& state) override {
(void) state;
avg = avg * 0.999 + state.startPos.inMeter() * 0.001;
avg = avg * 0.999 + state.position.inMeter() * 0.001;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

31
grid/walk/v2/modules/WalkStateHeading.h
View File

@@ -3,12 +3,39 @@
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
Heading startHeading;
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
WalkStateHeading(const Heading& curHeading) : startHeading(curHeading) {;}
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};