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

added new sanity-check assertions

Browse Source 
fixed issue with angles [bad interface]
- adjusted other parts accordingly
- added corresponding test-cases
started working on absolute heading
This commit is contained in:
FrankE
2017-03-31 11:47:29 +02:00
parent 2fdaa795b2
commit 8930be1e2c
10 changed files with 168 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
geo/Angle.h
View File

@@ -29,6 +29,13 @@ public:
return radToDeg(getRAD_2PI(x1,y1,x2,y2));
}
/** ensure the given radians-value is within [0:2pi] */
static float makeSafe_2PI(float rad) {
while(rad < 0) {rad += 2*PI;}
while(rad >= 2*PI) {rad -= 2*PI;}
return rad;
}
/**
* gets the angular difference between
@@ -44,13 +51,14 @@ public:
/**
* gets the angular difference between
* "angular change from r1 to r2"
* - the given radians [0:2PI]
* - as a change-in-direction between [-PI:+PI]
*/
static float getSignedDiffRAD_2PI(const float r1, const float r2) {
Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 out of bounds"); // [0:360] deg
Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds"); // [0:360] deg
float diff = r1-r2;
float diff = r2-r1;
if (diff > +PI) {diff = -(2*PI - diff);}
else if (diff < -PI) {diff = +(2*PI + diff);}
Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg

7
geo/Heading.h
View File

@@ -33,8 +33,11 @@ public:
}
/** signled angular difference [-PI:+PI] */
float getSignedDiff(const Heading other) const {
return Angle::getSignedDiffRAD_2PI(rad, other.rad);
// float getSignedDiff(const Heading other) const {
// return Angle::getSignedDiffRAD_2PI(other.rad, rad);
// }
static float getSignedDiff(const Heading from, const Heading to) {
return Angle::getSignedDiffRAD_2PI(from.rad, to.rad);
}
/** update the angle but ensure we stay within [0:2PI] */

2
grid/factory/v2/Importance.h
View File

@@ -153,6 +153,8 @@ public:
// sanity check
Assert::isNotNaN(n1.walkImportance, "detected NaN walk importance for " + n1.asString());
Assert::isNotNaN(n1.navImportance, "detected NaN walk importance for " + n1.asString());
Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");

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

@@ -104,9 +104,19 @@ public:
//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
probability = 1.0;
//probability = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0); // dist_m might be zero -> no edges -> no maximum
probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones)
// add the walk importance to the probabiliy [each node has a to-be-walked-probability depending on its distance to walls, etc...]
const float walkImportance = curNode->getWalkImportance();
Assert::isNotNaN(walkImportance, "grid-node's walk-importance is NaN. Did you forget to calculate the importance values after building the grid?");
Assert::isBetween(walkImportance, 0.0f, 2.5f, "grid-node's walk-importance is out of range. Did you forget to calculate the importance values after building the grid?");
probability *= walkImportance;// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones)
//probability = std::pow(probability, 5);
// sanity check
Assert::isNotNaN(probability, "detected NaN grid-walk probability");
// update after
updateAfter(currentState, *startNode, *curNode);

85
grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h Normal file
View File

@@ -0,0 +1,85 @@
#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H
#define WALKMODULEABSOLUTEHEADINGCONTROL_H
/**
* compare the state's absolute heading against a given compass [control]
*/
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
/** keep the state's heading */
template <typename Node, typename WalkState, typename Control> class WalkModuleAbsoluteHeadingControl : public WalkModule<Node, WalkState> {
const float sigma_rad;
const Control* ctrl;
public:
/** ctor. 180 should be OK! */
WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) {
// ensure the template WalkState inherits from 'WalkStateHeading'!
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) startNode;
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
// reset this one after every transition!
Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds");
// ignore for stairs?
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
// for elevator edges [same (x,y) but different z] just return 1
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
// 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 - the last error
const Heading stateHead = state.heading.direction;
// get the difference
const float angularDiff = head.getDiffHalfRAD(stateHead);
if (angularDiff > Angle::degToRad(180)) {return 0.05;}
if (angularDiff > Angle::degToRad(90)) {return 0.25;}
{return 0.70;}
}
};
#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H

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

@@ -77,7 +77,8 @@ public:
const Heading stateHead = state.heading.direction;
// get the error (signed difference) between both
const float angularDiff = stateHead.getSignedDiff(head);
// const float angularDiff = stateHead.getSignedDiff(head);
const float angularDiff = Heading::getSignedDiff(head, stateHead);
// adjust the error.
// note: the error may get > +/- 2PI but this is not an issue!

3
grid/walk/v2/modules/WalkModuleHeadingVonMises.h
View File

@@ -81,7 +81,8 @@ public:
const Heading stateHead = state.heading.direction;
// get the error (signed difference) between both
const float angularDiff = stateHead.getSignedDiff(head);
//const float angularDiff = stateHead.getSignedDiff(head);
const float angularDiff = Heading::getSignedDiff(head, stateHead);
// adjust the error.
// note: the error may get > +/- 2PI but this is not an issue!

2
main.cpp
View File

@@ -30,7 +30,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
::testing::GTEST_FLAG(filter) = "*Earth*";
::testing::GTEST_FLAG(filter) = "*Angle*";
//::testing::GTEST_FLAG(filter) = "*Barometer*";

26
sensors/offline/OfflineAndroid.h
View File

@@ -54,7 +54,7 @@ private:
std::vector<OfflineEntry<GyroscopeData>> gyro;
std::vector<OfflineEntry<AccelerometerData>> accel;
std::vector<OfflineEntry<AccelerometerData>> gravity;
std::vector<OfflineEntry<GravityData>> gravity;
std::vector<OfflineEntry<CompassData>> compass;
std::vector<OfflineEntry<BarometerData>> barometer;
@@ -86,7 +86,7 @@ public:
const std::vector<OfflineEntry<AccelerometerData>>& getAccelerometer() const {return accel;}
/** get all gravity readings */
const std::vector<OfflineEntry<AccelerometerData>>& getGravity() const {return gravity;}
const std::vector<OfflineEntry<GravityData>>& getGravity() const {return gravity;}
/** get all barometer readings */
const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}
@@ -173,8 +173,8 @@ private:
}
case (int) Offline::Sensor::GRAVITY: {
const AccelerometerData data = parseAccelerometer(sensorData);
gravity.push_back(OfflineEntry<AccelerometerData>(ts, data));
const GravityData data = parseGravity(sensorData);
gravity.push_back(OfflineEntry<GravityData>(ts, data));
if (listener) {listener->onGravity(ts, data);}
break;
}
@@ -301,6 +301,24 @@ private:
}
static inline GravityData parseGravity(const std::string& data) {
const size_t pos1 = data.find(';', 0);
const size_t pos2 = data.find(';', pos1+1);
const size_t pos3 = data.find(';', pos2+1);
Assert::isTrue(pos1 != std::string::npos, "format error");
Assert::isTrue(pos2 != std::string::npos, "format error");
Assert::isTrue(pos3 != std::string::npos, "format error");
const std::string sx = data.substr(0, pos1);
const std::string sy = data.substr(pos1+1, pos2-pos1-1);
const std::string sz = data.substr(pos2+1, pos3-pos2-1);
return GravityData(std::stof(sx), std::stof(sy), std::stof(sz));
}
/** parse the given Barometer entry */
static inline BarometerData parseBarometer(const std::string& data) {

37
tests/geo/TestAngle.cpp
View File

@@ -14,17 +14,38 @@ TEST(Angle, dir) {
}
TEST(Angle, safe) {
ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(0)))));
ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(360)))));
ASSERT_EQ(85, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(85)))));
ASSERT_EQ(155, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(155)))));
ASSERT_EQ(275, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(275)))));
ASSERT_EQ(355, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(355)))));
// negative
ASSERT_EQ(330, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-30)))));
ASSERT_EQ(270, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-90)))));
ASSERT_EQ(185, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-175)))));
// too positive
ASSERT_EQ(30, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(390)))));
ASSERT_EQ(140, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(500)))));
ASSERT_EQ(180, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(900)))));
}
TEST(Angle, calc) {
ASSERT_EQ(0, Angle::getDEG_360(0,0, +1,0)); // to the right
ASSERT_EQ(90, Angle::getDEG_360(0,0, 0,+1)); // upwards
ASSERT_EQ(180, Angle::getDEG_360(0,0, -1,0)); // to the left
ASSERT_EQ(270, Angle::getDEG_360(0,0, 0,-1)); // downwards
ASSERT_EQ(0, (int)Angle::getDEG_360(0,0, +1,0)); // to the right
ASSERT_EQ(90, (int)Angle::getDEG_360(0,0, 0,+1)); // upwards
ASSERT_EQ(180, (int)Angle::getDEG_360(0,0, -1,0)); // to the left
ASSERT_EQ(270, (int)Angle::getDEG_360(0,0, 0,-1)); // downwards
ASSERT_EQ(45, Angle::getDEG_360(0,0, +1,+1)); // to the upper right
ASSERT_EQ(135, Angle::getDEG_360(0,0, -1,+1)); // to the upper left
ASSERT_EQ(225, Angle::getDEG_360(0,0, -1,-1)); // to the lower left
ASSERT_EQ(315, Angle::getDEG_360(0,0, +1,-1)); // to the lower right
ASSERT_EQ(45, (int)Angle::getDEG_360(0,0, +1,+1)); // to the upper right
ASSERT_EQ(135, (int)Angle::getDEG_360(0,0, -1,+1)); // to the upper left
ASSERT_EQ(225, (int)Angle::getDEG_360(0,0, -1,-1)); // to the lower left
ASSERT_EQ(315, (int)Angle::getDEG_360(0,0, +1,-1)); // to the lower right
}