diff --git a/Assertions.h b/Assertions.h index 98ca4aa..4c727b8 100644 --- a/Assertions.h +++ b/Assertions.h @@ -51,7 +51,7 @@ namespace Assert { if (v != nullptr) {doThrow(err);} } - template static inline void isNotNull(const T v, const STR err) { + template static inline void isNotNull(const T& v, const STR err) { if (v == nullptr) {doThrow(err);} } diff --git a/CMakeLists.txt b/CMakeLists.txt index 908fa41..8a1f58a 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ FILE(GLOB HEADERS ./*/*/*/*.h ./*/*/*/*/*.h ./*/*/*/*/*/*.h + ./*/*/*/*/*/*/*.h ./tests/data/* ./tests/data/*/* ./tests/data/*/*/* @@ -41,6 +42,8 @@ FILE(GLOB SOURCES ./*/*.cpp ./*/*/*.cpp ./*/*/*/*.cpp + ./*/*/*/*/*.cpp + ./*/*/*/*/*/*.cpp ) FIND_PACKAGE( OpenMP REQUIRED) diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index ae59caa..d59aa97 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -35,7 +35,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index de7d3a4..93ce5fc 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -23,7 +23,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkPushForward.h b/grid/walk/GridWalkPushForward.h index 7d01da3..4234d39 100644 --- a/grid/walk/GridWalkPushForward.h +++ b/grid/walk/GridWalkPushForward.h @@ -32,7 +32,7 @@ private: static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20); /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 2d5b05e..b8520b9 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -35,7 +35,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkRandomHeadingUpdateAdv.h b/grid/walk/GridWalkRandomHeadingUpdateAdv.h index a387755..2135d2b 100644 --- a/grid/walk/GridWalkRandomHeadingUpdateAdv.h +++ b/grid/walk/GridWalkRandomHeadingUpdateAdv.h @@ -35,7 +35,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h index 9254fb9..3e2dbc6 100644 --- a/grid/walk/GridWalkShortestPathControl.h +++ b/grid/walk/GridWalkShortestPathControl.h @@ -79,7 +79,7 @@ protected: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index cc3ad88..e4dcb6c 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -24,7 +24,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkWeighted.h b/grid/walk/GridWalkWeighted.h index cd97f5a..5159398 100644 --- a/grid/walk/GridWalkWeighted.h +++ b/grid/walk/GridWalkWeighted.h @@ -39,7 +39,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkWeighted2.h b/grid/walk/GridWalkWeighted2.h index 26a8456..4e44043 100644 --- a/grid/walk/GridWalkWeighted2.h +++ b/grid/walk/GridWalkWeighted2.h @@ -40,7 +40,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/TestWalkWeighted3.h b/grid/walk/TestWalkWeighted3.h index c8eec62..bfdc3fd 100644 --- a/grid/walk/TestWalkWeighted3.h +++ b/grid/walk/TestWalkWeighted3.h @@ -40,7 +40,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index eaf10de..737caa6 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -19,7 +19,7 @@ private: /** all modules to evaluate */ std::vector*> modules; - RandomGenerator rnd; + Random::RandomGenerator rnd; public: diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 543059b..80a3dea 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -32,7 +32,7 @@ namespace GW3 { template class WalkerDirectDestination : public WalkerBase { - //RandomGenerator rnd; + //Random::RandomGenerator rnd; std::vector*> evals; diff --git a/main.cpp b/main.cpp index 3c2ccd8..939cb5e 100755 --- a/main.cpp +++ b/main.cpp @@ -90,9 +90,9 @@ int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); // skip all tests starting with LIVE_ - //::testing::GTEST_FLAG(filter) = "*Barometer*"; + //::testing::GTEST_FLAG(filter) = "*Barometer*"; - //::testing::GTEST_FLAG(filter) = "*Distribution.T*"; + ::testing::GTEST_FLAG(filter) = "*Distribution*"; //::testing::GTEST_FLAG(filter) = "*RingBuffer*"; //::testing::GTEST_FLAG(filter) = "*Grid.*"; @@ -109,7 +109,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; + //::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/math/Distributions.h b/math/Distributions.h index 1f2bb44..40d49a4 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -10,5 +10,6 @@ #include "distribution/Triangle.h" #include "distribution/NormalN.h" #include "distribution/Rectangular.h" +#include "distribution/NormalCDF.h" #endif // DISTRIBUTIONS_H diff --git a/math/DrawList.h b/math/DrawList.h index dd034ad..00d098b 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -3,7 +3,7 @@ #include -#include "Random.h" +#include "random/RandomGenerator.h" #include "../Assertions.h" /** @@ -41,13 +41,13 @@ private: std::vector elements; /** the used random number generator */ - RandomGenerator& gen; + Random::RandomGenerator& gen; private: /** default random generator. fallback */ - RandomGenerator defRndGen; + Random::RandomGenerator defRndGen; public: @@ -63,7 +63,7 @@ public: } /** ctor with custom RandomNumberGenerator */ - DrawList(RandomGenerator& gen) : cumProbability(0), gen(gen) { + DrawList(Random::RandomGenerator& gen) : cumProbability(0), gen(gen) { ; } diff --git a/math/Random.h b/math/Random.h deleted file mode 100644 index ae8d8c5..0000000 --- a/math/Random.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef RANDOM_H -#define RANDOM_H - -#include -#include -#include "../misc/Time.h" - -#ifdef USE_FIXED_SEED - #define RANDOM_SEED 1234 -#else - #define RANDOM_SEED ( std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1) ) -#endif - -//using RandomGenerator = std::minstd_rand; - -class RandomGenerator : public std::minstd_rand { - -public: - - /** ctor with default seed */ - RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} - - /** ctor with custom seed */ - RandomGenerator(result_type seed) : std::minstd_rand(seed) {;} - -}; - -#endif // RANDOM_H diff --git a/math/distribution/Exponential.h b/math/distribution/Exponential.h index db5dabe..39a81fc 100644 --- a/math/distribution/Exponential.h +++ b/math/distribution/Exponential.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" namespace Distribution { @@ -14,7 +14,7 @@ namespace Distribution { const T lambda; - RandomGenerator gen; + Random::RandomGenerator gen; std::exponential_distribution dist; public: diff --git a/math/distribution/KernelDensity.h b/math/distribution/KernelDensity.h index a103e3e..21be963 100644 --- a/math/distribution/KernelDensity.h +++ b/math/distribution/KernelDensity.h @@ -8,7 +8,7 @@ #include #include "../../Assertions.h" -#include "../Random.h" +#include "../random/RandomGenerator.h" namespace Distribution { diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index b40ce67..9424974 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" namespace Distribution { @@ -17,7 +17,7 @@ namespace Distribution { const T sigma; const T _a; - RandomGenerator gen; + Random::RandomGenerator gen; std::normal_distribution dist; public: diff --git a/math/distribution/NormalCDF.h b/math/distribution/NormalCDF.h new file mode 100644 index 0000000..62ba191 --- /dev/null +++ b/math/distribution/NormalCDF.h @@ -0,0 +1,65 @@ +#ifndef NORMALCDF_H +#define NORMALCDF_H + +#include +#include +#include "../../Assertions.h" + +namespace Distribution { + + /** cumulative density version of the normal distribution */ + template class NormalCDF { + + private: + + const T mu; + const T sigma; + + static T RationalApproximation(T t) + { + // Abramowitz and Stegun formula 26.2.23. + // The absolute value of the error should be less than 4.5 e-4. + T c[] = {2.515517, 0.802853, 0.010328}; + T d[] = {1.432788, 0.189269, 0.001308}; + return t - ((c[2]*t + c[1])*t + c[0]) / + (((d[2]*t + d[1])*t + d[0])*t + 1.0); + } + + public: + + /** create a new normally distributed CDF */ + NormalCDF(const T mu, const T sigma) : mu(mu), sigma(sigma) { + ; + } + + /** get the probability for val within the underlying CDF */ + T getProbability(const T val) const { + return getProbability(mu, sigma, val); + } + + /** calculate the probability within the underlying CDF */ + static T getProbability(const T mu, const T sigma, const T val) { + return (1.0 + std::exp( (val - mu) / (sigma * std::sqrt(2)) ) ) / 2.0; + } + + /** get the inverse CDF (https://en.wikipedia.org/wiki/Probit)*/ + static T getProbit(T p){ + + Assert::isBetween(p, 0.0, 1.0, "value not between"); + + // See: https://www.johndcook.com/blog/normal_cdf_inverse/ + if (p < 0.5) + { + // F^-1(p) = - G^-1(p) + return -RationalApproximation( sqrt(-2.0*log(p)) ); + } + else + { + // F^-1(p) = G^-1(1-p) + return RationalApproximation( sqrt(-2.0*log(1-p)) ); + } + } + }; +} + +#endif // NORMALCDF_H diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index 5075dea..db86011 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -7,7 +7,7 @@ #include #include "../../Assertions.h" -#include "../Random.h" +#include "../random/RandomGenerator.h" namespace Distribution { @@ -24,7 +24,7 @@ namespace Distribution { const Eigen::SelfAdjointEigenSolver eigenSolver; Eigen::MatrixXd transform; //can i make this const? - RandomGenerator gen; + Random::RandomGenerator gen; std::normal_distribution<> dist; public: diff --git a/math/distribution/Rectangular.h b/math/distribution/Rectangular.h index 3f3a5fb..45e0b40 100644 --- a/math/distribution/Rectangular.h +++ b/math/distribution/Rectangular.h @@ -4,7 +4,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" #include "Normal.h" diff --git a/math/distribution/Region.h b/math/distribution/Region.h index 884cf3f..d21c23b 100644 --- a/math/distribution/Region.h +++ b/math/distribution/Region.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" #include "Normal.h" diff --git a/math/distribution/Triangle.h b/math/distribution/Triangle.h index dbffbff..d385848 100644 --- a/math/distribution/Triangle.h +++ b/math/distribution/Triangle.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" #include "Normal.h" diff --git a/math/distribution/Uniform.h b/math/distribution/Uniform.h index d6011fe..6c17452 100644 --- a/math/distribution/Uniform.h +++ b/math/distribution/Uniform.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include @@ -14,7 +14,7 @@ namespace Distribution { private: - RandomGenerator gen; + Random::RandomGenerator gen; /** depending on T, Dist is either a uniform_real or uniform_int distribution */ typedef typename std::conditional< std::is_floating_point::value, std::uniform_real_distribution, std::uniform_int_distribution >::type Dist; diff --git a/math/stats/Statistics.h b/math/stats/Statistics.h new file mode 100644 index 0000000..ed4609b --- /dev/null +++ b/math/stats/Statistics.h @@ -0,0 +1,169 @@ +#ifndef STATISTICS_H +#define STATISTICS_H + +#include +#include +#include +#include + +namespace Stats { + + /** + * store values here and get statistics about their + * avg, median, std-dev, etc. + */ + template class Statistics { + + public: + + /** ctor */ + Statistics() : sum(), sumSquared(), cnt() {;} + + /** dtor */ + ~Statistics() {;} + + /** add a new value */ + void add(T value) { + ++cnt; + sum += value; + sumSquared += (value*value); + values.insert(value); + } + + /** add all values from the given statistics instance */ + void add(const Statistics& other) { + for (const T val : other.values) { + this->add(val); + } + } + + /** get the std-dev of all values */ + T getStdDev() const { + double E1 = sumSquared / (double) cnt; + double E2 = getAvg(); + return std::sqrt(E1 - (E2*E2)); + } + + /** get average value */ + T getAvg() const { + return sum / (double) cnt; + } + + /** get the given quantile (e.g. 0.5 for median) */ + T getQuantile(const double q) const { + if (q < 0) {return *values.begin();} + if (q >= 1) {return *(--values.end());} + uint32_t pos = cnt * q; + uint32_t curPos = 0; + for (auto val : values) { + if (curPos == pos) {return val;} + ++curPos; + } + return -1; + } + + /** get the median value (= Quantile 0.5) */ + T getMedian() const { + return getQuantile(0.5f); + } + + /** get smallest value */ + T getMin() const { + if (values.empty()) {return -1;} + return *(values.begin()); + } + + /** get largest value */ + T getMax() const { + if (values.empty()) {return -1;} + return *(--values.end()); + } + + /** get the range between min an max */ + T getRange() const { + return getMax() - getMin(); + } + + /** get the squared sum */ + T getSquaredSum() const { + return sumSquared; + } + + + /** get the sum of all values */ + T getSum() const { + return sum; + } + + /** get number of stored values */ + uint32_t getCount() const { + return cnt; + } + + /** get the number of values that are below "val" */ + T getNumValuesBelow(uint32_t val) const { + uint32_t numFound = 0; + for (auto curVal : values) { + if (curVal > val) {return numFound;} + ++numFound; + } + return numFound; + } + + /** get all contained values in ascending order */ + const std::multiset& getAll() const { + return values; + } + + /** get as string */ + std::string asString() const { + std::stringstream ss; + appendTo(ss); + return ss.str(); + } + + /** send to the given stream */ + void appendTo(std::ostream& out) const { + out.precision(6); + out.setf( std::ios::fixed, std:: ios::floatfield ); + out << "cnt(" << getCount() << ")\t"; + out << "min(" << getMin() << ")\t"; + out << "max(" << getMax() << ")\t"; + out << "range(" << getRange() << ")\t"; + out << "med(" << getMedian() << ")\t"; + out << "avg(" << getAvg() << ")\t"; + out << "stdDev(" << getStdDev() << ")\t"; + } + + /** send to stream */ + inline std::ostream& operator << (std::ostream& out) const { + appendTo(out); return out; + } + + /** reset all statistics */ + void reset() { + sum = T(); + sumSquared = T(); + cnt = 0; + values.clear(); + } + + private: + + /** sum of all added values */ + T sum; + + /** squared sum of all added values (for std-dev) */ + T sumSquared; + + /** the number of added values */ + uint32_t cnt; + + /** multiset to sort all values */ + std::multiset values; + + }; + +} + +#endif // STATISTICS_H diff --git a/sensors/activity/ActivityButterPressurePercent.h b/sensors/activity/ActivityButterPressurePercent.h index 80541fc..9dba76b 100644 --- a/sensors/activity/ActivityButterPressurePercent.h +++ b/sensors/activity/ActivityButterPressurePercent.h @@ -139,17 +139,17 @@ public: float densityStairsUp = Distribution::Normal::getProbability(-muStairs, variance, actValue); float densityElevatorUp = Distribution::Normal::getProbability(-muEleveator, variance, actValue); - _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!"); - _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!"); - _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!"); - _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!"); - _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!"); + Assert::isTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!"); + Assert::isTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!"); + Assert::isTrue( (densityStay == densityStay), "the probability of densityStay is null!"); + Assert::isTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!"); + Assert::isTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!"); - _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!"); - _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!"); - _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!"); - _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!"); - _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!"); + Assert::isTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!"); + Assert::isTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!"); + Assert::isTrue( (densityStay != 0.0f), "the probability of densityStay is null!"); + Assert::isTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!"); + Assert::isTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!"); //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert. densities[0] = densityElevatorDown; diff --git a/sensors/imu/MagnetometerData.h b/sensors/imu/MagnetometerData.h index 24136c7..ecd02d3 100644 --- a/sensors/imu/MagnetometerData.h +++ b/sensors/imu/MagnetometerData.h @@ -36,7 +36,7 @@ struct MagnetometerData { } float magnitude() const { - return std::sqrt( x*x + y*y + z*z ); + return std::sqrt( x*x + y*y + z*z ); } MagnetometerData& operator += (const MagnetometerData& o) { @@ -73,9 +73,9 @@ private: }; namespace std { - MagnetometerData sqrt(const MagnetometerData& o) { - return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z)); - } + inline MagnetometerData sqrt(const MagnetometerData& o) { + return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z)); + } } #endif // INDOOR_IMU_MAGNETOMETERDATA_H diff --git a/tests/math/TestDistribution.cpp b/tests/math/TestDistribution.cpp index 8886d45..68d5655 100644 --- a/tests/math/TestDistribution.cpp +++ b/tests/math/TestDistribution.cpp @@ -69,7 +69,33 @@ TEST(Distribution, VonMises) { } +TEST(Distribution, normalCDF1) { + Distribution::NormalCDF nd(0,1); + ASSERT_NEAR(0.5, nd.getProbability(0), 0.00001); + ASSERT_NEAR(0.5, Distribution::NormalCDF::getProbability(0, 1, 0), 0.00001); + + ASSERT_NEAR(1.0, nd.getProbability(5), 0.00001); + ASSERT_NEAR(1.0, Distribution::NormalCDF::getProbability(0, 1, 5), 0.00001); + + ASSERT_NEAR(0.0, nd.getProbability(-5), 0.00001); + ASSERT_NEAR(0.0, Distribution::NormalCDF::getProbability(0, 1, -5), 0.00001); + +} + +TEST(Distribution, normalCDF2) { + + Distribution::NormalCDF nd(3,1); + ASSERT_NEAR(0.5, nd.getProbability(3), 0.00001); + ASSERT_NEAR(0.5, Distribution::NormalCDF::getProbability(3, 1, 3), 0.00001); + + ASSERT_NEAR(1.0, nd.getProbability(3+5), 0.00001); + ASSERT_NEAR(1.0, Distribution::NormalCDF::getProbability(3, 1, 3+5), 0.00001); + + ASSERT_NEAR(0.0, nd.getProbability(3-5), 0.00001); + ASSERT_NEAR(0.0, Distribution::NormalCDF::getProbability(3, 1, 3-5), 0.00001); + +} //#include diff --git a/tests/math/filter/TestButter.cpp b/tests/math/filter/TestButter.cpp index 1946e1a..d0f39d6 100644 --- a/tests/math/filter/TestButter.cpp +++ b/tests/math/filter/TestButter.cpp @@ -5,7 +5,7 @@ #include "../../../misc/Time.h" #include "../../../math/Interpolator.h" -#include"../../../sensors/pressure/ActivityButterPressure.h" +#include"../../../sensors/activity/ActivityButterPressure.h" #include #include diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp index eb76a40..8ec30bf 100644 --- a/tests/sensors/imu/TestMotionDetection.cpp +++ b/tests/sensors/imu/TestMotionDetection.cpp @@ -97,7 +97,8 @@ TEST(MotionDetection, motionAxis) { TEST(MotionDetection, motionAngle) { MotionDetection md; - TurnDetection td; + PoseDetection pd; + TurnDetection td(&pd); //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; @@ -147,7 +148,7 @@ TEST(MotionDetection, motionAngle) { } else if (e.type == Offline::Sensor::ACC) { const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; - td.addAccelerometer(ts, _acc.data); + pd.addAccelerometer(ts, _acc.data); } else if (e.type == Offline::Sensor::GYRO) { const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; diff --git a/tests/sensors/imu/TestTurnDetection.cpp b/tests/sensors/imu/TestTurnDetection.cpp index 9af88f3..0aa2d69 100644 --- a/tests/sensors/imu/TestTurnDetection.cpp +++ b/tests/sensors/imu/TestTurnDetection.cpp @@ -6,33 +6,34 @@ TEST(TurnDetection, rotationMatrix) { - Eigen::Vector3f dst; dst << 0, 0, 1; - Eigen::Vector3f src; src << 1, 1, 0; src.normalize(); + Vector3 dst(0, 0, 1); + Vector3 src(1, 1, 0); + src = src.normalized(); // get a matrix that rotates "src" into "dst" - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst); + Matrix3 rot = PoseDetection::getRotationMatrix(src, dst); - Eigen::Vector3f res = rot * src; + Vector3 res = rot * src; - ASSERT_NEAR(dst(0), res(0), 0.01); - ASSERT_NEAR(dst(1), res(1), 0.01); - ASSERT_NEAR(dst(2), res(2), 0.01); + ASSERT_NEAR(dst.x, res.x, 0.01); + ASSERT_NEAR(dst.y, res.y, 0.01); + ASSERT_NEAR(dst.z, res.z, 0.01); } TEST(TurnDetection, gyroRotate) { - Eigen::Vector3f zAxis; zAxis << 0, 0, 1; - Eigen::Vector3f acc; acc << 0, 7.0, 7.0; + Vector3 zAxis(0, 0, 1); + Vector3 acc(0, 7.0, 7.0); - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis); + Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis); - Eigen::Vector3f gyro; gyro << 0, 60, 60; + Vector3 gyro(0, 60, 60); - Eigen::Vector3f gyro2; gyro2 << 0, 0, 84; + Vector3 gyro2(0, 0, 84); - Eigen::Vector3f gyro3 = rot * gyro; + Vector3 gyro3 = rot * gyro; ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0); @@ -41,10 +42,11 @@ TEST(TurnDetection, gyroRotate) { TEST(TurnDetection, xx) { - Eigen::Vector3f dst; dst << 0, 0, 1; - Eigen::Vector3f src; src << 0.0, 2.9, -10.0; src.normalize(); // sample accelerometer readings + Vector3 dst(0, 0, 1); + Vector3 src(0.0, 2.9, -10.0); + src = src.normalized(); // sample accelerometer readings - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst); + Matrix3 rot = PoseDetection::getRotationMatrix(src, dst); // Eigen::Vector3f x; x << 1, 0, 0; // Eigen::Vector3f z = src.normalized(); @@ -55,14 +57,14 @@ TEST(TurnDetection, xx) { // rot.row(1) = y; // rot.row(2) = z; - Eigen::Vector3f res = rot * src; + Vector3 res = rot * src; // ASSERT_NEAR(dst(0), res(0), 0.01); // ASSERT_NEAR(dst(1), res(1), 0.01); // ASSERT_NEAR(dst(2), res(2), 0.01); - Eigen::Vector3f gyro; gyro << 0, 10, 30; + Vector3 gyro(0, 10, 30); - Eigen::Vector3f gyro2 = rot * gyro; + Vector3 gyro2 = rot * gyro; int i = 0; (void) i; } diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 7c3451f..7ad31f6 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -4,8 +4,8 @@ #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" -#include "../../../sensors/pressure/ActivityButterPressure.h" -#include "../../../sensors/pressure/ActivityButterPressurePercent.h" +#include "../../../sensors/activity/ActivityButterPressure.h" +#include "../../../sensors/activity/ActivityButterPressurePercent.h" #include @@ -141,9 +141,10 @@ TEST(Barometer, Activity) { double value; while (iss >> ts >> value) { - ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + act.add(Timestamp::fromMS(ts), BarometerData(value)); + Activity currentAct = act.get(); rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); - actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); + actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData((int)currentAct))); } }