411 lines
11 KiB
C++
411 lines
11 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef OFFLINEANDROID_H
|
||
#define OFFLINEANDROID_H
|
||
|
||
#include <fstream>
|
||
#include <string>
|
||
|
||
#include "../../misc/Debug.h"
|
||
#include "../../Assertions.h"
|
||
#include "../../math/Interpolator.h"
|
||
#include "../../geo/Point3.h"
|
||
#include "../../data/Timestamp.h"
|
||
#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;
|
||
SensorData data;
|
||
|
||
/** ctor */
|
||
OfflineEntry(const Timestamp ts, const SensorData& data) : ts(ts), data(data) {;}
|
||
|
||
};
|
||
|
||
struct GroundTruthID {
|
||
int id;
|
||
GroundTruthID() {;}
|
||
GroundTruthID(const int id) : id(id) {;}
|
||
};
|
||
|
||
struct WalkedPath {
|
||
std::vector<Point3> pos_cm;
|
||
};
|
||
|
||
|
||
/**
|
||
* read sensor data files that were recorded using
|
||
* the old java android app
|
||
*/
|
||
class OfflineAndroid {
|
||
|
||
private:
|
||
|
||
std::vector<OfflineEntry<WiFiMeasurements>> wifi;
|
||
std::vector<OfflineEntry<GroundTruthID>> groundTruth;
|
||
std::vector<OfflineEntry<GyroscopeData>> gyro;
|
||
|
||
std::vector<OfflineEntry<AccelerometerData>> accel;
|
||
std::vector<OfflineEntry<GravityData>> 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:
|
||
|
||
/** ctor */
|
||
OfflineAndroid() {
|
||
;
|
||
}
|
||
|
||
/** get all ground truth readings */
|
||
const std::vector<OfflineEntry<GroundTruthID>>& getGroundTruth() const {return groundTruth;}
|
||
|
||
/** get all WiFi readings */
|
||
const std::vector<OfflineEntry<WiFiMeasurements>>& getWiFi() const {return wifi;}
|
||
|
||
/** get all gyroscope readings */
|
||
const std::vector<OfflineEntry<GyroscopeData>>& getGyroscope() const {return gyro;}
|
||
|
||
/** get all accelerometer readings */
|
||
const std::vector<OfflineEntry<AccelerometerData>>& getAccelerometer() const {return accel;}
|
||
|
||
/** get all gravity readings */
|
||
const std::vector<OfflineEntry<GravityData>>& getGravity() const {return gravity;}
|
||
|
||
/** 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;}
|
||
|
||
/** get the interpolated walked path */
|
||
Interpolator<Timestamp, Point3> getWalkedPathInterpolatorCM() const {
|
||
Assert::equal(getWalkedPath().pos_cm.size(), getGroundTruth().size(), "entry-number mismatch!");
|
||
Interpolator<Timestamp, Point3> interpol;
|
||
for (const OfflineEntry<GroundTruthID>& entry : getGroundTruth()) {
|
||
interpol.add(entry.ts, getWalkedPath().pos_cm[entry.data.id]);
|
||
}
|
||
return interpol;
|
||
}
|
||
|
||
public:
|
||
|
||
void parse(const std::string& file, Offline::Listener* listener = nullptr) {
|
||
|
||
Log::add(name, "parsing data file: " + file , false);
|
||
Log::tick();
|
||
|
||
// open the stream
|
||
std::ifstream inp(file);
|
||
|
||
// sanity check
|
||
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open " +file);}
|
||
|
||
// parse one line
|
||
while(!inp.eof()) {
|
||
|
||
// temporals
|
||
uint64_t ts;
|
||
char delim;
|
||
int32_t sensorID = 999999; // avoids issues with the last line
|
||
std::string sensorData;
|
||
|
||
// read from file
|
||
inp >> ts;
|
||
inp >> delim;
|
||
inp >> sensorID;
|
||
inp >> delim;
|
||
inp >> sensorData;
|
||
|
||
if (delim == 0) {break;}
|
||
|
||
parse(Timestamp::fromMS(ts), sensorID, sensorData, listener);
|
||
|
||
}
|
||
|
||
Log::tock();
|
||
|
||
Log::add(name,
|
||
"gyro(" + std::to_string(gyro.size()) + ") " +
|
||
"accel(" + std::to_string(accel.size()) + ") "
|
||
"wifi(" + std::to_string(wifi.size()) + ") " +
|
||
"gt(" + std::to_string(groundTruth.size()) + ") "
|
||
);
|
||
|
||
}
|
||
|
||
private:
|
||
|
||
/** parse the given data */
|
||
void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, Offline::Listener* listener) {
|
||
|
||
// how to parse
|
||
switch(sensorID) {
|
||
|
||
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 (int) Offline::Sensor::GRAVITY: {
|
||
const GravityData data = parseGravity(sensorData);
|
||
gravity.push_back(OfflineEntry<GravityData>(ts, data));
|
||
if (listener) {listener->onGravity(ts, data);}
|
||
break;
|
||
}
|
||
|
||
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 (int) Offline::Sensor::BARO: {
|
||
const BarometerData data = parseBarometer(sensorData);
|
||
barometer.push_back(OfflineEntry<BarometerData>(ts, data));
|
||
if (listener) {listener->onBarometer(ts, data);}
|
||
break;
|
||
}
|
||
|
||
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 (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
|
||
break;
|
||
}
|
||
|
||
case 100: {
|
||
walkedPath = parseWalkedPath(sensorData);
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
|
||
|
||
/** parse the given WiFiObservation string "MAC;freq;RSSI;MAC;freq;RSSI;...." */
|
||
static inline WiFiMeasurements parseWiFi(const Timestamp ts, std::string data) {
|
||
|
||
WiFiMeasurements obs;
|
||
|
||
// process all APs
|
||
while(!data.empty()) {
|
||
|
||
const std::string mac = data.substr(0, 17);
|
||
data = data.substr(17);
|
||
Assert::isTrue(data[0] == ';', "unexpected character");
|
||
data = data.substr(1);
|
||
|
||
const std::string freq = data.substr(0, 4);
|
||
data = data.substr(4);
|
||
Assert::isTrue(data[0] == ';', "unexpected character");
|
||
data = data.substr(1);
|
||
|
||
const int pos = data.find(';');
|
||
const std::string rssi = data.substr(0, pos);
|
||
data = data.substr(pos);
|
||
Assert::isTrue(data[0] == ';', "unexpected character");
|
||
data = data.substr(1);
|
||
|
||
const WiFiMeasurement e(mac, std::stof(rssi), ts);
|
||
obs.entries.push_back(e);
|
||
|
||
}
|
||
|
||
return obs;
|
||
|
||
}
|
||
|
||
static inline GyroscopeData parseGyroscope(const std::string& data) {
|
||
|
||
const size_t pos1 = data.find(';', 0);
|
||
const size_t pos2 = data.find(';', pos1+1);
|
||
const size_t pos3 = data.find(';', pos2+1);
|
||
|
||
Assert::isTrue(pos1 != std::string::npos, "format error");
|
||
Assert::isTrue(pos2 != std::string::npos, "format error");
|
||
Assert::isTrue(pos3 != std::string::npos, "format error");
|
||
|
||
const std::string sx = data.substr(0, pos1);
|
||
const std::string sy = data.substr(pos1+1, pos2-pos1-1);
|
||
const std::string sz = data.substr(pos2+1, pos3-pos2-1);
|
||
|
||
return GyroscopeData(std::stof(sx), std::stof(sy), std::stof(sz));
|
||
|
||
}
|
||
|
||
|
||
static inline AccelerometerData parseAccelerometer(const std::string& data) {
|
||
|
||
const size_t pos1 = data.find(';', 0);
|
||
const size_t pos2 = data.find(';', pos1+1);
|
||
const size_t pos3 = data.find(';', pos2+1);
|
||
|
||
Assert::isTrue(pos1 != std::string::npos, "format error");
|
||
Assert::isTrue(pos2 != std::string::npos, "format error");
|
||
Assert::isTrue(pos3 != std::string::npos, "format error");
|
||
|
||
const std::string sx = data.substr(0, pos1);
|
||
const std::string sy = data.substr(pos1+1, pos2-pos1-1);
|
||
const std::string sz = data.substr(pos2+1, pos3-pos2-1);
|
||
|
||
return AccelerometerData(std::stof(sx), std::stof(sy), std::stof(sz));
|
||
|
||
}
|
||
|
||
static inline GravityData parseGravity(const std::string& data) {
|
||
|
||
const size_t pos1 = data.find(';', 0);
|
||
const size_t pos2 = data.find(';', pos1+1);
|
||
const size_t pos3 = data.find(';', pos2+1);
|
||
|
||
Assert::isTrue(pos1 != std::string::npos, "format error");
|
||
Assert::isTrue(pos2 != std::string::npos, "format error");
|
||
Assert::isTrue(pos3 != std::string::npos, "format error");
|
||
|
||
const std::string sx = data.substr(0, pos1);
|
||
const std::string sy = data.substr(pos1+1, pos2-pos1-1);
|
||
const std::string sz = data.substr(pos2+1, pos3-pos2-1);
|
||
|
||
return GravityData(std::stof(sx), std::stof(sy), std::stof(sz));
|
||
|
||
}
|
||
|
||
/** parse the given Barometer entry */
|
||
static inline BarometerData parseBarometer(const std::string& data) {
|
||
|
||
BarometerData baro;
|
||
const int pos = data.find(';');
|
||
baro.hPa = std::stof(data.substr(0, pos));
|
||
return baro;
|
||
|
||
}
|
||
|
||
/** parse the given GroundTruth entry */
|
||
static inline GroundTruthID parseGroundTruthTick(const std::string& data) {
|
||
|
||
GroundTruthID id;
|
||
const int pos = data.find(';');
|
||
id.id = std::stoi(data.substr(0, pos));
|
||
return id;
|
||
|
||
}
|
||
|
||
|
||
/** parse the given WalkedPath string "x,y,z;x,y,z;x,y,..." */
|
||
static inline WalkedPath parseWalkedPath(std::string data) {
|
||
|
||
WalkedPath path;
|
||
|
||
// process all points
|
||
while(!data.empty()) {
|
||
|
||
const int pos1 = data.find(',');
|
||
const int pos2 = data.find(',', pos1+1);
|
||
const int pos3 = data.find(';', 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, pos3-pos2-1);
|
||
|
||
path.pos_cm.push_back(Point3(std::stof(x), std::stof(y), std::stof(z)));
|
||
|
||
data = data.substr(pos3+1);
|
||
|
||
}
|
||
|
||
return path;
|
||
|
||
}
|
||
|
||
/** 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
|