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

fixed some compiler warnings

Browse Source 
added equality checks to sensor-data classes
more robust sensor reader [fixed some issues]
added support for gps
added support for compass
added sensor-data-writer
added test-cases
minor changes
This commit is contained in:
FrankE
2017-03-21 16:25:36 +01:00
parent 18f48e23a8
commit bb43e7f0fe
20 changed files with 807 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
geo/Point2.h
View File

@@ -3,6 +3,7 @@
#include <cmath> #include <cmath>
#include <algorithm> #include <algorithm>
#include <string>
/** /**
* 2D Point * 2D Point

2
main.cpp
View File

@@ -29,7 +29,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
::testing::GTEST_FLAG(filter) = "*FloorplanCeilings*"; ::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
//::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*Barometer*";
//::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*";

4
math/filter/Butterworth.h
View File

@@ -77,9 +77,9 @@ namespace Filter {
const Scalar _b0, _b1, _b2, _a1, _a2, _gain; const Scalar _b0, _b1, _b2, _a1, _a2, _gain;
const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2; Scalar _z1, _z2;
Scalar _z1, _z2; const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2;
}; };

21
sensors/gps/GPSData.h
View File

@@ -3,6 +3,7 @@
#include "../../data/Timestamp.h" #include "../../data/Timestamp.h"
struct GPSData { struct GPSData {
/** time this measurement was received (NOT the GPS-time) */ /** time this measurement was received (NOT the GPS-time) */
@@ -15,12 +16,32 @@ struct GPSData {
float accuracy; // m [might be NAN] float accuracy; // m [might be NAN]
float speed; // m/s [might be NAN] float speed; // m/s [might be NAN]
/** ctor for invalid/unknown data */
GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;} GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
/** ctor */
GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN) {;} GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN) {;}
/** ctor */
GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt, const float accuracy) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN) {;} GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt, const float accuracy) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN) {;}
/** data valid? */
bool isValid() const {
return (lat == lat) && (lon == lon);
}
bool operator == (const GPSData& o) const {
return EQ_OR_NAN(lat, o.lat) &&
EQ_OR_NAN(lon, o.lon) &&
EQ_OR_NAN(alt, o.alt) &&
EQ_OR_NAN(accuracy, o.accuracy) &&
EQ_OR_NAN(speed, o.speed);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
}; };
#endif // GPSDATA_H #endif // GPSDATA_H

24
sensors/imu/AccelerometerData.h
View File

@@ -42,11 +42,25 @@ struct AccelerometerData {
return AccelerometerData(x/val, y/val, z/val); return AccelerometerData(x/val, y/val, z/val);
} }
std::string asString() const { std::string asString() const {
std::stringstream ss; std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")"; ss << "(" << x << "," << y << "," << z << ")";
return ss.str(); return ss.str();
} }
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const AccelerometerData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
}; };

56
sensors/imu/CompassData.h Normal file
View File

@@ -0,0 +1,56 @@
#ifndef COMPASSDATA_H
#define COMPASSDATA_H
#include <cmath>
#include <sstream>
/** data received from a compass sensor */
struct CompassData {
/** azimuth angle. NAN if not available */
float azimuth = NAN;
/** describes the sensor's quality */
float quality01 = 0;
/** empty ctor */
CompassData() : azimuth(NAN) {;}
/** data ctor */
CompassData(const float azimuth) : azimuth(azimuth), quality01(0) {;}
/** data ctor */
CompassData(const float azimuth, const float quality01) : azimuth(azimuth), quality01(quality01) {;}
/** get an instance describing invalid data */
static CompassData INVALID() {
return CompassData(NAN);
}
/** convert to string */
std::string asString() const {
std::stringstream ss;
ss << "(" << azimuth << ")";
return ss.str();
}
/** is the compass data valid? [compass present] */
bool isValid() const {
return azimuth == azimuth;
}
bool operator == (const CompassData& o) const {
return EQ_OR_NAN(azimuth, o.azimuth) &&
EQ_OR_NAN(quality01, o.quality01);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};
#endif // COMPASSDATA_H

24
sensors/imu/GravityData.h
View File

@@ -42,11 +42,25 @@ struct GravityData {
return GravityData(x/val, y/val, z/val); return GravityData(x/val, y/val, z/val);
} }
std::string asString() const { std::string asString() const {
std::stringstream ss; std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")"; ss << "(" << x << "," << y << "," << z << ")";
return ss.str(); return ss.str();
} }
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const GravityData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
}; };

24
sensors/imu/GyroscopeData.h
View File

@@ -23,11 +23,25 @@ struct GyroscopeData {
return std::sqrt( x*x + y*y + z*z ); return std::sqrt( x*x + y*y + z*z );
} }
std::string asString() const { std::string asString() const {
std::stringstream ss; std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")"; ss << "(" << x << "," << y << "," << z << ")";
return ss.str(); return ss.str();
} }
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const GyroscopeData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
}; };

24
sensors/imu/LinearAccelerationData.h
View File

@@ -42,11 +42,25 @@ struct LinearAccelerationData {
return LinearAccelerationData(x/val, y/val, z/val); return LinearAccelerationData(x/val, y/val, z/val);
} }
std::string asString() const { std::string asString() const {
std::stringstream ss; std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")"; ss << "(" << x << "," << y << "," << z << ")";
return ss.str(); return ss.str();
} }
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const LinearAccelerationData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
}; };

422
sensors/offline/FileReader.h
View File

@@ -14,286 +14,320 @@
#include "../../sensors/imu/GravityData.h" #include "../../sensors/imu/GravityData.h"
#include "../../sensors/imu/LinearAccelerationData.h" #include "../../sensors/imu/LinearAccelerationData.h"
#include "../../sensors/beacon/BeaconMeasurements.h" #include "../../sensors/beacon/BeaconMeasurements.h"
#include "../../sensors/gps/GPSData.h"
#include "../../sensors/imu/CompassData.h"
#include "../../geo/Point2.h" #include "../../geo/Point2.h"
#include "../../grid/factory/v2/GridFactory.h" #include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/Importance.h" #include "../../grid/factory/v2/Importance.h"
#include "../../floorplan/v2/Floorplan.h" #include "../../floorplan/v2/Floorplan.h"
class FileReader { #include "Splitter.h"
#include "Sensors.h"
public: #warning "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]"
template <typename T> struct TS { namespace Offline {
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 { class FileReader {
ACC,
GYRO,
WIFI,
POS,
BARO,
BEACON,
LIN_ACC,
GRAVITY,
};
/** entry for one sensor */ public:
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<int>> groundTruth;
std::vector<TS<WiFiMeasurements>> wifi; std::vector<TS<WiFiMeasurements>> wifi;
std::vector<TS<BeaconMeasurement>> beacon; std::vector<TS<BeaconMeasurement>> beacon;
std::vector<TS<AccelerometerData>> acc; std::vector<TS<AccelerometerData>> acc;
std::vector<TS<GyroscopeData>> gyro; std::vector<TS<GyroscopeData>> gyro;
std::vector<TS<BarometerData>> barometer; std::vector<TS<BarometerData>> barometer;
std::vector<TS<LinearAccelerationData>> lin_acc; std::vector<TS<LinearAccelerationData>> lin_acc;
std::vector<TS<GravityData>> gravity; std::vector<TS<GravityData>> gravity;
std::vector<TS<GPSData>> gps;
std::vector<TS<CompassData>> compass;
/** ALL entries */ /** ALL entries */
std::vector<Entry> entries; std::vector<Entry> entries;
public: static constexpr char sep = ';';
FileReader(const std::string& file) { public:
parse(file);
}
const std::vector<Entry>& getEntries() const {return entries;} /** empty ctor. call open() */
FileReader() {
;
}
/** ctor with filename */
FileReader(const std::string& file) {
open(file);
}
const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;} /** open the given file */
void open(const std::string& file) {
parse(file);
}
const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;} const std::vector<Entry>& getEntries() const {return entries;}
const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;} const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;} const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;} const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;} const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
const std::vector<TS<GravityData>>& getGravity() const {return gravity;} const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
private: const std::vector<TS<GPSData>>& getGPS() const {return gps;}
void parse(const std::string& file) { const std::vector<TS<CompassData>>& getCompass() const {return compass;}
std::ifstream inp(file); const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
while(!inp.eof() && !inp.bad()) { const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
uint64_t ts; const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
char delim;
int idx = -1;
std::string data;
inp >> ts; private:
inp >> delim;
inp >> idx;
inp >> delim;
inp >> data;
if (idx == 8) {parseWiFi(ts, data);} void parse(const std::string& file) {
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... std::ifstream inp(file);
// the loop is called one additional time after the last entry if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
// and keeps the entries of entry
} while(!inp.eof() && !inp.bad()) {
inp.close(); uint64_t ts;
char delim;
int idx = -1;
std::string data;
} inp >> ts;
inp >> delim;
inp >> idx;
inp >> delim;
inp >> data;
void parseLinearAcceleration(const uint64_t ts, const std::string& data){ if (idx == (int)Sensor::WIFI) {parseWiFi(ts, data);}
else if (idx == (int)Sensor::BEACON) {parseBeacons(ts, data);}
else if (idx == (int)Sensor::GROUND_TRUTH) {parseGroundTruth(ts, data);}
else if (idx == (int)Sensor::ACC) {parseAccelerometer(ts, data);}
else if (idx == (int)Sensor::GYRO) {parseGyroscope(ts, data);}
else if (idx == (int)Sensor::BARO) {parseBarometer(ts, data);}
else if (idx == (int)Sensor::LIN_ACC) {parseLinearAcceleration(ts,data);}
else if (idx == (int)Sensor::GRAVITY) {parseGravity(ts,data);}
else if (idx == (int)Sensor::COMPASS) {parseCompass(ts,data);}
else if (idx == (int)Sensor::GPS) {parseGPS(ts,data);}
const auto pos1 = data.find(';'); // TODO: this is a hack...
const auto pos2 = data.find(';', pos1+1); // the loop is called one additional time after the last entry
// and keeps the entries of entry
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))); inp.close();
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(';'); void parseLinearAcceleration(const uint64_t ts, const std::string& data){
const auto pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1); const auto pos1 = data.find(';');
const std::string y = data.substr(pos1+1, pos2-pos1-1); const auto pos2 = data.find(';', pos1+1);
const std::string z = data.substr(pos2+1);
TS<GravityData> elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z))); const std::string x = data.substr(0, pos1);
gravity.push_back(elem); const std::string y = data.substr(pos1+1, pos2-pos1-1);
entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); const std::string z = data.substr(pos2+1);
}
void parseAccelerometer(const uint64_t ts, const std::string& data) { 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));
}
const auto pos1 = data.find(';'); void parseGravity(const uint64_t ts, const std::string& data){
const auto pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1); const auto pos1 = data.find(';');
const std::string y = data.substr(pos1+1, pos2-pos1-1); const auto pos2 = data.find(';', pos1+1);
const std::string z = data.substr(pos2+1);
TS<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); const std::string x = data.substr(0, pos1);
acc.push_back(elem); const std::string y = data.substr(pos1+1, pos2-pos1-1);
entries.push_back(Entry(Sensor::ACC, ts, acc.size()-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 parseGyroscope(const uint64_t ts, const std::string& data) { void parseAccelerometer(const uint64_t ts, const std::string& data) {
const auto pos1 = data.find(';'); const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1); const auto pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1); const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1); const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1); const std::string z = data.substr(pos2+1);
TS<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); TS<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
gyro.push_back(elem); acc.push_back(elem);
entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
} }
void parseWiFi(const uint64_t ts, const std::string& data) { void parseGyroscope(const uint64_t ts, const std::string& data) {
std::string tmp = data; const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1);
// add new wifi reading const std::string x = data.substr(0, pos1);
wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements())); const std::string y = data.substr(pos1+1, pos2-pos1-1);
entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1)); const std::string z = data.substr(pos2+1);
// process all APs TS<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
while(!tmp.empty()) { gyro.push_back(elem);
entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
auto pos1 = tmp.find(';'); }
auto pos2 = tmp.find(';', pos1+1);
auto pos3 = tmp.find(';', pos2+1);
std::string mac = tmp.substr(0, pos1); void parseWiFi(const uint64_t ts, const std::string& data) {
std::string freq = tmp.substr(pos1+1, pos2);
std::string rssi = tmp.substr(pos2+1, pos3);
tmp = tmp.substr(pos3); WiFiMeasurements wifi;
assert(tmp[0] == ';'); tmp = tmp.substr(1); Splitter s(data, sep);
// append AP to current scan-entry for (size_t i = 0; i < s.size(); i += 3) {
WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
wifi.back().data.entries.push_back(e);
}
} const std::string mac = s.get(i+0);
const float freq = s.getFloat(i+1);
const float rssi = s.getFloat(i+2);
void parseBeacons(const uint64_t ts, const std::string& data) { // append AP to current scan-entry
WiFiMeasurement e(AccessPoint(mac), rssi, freq, Timestamp::fromMS(ts));
wifi.entries.push_back(e);
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); // add new wifi reading
const std::string rssi = data.substr(pos1+1, pos2); this->wifi.push_back(TS<WiFiMeasurements>(ts, wifi));
const std::string txp = data.substr(pos2+1, pos3); entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1));
//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 parseBeacons(const uint64_t ts, const std::string& data) {
void parseGroundTruth(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 auto pos1 = data.find(';'); const std::string mac = data.substr(0, pos1);
std::string gtIndex = data.substr(0, pos1); const std::string rssi = data.substr(pos1+1, pos2);
const std::string txp = data.substr(pos2+1, pos3);
TS<int> elem(ts, std::stoi(gtIndex)); //yes the timestamp is redundant here, but in case of multiusage...
groundTruth.push_back(elem); 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 parseBarometer(const uint64_t ts, const std::string& data) { void parseGroundTruth(const uint64_t ts, const std::string& data) {
const auto pos1 = data.find(';'); const auto pos1 = data.find(';');
std::string gtIndex = data.substr(0, pos1);
const std::string hPa = data.substr(0, pos1); TS<int> elem(ts, std::stoi(gtIndex));
groundTruth.push_back(elem);
TS<BarometerData> elem(ts, BarometerData(std::stof(hPa))); }
barometer.push_back(elem);
entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
} void parseBarometer(const uint64_t ts, const std::string& data) {
public: BarometerData baro;
const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const { Splitter s(data, sep);
// finde alle positionen der waypoints im gtPath aus map baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN);
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 TS<BarometerData> elem(ts, baro);
if(waypointsMap.find(gtp->id) == waypointsMap.end()){ barometer.push_back(elem);
waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)}); entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
}
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 void parseCompass(const uint64_t ts, const std::string& data) {
Interpolator<uint64_t, Point3> interpol;
int it = 0; CompassData compass;
for(int id : gtPath){ Splitter s(data, sep);
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 compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
uint64_t tsGT = groundTruth[it++].ts; compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
interpol.add(tsGT, itMap->second);
} TS<CompassData> elem(ts, compass);
this->compass.push_back(elem);
entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1));
if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){ }
throw std::string("No Ground Truth points found within the map.xml file");
}
return interpol; /** parse the given GPS entry */
} void parseGPS(const uint64_t ts, const std::string& data) {
}; GPSData gps;
Splitter s(data, sep);
gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);
TS<GPSData> elem(ts, gps);
this->gps.push_back(elem);
entries.push_back(Entry(Sensor::GPS, ts, this->gps.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 #endif // FILEREADER_H

92
sensors/offline/FileWriter.h Normal file
View File

@@ -0,0 +1,92 @@
#ifndef FILEWRITER_H
#define FILEWRITER_H
#include "../gps/GPSData.h"
#include "../imu/CompassData.h"
#include "../imu/LinearAccelerationData.h"
#include "../imu/GravityData.h"
#include "../pressure/BarometerData.h"
#include "../imu/GyroscopeData.h"
#include "../imu/AccelerometerData.h"
#include "../radio/WiFiMeasurements.h"
#include "Sensors.h"
#include <fstream>
namespace Offline {
class FileWriter {
private:
std::ofstream out;
static constexpr char sep = ';';
const std::string nl = "\n";
public:
FileWriter() {
;
}
~FileWriter() {
close();
}
void open(const std::string& file) {
out.open(file);
if (!out) {throw Exception("error opening file: " + file);}
}
void close() {
out.flush();
out.close();
}
void flush() {
out.flush();
}
void add(const Timestamp ts, const AccelerometerData& data) {
out << ts.ms() << sep << (int) Sensor::ACC << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const LinearAccelerationData& data) {
out << ts.ms() << sep << (int) Sensor::LIN_ACC << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const GravityData& data) {
out << ts.ms() << sep << (int) Sensor::GRAVITY << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const GyroscopeData& data) {
out << ts.ms() << sep << (int) Sensor::GYRO << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const BarometerData& data) {
out << ts.ms() << sep << (int) Sensor::BARO << sep << data.hPa << nl;
}
void add(const Timestamp ts, const GPSData& data) {
out << ts.ms() << sep << (int) Sensor::GPS << sep << data.lat << sep << data.lon << sep << data.alt << sep << data.accuracy << sep << data.speed << nl;
}
void add(const Timestamp ts, const CompassData& data) {
out << ts.ms() << sep << (int) Sensor::COMPASS << sep << data.azimuth << sep << data.quality01 << nl;
}
void add(const Timestamp ts, const WiFiMeasurements& data) {
out << ts.ms() << sep << (int) Sensor::WIFI;
for (const WiFiMeasurement& m : data.entries) {
out << sep << m.getAP().getMAC().asString();
out << sep << m.getFrequency();
out << sep << m.getRSSI();
}
out << "\n";
}
};
}
#endif // FILEWRITER_H

33
sensors/offline/Listener.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef OFFLINE_LISTENER_H
#define OFFLINE_LISTENER_H
#include "../gps/GPSData.h"
#include "../imu/CompassData.h"
#include "../imu/GravityData.h"
#include "../pressure/BarometerData.h"
#include "../imu/GyroscopeData.h"
#include "../imu/AccelerometerData.h"
#include "../radio/WiFiMeasurements.h"
namespace Offline {
/**
* listen for events/callbacks while parsing offline files
*/
class Listener {
public:
virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0;
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
virtual void onGPS(const Timestamp ts, const GPSData data) = 0;
virtual void onCompass(const Timestamp ts, const CompassData data) = 0;
};
}
#endif // OFFLINE_LISTENER_H

91
sensors/offline/OfflineAndroid.h
View File

@@ -12,8 +12,14 @@
#include "../radio/WiFiMeasurements.h" #include "../radio/WiFiMeasurements.h"
#include "../imu/AccelerometerData.h" #include "../imu/AccelerometerData.h"
#include "../imu/GyroscopeData.h" #include "../imu/GyroscopeData.h"
#include "../imu/CompassData.h"
#include "../gps/GPSData.h"
#include "../pressure/BarometerData.h" #include "../pressure/BarometerData.h"
#include "Splitter.h"
#include "Listener.h"
#include "Sensors.h"
template <typename SensorData> struct OfflineEntry { template <typename SensorData> struct OfflineEntry {
Timestamp ts; Timestamp ts;
@@ -35,20 +41,12 @@ struct WalkedPath {
}; };
/** listener for event callbacks */ /**
class OfflineAndroidListener { * read sensor data files that were recorded using
public: * the old java android app
virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0; */
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
};
/** read recorded android sensor data files */
class OfflineAndroid { class OfflineAndroid {
private: private:
std::vector<OfflineEntry<WiFiMeasurements>> wifi; std::vector<OfflineEntry<WiFiMeasurements>> wifi;
@@ -57,11 +55,15 @@ private:
std::vector<OfflineEntry<AccelerometerData>> accel; std::vector<OfflineEntry<AccelerometerData>> accel;
std::vector<OfflineEntry<AccelerometerData>> gravity; std::vector<OfflineEntry<AccelerometerData>> gravity;
std::vector<OfflineEntry<CompassData>> compass;
std::vector<OfflineEntry<BarometerData>> barometer; std::vector<OfflineEntry<BarometerData>> barometer;
std::vector<OfflineEntry<GPSData>> gps;
WalkedPath walkedPath; WalkedPath walkedPath;
static constexpr char sep = ';';
const char* name = "OfflineData"; const char* name = "OfflineData";
public: public:
@@ -89,6 +91,12 @@ public:
/** get all barometer readings */ /** get all barometer readings */
const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;} const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}
/** get all compass readings */
const std::vector<OfflineEntry<CompassData>>& getCompass() const {return compass;}
/** get all gps readings */
const std::vector<OfflineEntry<GPSData>>& getGPS() const {return gps;}
/** get the walked path */ /** get the walked path */
const WalkedPath& getWalkedPath() const {return walkedPath;} const WalkedPath& getWalkedPath() const {return walkedPath;}
@@ -105,7 +113,7 @@ public:
public: public:
void parse(const std::string& file, OfflineAndroidListener* listener = nullptr) { void parse(const std::string& file, Offline::Listener* listener = nullptr) {
Log::add(name, "parsing data file: " + file , false); Log::add(name, "parsing data file: " + file , false);
Log::tick(); Log::tick();
@@ -152,47 +160,61 @@ public:
private: private:
/** parse the given data */ /** parse the given data */
void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, OfflineAndroidListener* listener) { void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, Offline::Listener* listener) {
// how to parse // how to parse
switch(sensorID) { switch(sensorID) {
case 0: { case (int) Offline::Sensor::ACC: {
const AccelerometerData data = parseAccelerometer(sensorData); const AccelerometerData data = parseAccelerometer(sensorData);
accel.push_back(OfflineEntry<AccelerometerData>(ts, data)); accel.push_back(OfflineEntry<AccelerometerData>(ts, data));
if (listener) {listener->onAccelerometer(ts, data);} if (listener) {listener->onAccelerometer(ts, data);}
break; break;
} }
case 1: { case (int) Offline::Sensor::GRAVITY: {
const AccelerometerData data = parseAccelerometer(sensorData); const AccelerometerData data = parseAccelerometer(sensorData);
gravity.push_back(OfflineEntry<AccelerometerData>(ts, data)); gravity.push_back(OfflineEntry<AccelerometerData>(ts, data));
if (listener) {listener->onGravity(ts, data);} if (listener) {listener->onGravity(ts, data);}
break; break;
} }
case 3: { case (int) Offline::Sensor::GYRO: {
const GyroscopeData data = parseGyroscope(sensorData); const GyroscopeData data = parseGyroscope(sensorData);
gyro.push_back(OfflineEntry<GyroscopeData>(ts, data)); gyro.push_back(OfflineEntry<GyroscopeData>(ts, data));
if (listener) {listener->onGyroscope(ts, data);} if (listener) {listener->onGyroscope(ts, data);}
break; break;
} }
case 5: { case (int) Offline::Sensor::BARO: {
const BarometerData data = parseBarometer(sensorData); const BarometerData data = parseBarometer(sensorData);
barometer.push_back(OfflineEntry<BarometerData>(ts, data)); barometer.push_back(OfflineEntry<BarometerData>(ts, data));
if (listener) {listener->onBarometer(ts, data);} if (listener) {listener->onBarometer(ts, data);}
break; break;
} }
case 8: { case (int) Offline::Sensor::WIFI: {
const WiFiMeasurements data = parseWiFi(ts, sensorData); const WiFiMeasurements data = parseWiFi(ts, sensorData);
wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data)); wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
if (listener) {listener->onWiFi(ts, data);} if (listener) {listener->onWiFi(ts, data);}
break; break;
} }
case 99: { case (int) Offline::Sensor::COMPASS: {
const CompassData data = parseCompass(sensorData);
compass.push_back(OfflineEntry<CompassData>(ts, data));
if (listener) {listener->onCompass(ts, data);}
break;
}
case (int) Offline::Sensor::GPS: {
const GPSData data = parseGPS(sensorData);
gps.push_back(OfflineEntry<GPSData>(ts, data));
if (listener) {listener->onGPS(ts, data);}
break;
}
case (int) Offline::Sensor::GROUND_TRUTH: {
const GroundTruthID data = parseGroundTruthTick(sensorData); const GroundTruthID data = parseGroundTruthTick(sensorData);
groundTruth.push_back(OfflineEntry<GroundTruthID>(ts, data)); groundTruth.push_back(OfflineEntry<GroundTruthID>(ts, data));
// TODO listener // TODO listener
@@ -326,6 +348,35 @@ private:
} }
/** parse the given Compass entry */
static inline CompassData parseCompass(const std::string& data) {
CompassData compass;
Splitter s(data, sep);
compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
return compass;
}
/** parse the given GPS entry */
static inline GPSData parseGPS(const std::string& data) {
GPSData gps;
Splitter s(data, sep);
gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);
return gps;
}
}; };
#endif // OFFLINEANDROID_H #endif // OFFLINEANDROID_H

38
sensors/offline/Sensors.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef OFFLINE_SENSORS_H
#define OFFLINE_SENSORS_H
namespace Offline {
enum class Sensor {
ACC = 0,
GRAVITY = 1,
LIN_ACC = 2,
GYRO = 3,
BARO = 5,
WIFI = 8,
BEACON = 9,
COMPASS = 15,
GPS = 16,
GROUND_TRUTH = 99,
POS = 1001, // IPIN2016
};
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) {;}
};
/** 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) {;}
};
}
#endif // OFFLINE_SENSORS_H

53
sensors/offline/Splitter.h Normal file
View File

@@ -0,0 +1,53 @@
#ifndef DATA_SPLITTER_H
#define DATA_SPLITTER_H
#include <string>
#include <vector>
/**
* split an input-file into various tokens
*/
class Splitter {
std::string str;
char sep = ';';
std::vector<std::string> split;
public:
/** ctor */
Splitter(const std::string& str, const char sep = ';') : str(str), sep(sep) {
build();
}
bool has(const int idx) const {return split.size() > idx;}
const std::string& get(const int idx) const {return split.at(idx);}
const float getFloat(const int idx) const {return std::stof(get(idx));}
size_t size() const {return split.size();}
private:
void build() {
std::string cur;
for (char c : str) {
if (c == sep) {
split.push_back(cur);
cur = "";
} else {
cur += c;
}
}
split.push_back(cur);
}
};
#endif // DATA_SPLITTER_H

13
sensors/pressure/BarometerData.h
View File

@@ -13,6 +13,19 @@ struct BarometerData {
explicit BarometerData(const float hPa) : hPa(hPa) {;} explicit BarometerData(const float hPa) : hPa(hPa) {;}
/** valid data? */
bool isValid() const {
return hPa == hPa;
}
bool operator == (const BarometerData& o ) const {
return EQ_OR_NAN(hPa, o.hPa);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
}; };
#endif // BAROMETERDATA_H #endif // BAROMETERDATA_H

4
sensors/radio/WiFiMeasurement.h
View File

@@ -20,7 +20,7 @@ private:
float rssi; float rssi;
/** OPTIONAL. frequence the signal was received */ /** OPTIONAL. frequence the signal was received */
float freq; float freq = NAN;
/** OPTIONAL. timestamp the measurement was recorded at */ /** OPTIONAL. timestamp the measurement was recorded at */
Timestamp ts; Timestamp ts;
@@ -28,7 +28,7 @@ private:
public: public:
/** ctor */ /** ctor */
WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) { WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi), freq(NAN) {
; ;
} }

34
sensors/radio/setup/WiFiOptimizerLogDistCeiling.h
View File

@@ -94,6 +94,15 @@ namespace WiFiOptimizer {
return ss.str(); return ss.str();
} }
/** we add some constraints to the parameter range */
bool outOfRange() const {
return (waf > 0) ||
(txp < -50) ||
(txp > -30) ||
(exp > 4) ||
(exp < 1);
}
}; };
/** add MAC-info to params */ /** add MAC-info to params */
@@ -136,17 +145,17 @@ namespace WiFiOptimizer {
return false; return false;
}; };
const APFilter MIN_8_FPS = [] (const int numFingerprints, const MACAddress& mac) { const APFilter MIN_5_FPS = [] (const int numFingerprints, const MACAddress& mac) {
(void) mac; (void) mac;
return numFingerprints < 8; return numFingerprints < 5;
}; };
private: private:
Mode mode = Mode::QUALITY;
Floorplan::IndoorMap* map; Floorplan::IndoorMap* map;
Mode mode = Mode::QUALITY;
const char* name = "WiFiOptLDC"; const char* name = "WiFiOptLDC";
public: public:
@@ -182,6 +191,7 @@ namespace WiFiOptimizer {
} }
const float avgErr = errSum / errCnt; const float avgErr = errSum / errCnt;
Log::add(name, "optimized APs: " + std::to_string(errCnt));
Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB"); Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
// done // done
@@ -189,6 +199,7 @@ namespace WiFiOptimizer {
} }
/** optimize the given AP */ /** optimize the given AP */
APParams optimize(const MACAddress& mac, Stats& res) const { APParams optimize(const MACAddress& mac, Stats& res) const {
@@ -210,8 +221,8 @@ namespace WiFiOptimizer {
LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20), // x LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20), // x
LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y
LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z
LeOpt::MinMax(-50, -30), // txp LeOpt::MinMax(-50, -30), // txp
LeOpt::MinMax(1, 5), // exp LeOpt::MinMax(1, 4), // exp
LeOpt::MinMax(-15, -0), // waf LeOpt::MinMax(-15, -0), // waf
}; };
@@ -271,17 +282,10 @@ namespace WiFiOptimizer {
float getErrorLogDistCeiling(const MACAddress& mac, const std::vector<RSSIatPosition>& entries, const float* data, Stats* stats = nullptr) const { float getErrorLogDistCeiling(const MACAddress& mac, const std::vector<RSSIatPosition>& entries, const float* data, Stats* stats = nullptr) const {
constexpr float hugeError = 1e10;
const APParams* params = (APParams*) data; const APParams* params = (APParams*) data;
// some sanity checks // some sanity checks
if (params->waf > 0) {return hugeError;} if (params->outOfRange()) {return 1e10;}
if (params->txp < -50) {return hugeError;}
if (params->txp > -30) {return hugeError;}
if (params->exp > 4) {return hugeError;}
if (params->exp < 1) {return hugeError;}
// current position guess for the AP; // current position guess for the AP;
const Point3 apPos_m = params->getPos(); const Point3 apPos_m = params->getPos();
@@ -309,7 +313,7 @@ namespace WiFiOptimizer {
} }
// adjust the error // adjust the error
err += diff*diff; err += std::pow(std::abs(diff), 2.0);
++cnt; ++cnt;
// max distance penality // max distance penality

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

@@ -38,7 +38,7 @@ TEST(MotionDetection, motionAxis) {
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
//std::string filename = getDataFile("motion/table_flat.csv"); //std::string filename = getDataFile("motion/table_flat.csv");
FileReader fr(filename); Offline::FileReader fr(filename);
K::Gnuplot gp; K::Gnuplot gp;
K::GnuplotPlot plot; K::GnuplotPlot plot;
@@ -52,14 +52,14 @@ TEST(MotionDetection, motionAxis) {
Timestamp lastTs; Timestamp lastTs;
//calc motion axis //calc motion axis
for (const FileReader::Entry& e : fr.getEntries()) { for (const Offline::Entry& e : fr.getEntries()) {
ts = Timestamp::fromMS(e.ts); ts = Timestamp::fromMS(e.ts);
if (e.type == FileReader::Sensor::LIN_ACC) { if (e.type == Offline::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == FileReader::Sensor::GRAVITY) { } else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data); md.addGravity(ts, fr.getGravity()[e.idx].data);
curVec = md.getCurrentMotionAxis(); curVec = md.getCurrentMotionAxis();
motionAxisAngleRad = md.getMotionChangeInRad(); motionAxisAngleRad = md.getMotionChangeInRad();
@@ -126,7 +126,7 @@ TEST(MotionDetection, motionAngle) {
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
//std::string filename = getDataFile("motion/table_flat.csv"); //std::string filename = getDataFile("motion/table_flat.csv");
FileReader fr(filename); Offline::FileReader fr(filename);
Timestamp ts; Timestamp ts;
//save for later plotting //save for later plotting
@@ -134,23 +134,23 @@ TEST(MotionDetection, motionAngle) {
std::vector<float> delta_turnAngles; std::vector<float> delta_turnAngles;
//calc motion axis //calc motion axis
for (const FileReader::Entry& e : fr.getEntries()) { for (const Offline::Entry& e : fr.getEntries()) {
ts = Timestamp::fromMS(e.ts); ts = Timestamp::fromMS(e.ts);
if (e.type == FileReader::Sensor::LIN_ACC) { if (e.type == Offline::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == FileReader::Sensor::GRAVITY) { } else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data); md.addGravity(ts, fr.getGravity()[e.idx].data);
delta_motionAngles.push_back(md.getMotionChangeInRad()); delta_motionAngles.push_back(md.getMotionChangeInRad());
} else if (e.type == FileReader::Sensor::ACC) { } else if (e.type == Offline::Sensor::ACC) {
const FileReader::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx]; const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
td.addAccelerometer(ts, _acc.data); td.addAccelerometer(ts, _acc.data);
} else if (e.type == FileReader::Sensor::GYRO) { } else if (e.type == Offline::Sensor::GYRO) {
const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx]; const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data)); delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
} }

89
tests/sensors/offline/TestReadWrite.cpp Normal file
View File

@@ -0,0 +1,89 @@
#ifdef WITH_TESTS
#include "../../Tests.h"
#include "../../../sensors/offline/FileReader.h"
#include "../../../sensors/offline/FileWriter.h"
TEST(Offline, readWrite) {
std::string fileName = "/tmp/test.dat";
Offline::FileWriter out;
out.open(fileName);
const GPSData gps(Timestamp::fromMS(1), 2, 3, 4);
out.add(Timestamp::fromMS(11), gps);
const CompassData compass(4, 2);
out.add(Timestamp::fromMS(13), compass);
const BarometerData baro(3);
out.add(Timestamp::fromMS(15), baro);
const AccelerometerData acc(3,4,5);
out.add(Timestamp::fromMS(17), acc);
const GravityData grav(5,9,7);
out.add(Timestamp::fromMS(19), grav);
const GyroscopeData gyro(8, 5,11);
out.add(Timestamp::fromMS(21), gyro);
const LinearAccelerationData lina(13, 12, 11);
out.add(Timestamp::fromMS(23), lina);
WiFiMeasurements w1;
w1.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:55:66")), -70));
w1.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:55:67")), -72));
w1.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:55:68")), -74));
out.add(Timestamp::fromMS(25), w1);
WiFiMeasurements w2;
w2.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:aa:66")), -60));
w2.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:aa:67")), -62));
w2.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:aa:68")), -64));
out.add(Timestamp::fromMS(27), w2);
out.close();
Offline::FileReader reader;
reader.open(fileName);
// check number of entries
ASSERT_EQ(1, reader.getGPS().size());
ASSERT_EQ(1, reader.getCompass().size());
ASSERT_EQ(1, reader.getBarometer().size());
ASSERT_EQ(1, reader.getAccelerometer().size());
ASSERT_EQ(1, reader.getGravity().size());
ASSERT_EQ(1, reader.getGyroscope().size());
ASSERT_EQ(1, reader.getLinearAcceleration().size());
ASSERT_EQ(2, reader.getWiFiGroupedByTime().size());
// check timestamps
ASSERT_EQ(11, reader.getGPS().front().ts);
ASSERT_EQ(13, reader.getCompass().front().ts);
ASSERT_EQ(15, reader.getBarometer().front().ts);
ASSERT_EQ(17, reader.getAccelerometer().front().ts);
ASSERT_EQ(19, reader.getGravity().front().ts);
ASSERT_EQ(21, reader.getGyroscope().front().ts);
ASSERT_EQ(23, reader.getLinearAcceleration().front().ts);
ASSERT_EQ(25, reader.getWiFiGroupedByTime().front().ts);
ASSERT_EQ(27, reader.getWiFiGroupedByTime().back().ts);
// check content
ASSERT_EQ(gps, reader.getGPS().front().data);
ASSERT_EQ(compass, reader.getCompass().front().data);
ASSERT_EQ(baro, reader.getBarometer().front().data);
ASSERT_EQ(acc, reader.getAccelerometer().front().data);
ASSERT_EQ(grav, reader.getGravity().front().data);
ASSERT_EQ(gyro, reader.getGyroscope().front().data);
ASSERT_EQ(lina, reader.getLinearAcceleration().front().data);
int i = 0; (void) i;
}
#endif