fixed some compiler warnings
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:
@@ -14,286 +14,320 @@
|
||||
#include "../../sensors/imu/GravityData.h"
|
||||
#include "../../sensors/imu/LinearAccelerationData.h"
|
||||
#include "../../sensors/beacon/BeaconMeasurements.h"
|
||||
|
||||
#include "../../sensors/gps/GPSData.h"
|
||||
#include "../../sensors/imu/CompassData.h"
|
||||
|
||||
#include "../../geo/Point2.h"
|
||||
#include "../../grid/factory/v2/GridFactory.h"
|
||||
#include "../../grid/factory/v2/Importance.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 {
|
||||
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) {;}
|
||||
};
|
||||
namespace Offline {
|
||||
|
||||
enum class Sensor {
|
||||
ACC,
|
||||
GYRO,
|
||||
WIFI,
|
||||
POS,
|
||||
BARO,
|
||||
BEACON,
|
||||
LIN_ACC,
|
||||
GRAVITY,
|
||||
};
|
||||
class FileReader {
|
||||
|
||||
/** 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) {;}
|
||||
};
|
||||
public:
|
||||
|
||||
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;
|
||||
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;
|
||||
std::vector<TS<GPSData>> gps;
|
||||
std::vector<TS<CompassData>> compass;
|
||||
|
||||
/** ALL entries */
|
||||
std::vector<Entry> entries;
|
||||
/** ALL entries */
|
||||
std::vector<Entry> entries;
|
||||
|
||||
public:
|
||||
static constexpr char sep = ';';
|
||||
|
||||
FileReader(const std::string& file) {
|
||||
parse(file);
|
||||
}
|
||||
public:
|
||||
|
||||
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);
|
||||
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
|
||||
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
|
||||
|
||||
while(!inp.eof() && !inp.bad()) {
|
||||
const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
|
||||
|
||||
uint64_t ts;
|
||||
char delim;
|
||||
int idx = -1;
|
||||
std::string data;
|
||||
const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
|
||||
|
||||
inp >> ts;
|
||||
inp >> delim;
|
||||
inp >> idx;
|
||||
inp >> delim;
|
||||
inp >> data;
|
||||
private:
|
||||
|
||||
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);}
|
||||
void parse(const std::string& file) {
|
||||
|
||||
// TODO: this is a hack...
|
||||
// the loop is called one additional time after the last entry
|
||||
// and keeps the entries of entry
|
||||
std::ifstream inp(file);
|
||||
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
|
||||
|
||||
}
|
||||
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(';');
|
||||
const auto pos2 = data.find(';', pos1+1);
|
||||
// TODO: this is a hack...
|
||||
// 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)));
|
||||
lin_acc.push_back(elem);
|
||||
entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));
|
||||
}
|
||||
inp.close();
|
||||
|
||||
void parseGravity(const uint64_t ts, const std::string& data){
|
||||
}
|
||||
|
||||
const auto pos1 = data.find(';');
|
||||
const auto pos2 = data.find(';', pos1+1);
|
||||
void parseLinearAcceleration(const uint64_t ts, const std::string& data){
|
||||
|
||||
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);
|
||||
const auto pos1 = data.find(';');
|
||||
const auto pos2 = data.find(';', pos1+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));
|
||||
}
|
||||
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);
|
||||
|
||||
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(';');
|
||||
const auto pos2 = data.find(';', pos1+1);
|
||||
void parseGravity(const uint64_t ts, const std::string& data){
|
||||
|
||||
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);
|
||||
const auto pos1 = data.find(';');
|
||||
const auto pos2 = data.find(';', pos1+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));
|
||||
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 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 pos2 = data.find(';', pos1+1);
|
||||
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);
|
||||
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));
|
||||
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 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
|
||||
wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements()));
|
||||
entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-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);
|
||||
|
||||
// process all APs
|
||||
while(!tmp.empty()) {
|
||||
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));
|
||||
|
||||
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);
|
||||
void parseWiFi(const uint64_t ts, const std::string& data) {
|
||||
|
||||
tmp = tmp.substr(pos3);
|
||||
assert(tmp[0] == ';'); tmp = tmp.substr(1);
|
||||
WiFiMeasurements wifi;
|
||||
Splitter s(data, sep);
|
||||
|
||||
// append AP to current scan-entry
|
||||
WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
|
||||
wifi.back().data.entries.push_back(e);
|
||||
}
|
||||
for (size_t i = 0; i < s.size(); i += 3) {
|
||||
|
||||
}
|
||||
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);
|
||||
const std::string rssi = data.substr(pos1+1, pos2);
|
||||
const std::string txp = data.substr(pos2+1, pos3);
|
||||
// add new wifi reading
|
||||
this->wifi.push_back(TS<WiFiMeasurements>(ts, wifi));
|
||||
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(';');
|
||||
std::string gtIndex = data.substr(0, pos1);
|
||||
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);
|
||||
|
||||
TS<int> elem(ts, std::stoi(gtIndex));
|
||||
groundTruth.push_back(elem);
|
||||
//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 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:
|
||||
const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const {
|
||||
BarometerData baro;
|
||||
Splitter s(data, sep);
|
||||
|
||||
// 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){
|
||||
baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN);
|
||||
|
||||
//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!");
|
||||
}
|
||||
TS<BarometerData> elem(ts, baro);
|
||||
barometer.push_back(elem);
|
||||
entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// bringe diese in richtige reihenfolge und füge timestamp hinzu
|
||||
Interpolator<uint64_t, Point3> interpol;
|
||||
void parseCompass(const uint64_t ts, const std::string& data) {
|
||||
|
||||
int it = 0;
|
||||
for(int id : gtPath){
|
||||
auto itMap = waypointsMap.find(id);
|
||||
if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");}
|
||||
CompassData compass;
|
||||
Splitter s(data, sep);
|
||||
|
||||
//the time, when the gt button was clicked on the app
|
||||
uint64_t tsGT = groundTruth[it++].ts;
|
||||
interpol.add(tsGT, itMap->second);
|
||||
compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
|
||||
compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
|
||||
|
||||
}
|
||||
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
|
||||
|
||||
92
sensors/offline/FileWriter.h
Normal file
92
sensors/offline/FileWriter.h
Normal 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
33
sensors/offline/Listener.h
Normal 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
|
||||
@@ -12,8 +12,14 @@
|
||||
#include "../radio/WiFiMeasurements.h"
|
||||
#include "../imu/AccelerometerData.h"
|
||||
#include "../imu/GyroscopeData.h"
|
||||
#include "../imu/CompassData.h"
|
||||
#include "../gps/GPSData.h"
|
||||
#include "../pressure/BarometerData.h"
|
||||
|
||||
#include "Splitter.h"
|
||||
#include "Listener.h"
|
||||
#include "Sensors.h"
|
||||
|
||||
template <typename SensorData> struct OfflineEntry {
|
||||
|
||||
Timestamp ts;
|
||||
@@ -35,20 +41,12 @@ struct WalkedPath {
|
||||
};
|
||||
|
||||
|
||||
/** listener for event callbacks */
|
||||
class OfflineAndroidListener {
|
||||
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;
|
||||
};
|
||||
|
||||
/** read recorded android sensor data files */
|
||||
/**
|
||||
* read sensor data files that were recorded using
|
||||
* the old java android app
|
||||
*/
|
||||
class OfflineAndroid {
|
||||
|
||||
|
||||
private:
|
||||
|
||||
std::vector<OfflineEntry<WiFiMeasurements>> wifi;
|
||||
@@ -57,11 +55,15 @@ private:
|
||||
|
||||
std::vector<OfflineEntry<AccelerometerData>> accel;
|
||||
std::vector<OfflineEntry<AccelerometerData>> gravity;
|
||||
std::vector<OfflineEntry<CompassData>> compass;
|
||||
|
||||
std::vector<OfflineEntry<BarometerData>> barometer;
|
||||
|
||||
std::vector<OfflineEntry<GPSData>> gps;
|
||||
|
||||
WalkedPath walkedPath;
|
||||
|
||||
static constexpr char sep = ';';
|
||||
const char* name = "OfflineData";
|
||||
|
||||
public:
|
||||
@@ -89,6 +91,12 @@ public:
|
||||
/** get all barometer readings */
|
||||
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 */
|
||||
const WalkedPath& getWalkedPath() const {return walkedPath;}
|
||||
@@ -105,7 +113,7 @@ 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::tick();
|
||||
@@ -152,47 +160,61 @@ public:
|
||||
private:
|
||||
|
||||
/** 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
|
||||
switch(sensorID) {
|
||||
|
||||
case 0: {
|
||||
case (int) Offline::Sensor::ACC: {
|
||||
const AccelerometerData data = parseAccelerometer(sensorData);
|
||||
accel.push_back(OfflineEntry<AccelerometerData>(ts, data));
|
||||
if (listener) {listener->onAccelerometer(ts, data);}
|
||||
break;
|
||||
}
|
||||
|
||||
case 1: {
|
||||
case (int) Offline::Sensor::GRAVITY: {
|
||||
const AccelerometerData data = parseAccelerometer(sensorData);
|
||||
gravity.push_back(OfflineEntry<AccelerometerData>(ts, data));
|
||||
if (listener) {listener->onGravity(ts, data);}
|
||||
break;
|
||||
}
|
||||
|
||||
case 3: {
|
||||
case (int) Offline::Sensor::GYRO: {
|
||||
const GyroscopeData data = parseGyroscope(sensorData);
|
||||
gyro.push_back(OfflineEntry<GyroscopeData>(ts, data));
|
||||
if (listener) {listener->onGyroscope(ts, data);}
|
||||
break;
|
||||
}
|
||||
|
||||
case 5: {
|
||||
case (int) Offline::Sensor::BARO: {
|
||||
const BarometerData data = parseBarometer(sensorData);
|
||||
barometer.push_back(OfflineEntry<BarometerData>(ts, data));
|
||||
if (listener) {listener->onBarometer(ts, data);}
|
||||
break;
|
||||
}
|
||||
|
||||
case 8: {
|
||||
case (int) Offline::Sensor::WIFI: {
|
||||
const WiFiMeasurements data = parseWiFi(ts, sensorData);
|
||||
wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
|
||||
if (listener) {listener->onWiFi(ts, data);}
|
||||
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);
|
||||
groundTruth.push_back(OfflineEntry<GroundTruthID>(ts, data));
|
||||
// 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
|
||||
|
||||
38
sensors/offline/Sensors.h
Normal file
38
sensors/offline/Sensors.h
Normal 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
53
sensors/offline/Splitter.h
Normal 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
|
||||
Reference in New Issue
Block a user