diff --git a/CMakeLists.txt b/CMakeLists.txt index d8d105a..d7b87de 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O0 + #-O0 -march=native -DWITH_TESTS diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 7f00748..66ffe3d 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/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; } diff --git a/main.cpp b/main.cpp index 03d1e84..fbb4e08 100755 --- a/main.cpp +++ 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*"; diff --git a/sensors/imu/GravityData.h b/sensors/imu/GravityData.h new file mode 100644 index 0000000..d522f20 --- /dev/null +++ b/sensors/imu/GravityData.h @@ -0,0 +1,53 @@ +#ifndef GRAVITYDATA_H +#define GRAVITYDATA_H + +#include +#include + + +/** 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 diff --git a/sensors/imu/LinearAccelerationData.h b/sensors/imu/LinearAccelerationData.h new file mode 100644 index 0000000..9ae116c --- /dev/null +++ b/sensors/imu/LinearAccelerationData.h @@ -0,0 +1,53 @@ +#ifndef LINEARACCELERATIONDATA_H +#define LINEARACCELERATIONDATA_H + +#include +#include + + +/** 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 diff --git a/sensors/imu/MotionDetection.h b/sensors/imu/MotionDetection.h new file mode 100644 index 0000000..76284fe --- /dev/null +++ b/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 + +#include +#include + +#include +#include +#include +#include +#include + + +#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 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 diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h new file mode 100644 index 0000000..bc30410 --- /dev/null +++ b/sensors/offline/FileReader.h @@ -0,0 +1,299 @@ +#ifndef FILEREADER_H +#define FILEREADER_H + +#include +#include +#include +#include + +#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 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> groundTruth; + std::vector> wifi; + std::vector> beacon; + std::vector> acc; + std::vector> gyro; + std::vector> barometer; + std::vector> lin_acc; + std::vector> gravity; + + /** ALL entries */ + std::vector entries; + +public: + + FileReader(const std::string& file) { + parse(file); + } + + const std::vector& getEntries() const {return entries;} + + + const std::vector>& getGroundTruth() const {return groundTruth;} + + const std::vector>& getWiFiGroupedByTime() const {return wifi;} + + const std::vector>& getBeacons() const {return beacon;} + + const std::vector>& getAccelerometer() const {return acc;} + + const std::vector>& getGyroscope() const {return gyro;} + + const std::vector>& getBarometer() const {return barometer;} + + const std::vector>& getLinearAcceleration() const {return lin_acc;} + + const std::vector>& 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 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 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 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 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(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 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 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 elem(ts, BarometerData(std::stof(hPa))); + barometer.push_back(elem); + entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); + + } + +public: + const Interpolator getGroundTruthPath(Floorplan::IndoorMap* map, std::vector gtPath) const { + + // finde alle positionen der waypoints im gtPath aus map + std::unordered_map 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 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 diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp new file mode 100644 index 0000000..47bc411 --- /dev/null +++ b/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 delta_motionAngles; + std::vector 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& _acc = fr.getAccelerometer()[e.idx]; + td.addAccelerometer(ts, _acc.data); + + } else if (e.type == FileReader::Sensor::GYRO) { + const FileReader::TS& _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