many small changes, added filereader with beacons, added motion detection stuff, testcases

CMakeLists.txt

@@ -68,7 +68,7 @@ ADD_DEFINITIONS(
 	-fstack-protector-all
 
 	-g3
-	-O0
+        #-O0
 	-march=native
 
 	-DWITH_TESTS

floorplan/v2/FloorplanReader.h

@@ -281,12 +281,12 @@ namespace Floorplan {
 			Beacon* b = new Beacon();
 			b->mac = n->Attribute("mac");
 			b->name = n->Attribute("name");
-			b->major = n->Attribute("major");
-			b->minor = n->Attribute("minor");
-			b->uuid = n->Attribute("uuid");
-			b->model.txp = n->FloatAttribute("mdl_txp");
-			b->model.exp = n->FloatAttribute("mdl_exp");
-			b->model.waf = n->FloatAttribute("mdl_waf");
+//            b->major = n->Attribute("major");
+//            b->minor = n->Attribute("minor");
+//            b->uuid = n->Attribute("uuid");
+//            b->model.txp = n->FloatAttribute("mdl_txp");
+//            b->model.exp = n->FloatAttribute("mdl_exp");
+//            b->model.waf = n->FloatAttribute("mdl_waf");
 			b->pos = parsePoint3(n);
 			return b;
 		}

main.cpp

@@ -25,12 +25,12 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Grid.*";
 	//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
 
-    ::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
+    //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
     //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
 
 
-
-    ::testing::GTEST_FLAG(filter) = "*Barometer*";
+    ::testing::GTEST_FLAG(filter) = "*MotionDetection*";
+    //::testing::GTEST_FLAG(filter) = "*Barometer*";
 	//::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*";
 
 	//::testing::GTEST_FLAG(filter) = "Heading*";

sensors/imu/GravityData.h

@@ -0,0 +1,53 @@
+#ifndef GRAVITYDATA_H
+#define GRAVITYDATA_H
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from an accelerometer sensor */
+struct GravityData {
+
+	float x;
+	float y;
+	float z;
+
+    GravityData() : x(0), y(0), z(0) {;}
+
+    GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+    GravityData& operator += (const GravityData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+    GravityData& operator -= (const GravityData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+    GravityData operator - (const GravityData& o) const {
+        return GravityData(x-o.x, y-o.y, z-o.z);
+	}
+
+    GravityData operator / (const float val) const {
+        return GravityData(x/val, y/val, z/val);
+	}
+
+        std::string asString() const {
+            std::stringstream ss;
+            ss << "(" << x << "," << y << "," << z << ")";
+            return ss.str();
+        }
+
+};
+
+#endif // GRAVITYDATA_H

sensors/imu/LinearAccelerationData.h

@@ -0,0 +1,53 @@
+#ifndef LINEARACCELERATIONDATA_H
+#define LINEARACCELERATIONDATA_H
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from an accelerometer sensor */
+struct LinearAccelerationData {
+
+	float x;
+	float y;
+	float z;
+
+    LinearAccelerationData() : x(0), y(0), z(0) {;}
+
+    LinearAccelerationData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+    LinearAccelerationData& operator += (const LinearAccelerationData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+    LinearAccelerationData& operator -= (const LinearAccelerationData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+    LinearAccelerationData operator - (const LinearAccelerationData& o) const {
+        return LinearAccelerationData(x-o.x, y-o.y, z-o.z);
+	}
+
+    LinearAccelerationData operator / (const float val) const {
+        return LinearAccelerationData(x/val, y/val, z/val);
+	}
+
+        std::string asString() const {
+            std::stringstream ss;
+            ss << "(" << x << "," << y << "," << z << ")";
+            return ss.str();
+        }
+
+};
+
+#endif // LINEARACCELERATIONDATA_H

sensors/imu/MotionDetection.h

@@ -0,0 +1,163 @@
+#ifndef MOTIONDETECTION_H
+#define MOTIONDETECTION_H
+
+#include "GravityData.h"
+#include "LinearAccelerationData.h"
+#include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../misc/Debug.h"
+
+#include <eigen3/Eigen/Dense>
+
+#include <cmath>
+#include <vector>
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+
+
+#include "../../Assertions.h"
+
+class MotionDetection {
+
+private:
+
+    bool newAccelerationMeasurementArrived = false;
+    bool newGravityMeasurementArrived = false;
+
+    Eigen::Vector3f curGravity;
+    Eigen::Vector3f curLinearAcceleration;
+
+    //fast algo
+    Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix
+
+    int numMeasurementsPerInterval, updateCnt;
+    int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms
+
+    struct Motion{
+        Eigen::Vector2f vec = Eigen::Vector2f::Identity();
+        Timestamp lastEstimation;
+    };
+
+    Motion curMotion;
+    Motion prevMotion;
+
+    const char* name = "MotionDetection";
+
+public:
+
+	/** ctor */
+    MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) {
+        ;
+	}
+
+    void addGravity(const Timestamp& ts, const GravityData& grav){
+
+            curGravity << grav.x, grav.y, grav.z;
+            newGravityMeasurementArrived = true;
+
+            updateProjectionVectorFast(ts);
+    }
+
+    void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) {
+
+            curLinearAcceleration << acc.x, acc.y, acc.z;
+            newAccelerationMeasurementArrived = true;
+
+            updateProjectionVectorFast(ts);
+    }
+
+    /** return the current motion axis
+     * NOTE: if no data is available, this vector is the Identity
+    */
+    Eigen::Vector2f getCurrentMotionAxis(){
+        return curMotion.vec;
+    }
+
+    /** returns the radians between [-pi, pi] between successive motion axis estimations */
+    float getMotionChangeInRad(){
+        //TODO: put this in an EigenHelper Class within geo
+        const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x();
+        const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f));
+
+        //nan?
+        if(std::isnan(ang)){
+            Log::add(name, "The motion change angle is nAn, this is not correct!");
+        }
+
+        if(updateCnt < 2){
+            return 0;
+        }
+
+        return ang;
+    }
+
+private:
+
+    FRIEND_TEST(MotionDetection, motionAngle);
+    FRIEND_TEST(MotionDetection, motionAxis);
+
+    Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){
+
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covarianceMatrix);
+        return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue
+    }
+
+    void updateProjectionVectorFast(const Timestamp& ts){
+
+        //make sure we have both measurements for calculation
+        if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){
+
+            numMeasurementsPerInterval++;
+
+            //project acc into perpendicular plane of grav (using standard vector projection)
+            Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity;
+
+            //if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this
+            if(proj.isZero()){
+                proj = curLinearAcceleration;
+                Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?");
+            }
+
+            //we are only interested in x,y
+            Eigen::Vector2f vec;
+            vec << proj.x(), proj.y();
+
+            // sum this up for later averaging.
+            sumProjectedCov += vec*vec.transpose();
+
+            // start with the first available timestamp
+            if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;}
+
+            //update the motion axis depending on the update interval
+            if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){
+
+                prevMotion = curMotion;
+
+                //calculate the average of the coveriance matrix
+                Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval;
+
+                curMotion.vec = updateMotionAxis(Q);
+                curMotion.lastEstimation = ts;
+
+                reset();
+            }
+
+            newGravityMeasurementArrived = false;
+            newAccelerationMeasurementArrived = false;
+        }
+        //do nothing
+    }
+
+    void reset(){
+        numMeasurementsPerInterval = 0;
+        sumProjectedCov = Eigen::Matrix2f::Zero();
+        ++updateCnt;
+    }
+
+};
+
+#endif // MOTIONDETECTION_H

sensors/offline/FileReader.h

@@ -0,0 +1,299 @@
+#ifndef FILEREADER_H
+#define FILEREADER_H
+
+#include <fstream>
+#include <Indoor/Exception.h>
+#include <vector>
+#include <unordered_map>
+
+#include "../../math/Interpolator.h"
+#include "../../sensors/radio/WiFiMeasurements.h"
+#include "../../sensors/pressure/BarometerData.h"
+#include "../../sensors/imu/AccelerometerData.h"
+#include "../../sensors/imu/GyroscopeData.h"
+#include "../../sensors/imu/GravityData.h"
+#include "../../sensors/imu/LinearAccelerationData.h"
+#include "../../sensors/beacon/BeaconMeasurements.h"
+
+
+#include "../../geo/Point2.h"
+#include "../../grid/factory/v2/GridFactory.h"
+#include "../../grid/factory/v2/Importance.h"
+#include "../../floorplan/v2/Floorplan.h"
+
+class FileReader {
+
+public:
+
+    template <typename T> struct TS {
+        const uint64_t ts;
+        T data;
+        TS(const uint64_t ts) : ts(ts) {;}
+        TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
+    };
+
+    enum class Sensor {
+        ACC,
+        GYRO,
+        WIFI,
+        POS,
+        BARO,
+        BEACON,
+        LIN_ACC,
+        GRAVITY,
+    };
+
+    /** entry for one sensor */
+    struct Entry {
+        Sensor type;
+        uint64_t ts;
+        int idx;
+        Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
+    };
+
+    std::vector<TS<int>> groundTruth;
+    std::vector<TS<WiFiMeasurements>> wifi;
+    std::vector<TS<BeaconMeasurement>> beacon;
+    std::vector<TS<AccelerometerData>> acc;
+    std::vector<TS<GyroscopeData>> gyro;
+    std::vector<TS<BarometerData>> barometer;
+    std::vector<TS<LinearAccelerationData>> lin_acc;
+    std::vector<TS<GravityData>> gravity;
+
+    /** ALL entries */
+    std::vector<Entry> entries;
+
+public:
+
+    FileReader(const std::string& file) {
+        parse(file);
+    }
+
+    const std::vector<Entry>& getEntries() const {return entries;}
+
+
+    const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
+
+    const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
+
+    const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
+
+    const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
+
+    const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
+
+    const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
+
+    const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
+
+    const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
+
+private:
+
+    void parse(const std::string& file) {
+
+        std::ifstream inp(file);
+        if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
+
+        while(!inp.eof() && !inp.bad()) {
+
+            uint64_t ts;
+            char delim;
+            int idx = -1;
+            std::string data;
+
+            inp >> ts;
+            inp >> delim;
+            inp >> idx;
+            inp >> delim;
+            inp >> data;
+
+            if		(idx == 8)	{parseWiFi(ts, data);}
+            else if	(idx == 9)	{parseBeacons(ts, data);}
+            else if	(idx == 99) {parseGroundTruth(ts, data);}
+            else if	(idx == 0)  {parseAccelerometer(ts, data);}
+            else if	(idx == 3)  {parseGyroscope(ts, data);}
+            else if (idx == 5)	{parseBarometer(ts, data);}
+            else if (idx == 2)  {parseLinearAcceleration(ts,data);}
+            else if (idx == 1)  {parseGravity(ts,data);}
+
+            // TODO: this is a hack...
+            // the loop is called one additional time after the last entry
+            // and keeps the entries of entry
+
+        }
+
+        inp.close();
+
+    }
+
+    void parseLinearAcceleration(const uint64_t ts, const std::string& data){
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<LinearAccelerationData> elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z)));
+        lin_acc.push_back(elem);
+        entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));
+    }
+
+    void parseGravity(const uint64_t ts, const std::string& data){
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<GravityData> elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z)));
+        gravity.push_back(elem);
+        entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));
+    }
+
+    void parseAccelerometer(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
+        acc.push_back(elem);
+        entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
+
+    }
+
+    void parseGyroscope(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
+        gyro.push_back(elem);
+        entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
+
+    }
+
+    void parseWiFi(const uint64_t ts, const std::string& data) {
+
+        std::string tmp = data;
+
+        // add new wifi reading
+        wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements()));
+        entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1));
+
+        // process all APs
+        while(!tmp.empty()) {
+
+            auto pos1 = tmp.find(';');
+            auto pos2 = tmp.find(';', pos1+1);
+            auto pos3 = tmp.find(';', pos2+1);
+
+            std::string mac = tmp.substr(0, pos1);
+            std::string freq = tmp.substr(pos1+1, pos2);
+            std::string rssi = tmp.substr(pos2+1, pos3);
+
+            tmp = tmp.substr(pos3);
+            assert(tmp[0] == ';'); tmp = tmp.substr(1);
+
+            // append AP to current scan-entry
+            WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
+            wifi.back().data.entries.push_back(e);
+        }
+
+    }
+
+    void parseBeacons(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+        const auto pos3 = data.find(';', pos2+1);
+
+        const std::string mac = data.substr(0, pos1);
+        const std::string rssi = data.substr(pos1+1, pos2);
+        const std::string txp = data.substr(pos2+1, pos3);
+
+        //yes the timestamp is redundant here, but in case of multiusage...
+        TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
+        beacon.push_back(e);
+        entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
+
+    }
+
+    void parseGroundTruth(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        std::string gtIndex = data.substr(0, pos1);
+
+        TS<int> elem(ts, std::stoi(gtIndex));
+        groundTruth.push_back(elem);
+
+    }
+
+    void parseBarometer(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+
+        const std::string hPa = data.substr(0, pos1);
+
+        TS<BarometerData> elem(ts, BarometerData(std::stof(hPa)));
+        barometer.push_back(elem);
+        entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
+
+    }
+
+public:
+    const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const {
+
+        // finde alle positionen der waypoints im gtPath aus map
+        std::unordered_map<int, Point3> waypointsMap;
+        for(Floorplan::Floor* f : map->floors){
+            float h = f->atHeight;
+            for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){
+
+                //wenn die gleiche id 2x vergeben wurde, knallt es
+                if(waypointsMap.find(gtp->id) == waypointsMap.end()){
+                    waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)});
+                }
+                else{
+                    throw std::string("the floorplan's ground truth contains two points with identical id's!");
+                }
+
+            }
+        }
+
+        // bringe diese in richtige reihenfolge und füge timestamp hinzu
+        Interpolator<uint64_t, Point3> interpol;
+
+        int it = 0;
+        for(int id : gtPath){
+            auto itMap = waypointsMap.find(id);
+            if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");}
+
+            //the time, when the gt button was clicked on the app
+            uint64_t tsGT = groundTruth[it++].ts;
+            interpol.add(tsGT, itMap->second);
+
+        }
+
+        if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){
+            throw std::string("No Ground Truth points found within the map.xml file");
+        }
+
+        return interpol;
+    }
+
+};
+
+#endif // FILEREADER_H

tests/sensors/imu/TestMotionDetection.cpp

@@ -0,0 +1,200 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/imu/MotionDetection.h"
+#include "../../../sensors/imu/TurnDetection.h"
+#include "../../../sensors/offline/FileReader.h"
+
+/** visualize the motionAxis */
+TEST(MotionDetection, motionAxis) {
+
+    MotionDetection md;
+
+    //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";
+
+    //Walking with smartphone straight and always parallel to motion axis
+    //std::string filename = getDataFile("motion/straight_potrait.csv");
+
+    //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
+    std::string filename = getDataFile("motion/straight_landscape_left.csv");
+    //std::string filename = getDataFile("motion/straight_landscape_right.csv");
+
+    //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
+    //std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
+
+    //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait.csv");
+
+    //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_landscape.csv");
+
+    //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
+
+    //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
+    //std::string filename = getDataFile("motion/rounds_pocket.csv");
+
+    //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
+    //std::string filename = getDataFile("motion/table_flat.csv");
+
+    FileReader fr(filename);
+
+    K::Gnuplot gp;
+    K::GnuplotPlot plot;
+
+    gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
+
+
+    Eigen::Vector2f curVec;
+    float motionAxisAngleRad;
+    Timestamp ts;
+    Timestamp lastTs;
+
+    //calc motion axis
+    for (const FileReader::Entry& e : fr.getEntries()) {
+
+        ts = Timestamp::fromMS(e.ts);
+
+        if (e.type == FileReader::Sensor::LIN_ACC) {
+            md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
+
+        } else if (e.type == FileReader::Sensor::GRAVITY) {
+            md.addGravity(ts, fr.getGravity()[e.idx].data);
+            curVec = md.getCurrentMotionAxis();
+            motionAxisAngleRad = md.getMotionChangeInRad();
+        }
+
+        // start with the first available timestamp
+        if (lastTs.isZero()) {lastTs = ts;}
+
+        if(ts - lastTs > Timestamp::fromMS(500)) {
+
+            lastTs = ts;
+
+            K::GnuplotPoint2 raw_p1(0, 0);
+            K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
+            K::GnuplotPlotElementLines motionLines;
+            motionLines.addSegment(raw_p1, raw_p2);
+            plot.add(&motionLines);
+
+            gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159  << "' at screen 0.1,0.1\n";
+
+            gp.draw(plot);
+            gp.flush();
+            //usleep(5000*33);
+        }
+    }
+
+    //was passiert bei grenzwerten. 90° oder sowas.
+    //wie stabil ist die motion axis eigentlich?
+    //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
+    //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
+
+}
+
+/** comparing motionAngle and turnAngle */
+TEST(MotionDetection, motionAngle) {
+
+    MotionDetection md;
+    TurnDetection td;
+
+    //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";
+
+    //Walking with smartphone straight and always parallel to motion axis
+    std::string filename = getDataFile("motion/straight_potrait.csv");
+
+    //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
+    //std::string filename = getDataFile("motion/straight_landscape_left.csv");
+    //std::string filename = getDataFile("motion/straight_landscape_right.csv");
+
+    //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
+    //std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
+
+    //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait.csv");
+
+    //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_landscape.csv");
+
+    //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
+
+    //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
+    //std::string filename = getDataFile("motion/rounds_pocket.csv");
+
+    //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
+    //std::string filename = getDataFile("motion/table_flat.csv");
+
+    FileReader fr(filename);
+    Timestamp ts;
+
+    //save for later plotting
+    std::vector<float> delta_motionAngles;
+    std::vector<float> delta_turnAngles;
+
+    //calc motion axis
+    for (const FileReader::Entry& e : fr.getEntries()) {
+
+        ts = Timestamp::fromMS(e.ts);
+
+        if (e.type == FileReader::Sensor::LIN_ACC) {
+            md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
+
+        } else if (e.type == FileReader::Sensor::GRAVITY) {
+            md.addGravity(ts, fr.getGravity()[e.idx].data);
+            delta_motionAngles.push_back(md.getMotionChangeInRad());
+
+        } else if (e.type == FileReader::Sensor::ACC) {
+            const FileReader::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
+            td.addAccelerometer(ts, _acc.data);
+
+        } else if (e.type == FileReader::Sensor::GYRO) {
+            const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
+            delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
+        }
+
+    }
+
+    //draw motion
+    static K::Gnuplot gpMotion;
+    K::GnuplotPlot plotMotion;
+    K::GnuplotPlotElementLines motionLines;
+
+    for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
+
+        K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
+        K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
+        motionLines.addSegment(raw_p1, raw_p2);
+
+    }
+
+    gpMotion << "set title 'Motion Detection'\n";
+    plotMotion.add(&motionLines);
+    gpMotion.draw(plotMotion);
+    gpMotion.flush();
+
+
+    //draw rotation
+    static K::Gnuplot gpTurn;
+    K::GnuplotPlot plotTurn;
+    K::GnuplotPlotElementLines turnLines;
+
+    for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
+
+        K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
+        K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
+        turnLines.addSegment(raw_p1, raw_p2);
+    }
+
+    gpTurn << "set title 'Turn Detection'\n";
+    plotTurn.add(&turnLines);
+    gpTurn.draw(plotTurn);
+    gpTurn.flush();
+
+    sleep(1);
+
+}
+
+
+#endif