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

ref #39 #40 moved all stuff left in KLib into Indoor. We are now able to perform localization without the need of KLib. Only K::Gnuplot is needed for drawing, but this will be separated into an own project in the future

Browse Source
This commit is contained in:
toni
2017-11-15 17:43:32 +01:00
parent 72932ad90f
commit c8063bc862
35 changed files with 340 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Assertions.h
View File

@@ -51,7 +51,7 @@ namespace Assert {
if (v != nullptr) {doThrow(err);}
}
template <typename T, typename STR> static inline void isNotNull(const T v, const STR err) {
template <typename T, typename STR> static inline void isNotNull(const T& v, const STR err) {
if (v == nullptr) {doThrow(err);}
}

3
CMakeLists.txt
View File

@@ -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)

2
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
View File

@@ -35,7 +35,7 @@ private:
DrawList<T&> drawer;
/** fast random-number-generator */
RandomGenerator gen;
Random::RandomGenerator gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkPathControl.h
View File

@@ -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<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkPushForward.h
View File

@@ -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<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkRandomHeadingUpdate.h
View File

@@ -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<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkRandomHeadingUpdateAdv.h
View File

@@ -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<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkShortestPathControl.h
View File

@@ -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<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkSimpleControl.h
View File

@@ -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<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkWeighted.h
View File

@@ -39,7 +39,7 @@ private:
DrawList<T&> drawer;
/** fast random-number-generator */
RandomGenerator gen;
Random::RandomGenerator gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/GridWalkWeighted2.h
View File

@@ -40,7 +40,7 @@ private:
DrawList<T&> drawer;
/** fast random-number-generator */
RandomGenerator gen;
Random::RandomGenerator gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

2
grid/walk/TestWalkWeighted3.h
View File

@@ -40,7 +40,7 @@ private:
DrawList<T&> drawer;
/** fast random-number-generator */
RandomGenerator gen;
Random::RandomGenerator gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

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

@@ -19,7 +19,7 @@ private:
/** all modules to evaluate */
std::vector<WalkModule<Node, WalkState>*> modules;
RandomGenerator rnd;
Random::RandomGenerator rnd;
public:

2
grid/walk/v3/Walker.h
View File

@@ -32,7 +32,7 @@ namespace GW3 {
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
//RandomGenerator rnd;
//Random::RandomGenerator rnd;
std::vector<WalkEvaluator<Node>*> evals;

6
main.cpp
View File

@@ -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*";

1
math/Distributions.h
View File

@@ -10,5 +10,6 @@
#include "distribution/Triangle.h"
#include "distribution/NormalN.h"
#include "distribution/Rectangular.h"
#include "distribution/NormalCDF.h"
#endif // DISTRIBUTIONS_H

8
math/DrawList.h
View File

@@ -3,7 +3,7 @@
#include <vector>
#include "Random.h"
#include "random/RandomGenerator.h"
#include "../Assertions.h"
/**
@@ -41,13 +41,13 @@ private:
std::vector<Entry> 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) {
;
}

28
math/Random.h
View File

@@ -1,28 +0,0 @@
#ifndef RANDOM_H
#define RANDOM_H
#include <cmath>
#include <random>
#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

4
math/distribution/Exponential.h
View File

@@ -3,7 +3,7 @@
#include <cmath>
#include <random>
#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<T> dist;
public:

2
math/distribution/KernelDensity.h
View File

@@ -8,7 +8,7 @@
#include <eigen3/Eigen/Dense>
#include "../../Assertions.h"
#include "../Random.h"
#include "../random/RandomGenerator.h"
namespace Distribution {

4
math/distribution/Normal.h
View File

@@ -3,7 +3,7 @@
#include <cmath>
#include <random>
#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<T> dist;
public:

65
math/distribution/NormalCDF.h Normal file
View File

@@ -0,0 +1,65 @@
#ifndef NORMALCDF_H
#define NORMALCDF_H
#include <algorithm>
#include <random>
#include "../../Assertions.h"
namespace Distribution {
/** cumulative density version of the normal distribution */
template <typename T> 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

4
math/distribution/NormalN.h
View File

@@ -7,7 +7,7 @@
#include <eigen3/Eigen/Dense>
#include "../../Assertions.h"
#include "../Random.h"
#include "../random/RandomGenerator.h"
namespace Distribution {
@@ -24,7 +24,7 @@ namespace Distribution {
const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver;
Eigen::MatrixXd transform; //can i make this const?
RandomGenerator gen;
Random::RandomGenerator gen;
std::normal_distribution<> dist;
public:

2
math/distribution/Rectangular.h
View File

@@ -4,7 +4,7 @@
#include <cmath>
#include <random>
#include "../Random.h"
#include "../random/RandomGenerator.h"
#include "../../Assertions.h"
#include "Normal.h"

2
math/distribution/Region.h
View File

@@ -3,7 +3,7 @@
#include <cmath>
#include <random>
#include "../Random.h"
#include "../random/RandomGenerator.h"
#include "../../Assertions.h"
#include "Normal.h"

2
math/distribution/Triangle.h
View File

@@ -3,7 +3,7 @@
#include <cmath>
#include <random>
#include "../Random.h"
#include "../random/RandomGenerator.h"
#include "../../Assertions.h"
#include "Normal.h"

4
math/distribution/Uniform.h
View File

@@ -3,7 +3,7 @@
#include <cmath>
#include <random>
#include "../Random.h"
#include "../random/RandomGenerator.h"
#include <type_traits>
@@ -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<T>::value, std::uniform_real_distribution<T>, std::uniform_int_distribution<T> >::type Dist;

169
math/stats/Statistics.h Normal file
View File

@@ -0,0 +1,169 @@
#ifndef STATISTICS_H
#define STATISTICS_H
#include <set>
#include <cstdint>
#include <sstream>
#include <cmath>
namespace Stats {
/**
* store values here and get statistics about their
* avg, median, std-dev, etc.
*/
template <typename T> 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<T>& 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<T>& 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<T> values;
};
}
#endif // STATISTICS_H

20
sensors/activity/ActivityButterPressurePercent.h
View File

@@ -139,17 +139,17 @@ public:
float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
float densityElevatorUp = Distribution::Normal<float>::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;

8
sensors/imu/MagnetometerData.h
View File

@@ -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

26
tests/math/TestDistribution.cpp
View File

@@ -69,7 +69,33 @@ TEST(Distribution, VonMises) {
}
TEST(Distribution, normalCDF1) {
Distribution::NormalCDF<double> nd(0,1);
ASSERT_NEAR(0.5, nd.getProbability(0), 0.00001);
ASSERT_NEAR(0.5, Distribution::NormalCDF<double>::getProbability(0, 1, 0), 0.00001);
ASSERT_NEAR(1.0, nd.getProbability(5), 0.00001);
ASSERT_NEAR(1.0, Distribution::NormalCDF<double>::getProbability(0, 1, 5), 0.00001);
ASSERT_NEAR(0.0, nd.getProbability(-5), 0.00001);
ASSERT_NEAR(0.0, Distribution::NormalCDF<double>::getProbability(0, 1, -5), 0.00001);
}
TEST(Distribution, normalCDF2) {
Distribution::NormalCDF<double> nd(3,1);
ASSERT_NEAR(0.5, nd.getProbability(3), 0.00001);
ASSERT_NEAR(0.5, Distribution::NormalCDF<double>::getProbability(3, 1, 3), 0.00001);
ASSERT_NEAR(1.0, nd.getProbability(3+5), 0.00001);
ASSERT_NEAR(1.0, Distribution::NormalCDF<double>::getProbability(3, 1, 3+5), 0.00001);
ASSERT_NEAR(0.0, nd.getProbability(3-5), 0.00001);
ASSERT_NEAR(0.0, Distribution::NormalCDF<double>::getProbability(3, 1, 3-5), 0.00001);
}
//#include <fstream>

2
tests/math/filter/TestButter.cpp
View File

@@ -5,7 +5,7 @@
#include "../../../misc/Time.h"
#include "../../../math/Interpolator.h"
#include"../../../sensors/pressure/ActivityButterPressure.h"
#include"../../../sensors/activity/ActivityButterPressure.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>

5
tests/sensors/imu/TestMotionDetection.cpp
View File

@@ -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<AccelerometerData>& _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<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];

40
tests/sensors/imu/TestTurnDetection.cpp
View File

@@ -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;
}

9
tests/sensors/pressure/TestBarometer.cpp
View File

@@ -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 <random>
@@ -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)));
}
}