526 lines
16 KiB
C++
526 lines
16 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 FILEREADER_H
|
||
#define FILEREADER_H
|
||
|
||
#include <fstream>
|
||
#include <Indoor/Exception.h>
|
||
#include <vector>
|
||
#include <unordered_map>
|
||
|
||
#include "../../math/Interpolator.h"
|
||
#include "../../sensors/radio/WiFiMeasurements.h"
|
||
#include "../../sensors/pressure/BarometerData.h"
|
||
#include "../../sensors/imu/AccelerometerData.h"
|
||
#include "../../sensors/imu/GyroscopeData.h"
|
||
#include "../../sensors/imu/GravityData.h"
|
||
#include "../../sensors/imu/LinearAccelerationData.h"
|
||
#include "../../sensors/beacon/BeaconMeasurements.h"
|
||
#include "../../sensors/gps/GPSData.h"
|
||
#include "../../sensors/imu/CompassData.h"
|
||
#include "../../sensors/imu/MagnetometerData.h"
|
||
|
||
#include "../../geo/Point2.h"
|
||
#include "../../grid/factory/v2/GridFactory.h"
|
||
#include "../../grid/factory/v2/Importance.h"
|
||
#include "../../floorplan/v2/Floorplan.h"
|
||
#include "../../floorplan/v2/FloorplanHelper.h"
|
||
|
||
#include "Splitter.h"
|
||
#include "Sensors.h"
|
||
#include "Listener.h"
|
||
|
||
#pragma message "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]"
|
||
|
||
namespace Offline {
|
||
|
||
/**
|
||
* read and parse previously recorded ["offline"] files
|
||
*/
|
||
class FileReader {
|
||
|
||
public:
|
||
|
||
using GroundTruth = Interpolator<Timestamp, Point3>;
|
||
|
||
/** all entries grouped by sensor */
|
||
std::vector<TS<int>> groundTruth;
|
||
std::vector<TS<int>> activity;
|
||
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;
|
||
std::vector<TS<MagnetometerData>> magnetometer;
|
||
std::vector<TS<WiFiMeasurement>> wifiFtm;
|
||
|
||
/** all entries in linear order as they appeared while recording */
|
||
std::vector<Entry> entries;
|
||
|
||
static constexpr char sep = ';';
|
||
|
||
bool convertNanoSecondTimestamps = true; // otherwise use milliseconds
|
||
|
||
public:
|
||
|
||
/** empty ctor. call open() */
|
||
FileReader() {
|
||
;
|
||
}
|
||
|
||
/** ctor with filename */
|
||
FileReader(const std::string& file) {
|
||
open(file);
|
||
}
|
||
|
||
/** ctor with filename */
|
||
FileReader(const std::string& file, bool convertNanoSecondTimestamps)
|
||
: convertNanoSecondTimestamps(convertNanoSecondTimestamps)
|
||
{
|
||
open(file);
|
||
}
|
||
|
||
/** open the given file */
|
||
void open(const std::string& file) {
|
||
clear();
|
||
parse(file);
|
||
}
|
||
|
||
/** remove all parsed entries */
|
||
void clear() {
|
||
entries.clear();
|
||
groundTruth.clear();
|
||
wifi.clear();
|
||
beacon.clear();
|
||
acc.clear();
|
||
gyro.clear();
|
||
gps.clear();
|
||
compass.clear();
|
||
barometer.clear();
|
||
lin_acc.clear();
|
||
gravity.clear();
|
||
magnetometer.clear();
|
||
wifiFtm.clear();
|
||
}
|
||
|
||
const std::vector<Entry>& getEntries() const {return entries;}
|
||
|
||
const std::vector<TS<int>>& getActivity() const {return activity;}
|
||
|
||
const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
|
||
|
||
const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
|
||
|
||
const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
|
||
|
||
const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
|
||
|
||
const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
|
||
|
||
const std::vector<TS<GPSData>>& getGPS() const {return gps;}
|
||
|
||
const std::vector<TS<CompassData>>& getCompass() const {return compass;}
|
||
|
||
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
|
||
|
||
const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
|
||
|
||
const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
|
||
|
||
const std::vector<TS<MagnetometerData>>& getMagnetometer() const {return magnetometer;}
|
||
|
||
const std::vector<TS<WiFiMeasurement>>& getWifiFtm() const { return wifiFtm; }
|
||
|
||
/** get an interpolateable ground-truth based on the time-clicks during recording */
|
||
GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {
|
||
|
||
// sanity check: given path [indices to ground-truth points within the map]
|
||
// must have the same size as the number of clicks during recording
|
||
Assert::equal(groundTruthPoints.size(), groundTruth.size(), "mismatch of ground-truth points between given path and recording");
|
||
|
||
// allows getting a position on the ground-truth given a timestamp
|
||
GroundTruth interpol;
|
||
|
||
// all ground-truth points within the map
|
||
static std::unordered_map<int, Point3> gt = FloorplanHelper::getGroundTruthPoints(map);
|
||
|
||
// process each "tap smartphone when reaching ground-truth-point"
|
||
for (const TS<int>& entry : groundTruth) {
|
||
const Timestamp ts = Timestamp::fromMS(entry.ts);
|
||
const int idx = entry.data; // starting at 0, incrementing over time [1st point, 2nd points, 3d points, ...]
|
||
const int id = groundTruthPoints[idx]; // convert point number to point-id within floorplan
|
||
const auto& it = gt.find(id);
|
||
if (it == gt.end()) {throw Exception("missing ground-truth point ID:" + std::to_string(id));}
|
||
const Point3 pos = it->second;
|
||
interpol.add(ts, pos);
|
||
}
|
||
|
||
// done
|
||
return interpol;
|
||
|
||
}
|
||
|
||
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()) {
|
||
|
||
// read the next line from the file
|
||
// this ensures that we will not read beyond one line within the next steps
|
||
std::string line;
|
||
std::getline(inp, line);
|
||
|
||
// create a stream for the read line
|
||
std::stringstream sline(line);
|
||
|
||
uint64_t ts;
|
||
char delim;
|
||
int idx = -1;
|
||
std::string data;
|
||
|
||
// split the line
|
||
sline >> ts;
|
||
sline >> delim;
|
||
sline >> idx;
|
||
sline >> delim;
|
||
sline >> data; // might be "" if there is no data
|
||
|
||
if (convertNanoSecondTimestamps)
|
||
{
|
||
ts /= 1000000;
|
||
}
|
||
|
||
|
||
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);}
|
||
else if (idx == (int)Sensor::MAGNETOMETER) {parseMagnetometer(ts,data);}
|
||
else if (idx == (int)Sensor::WIFI_FTM) {parseWifiFtm(ts, data);}
|
||
else if (idx == (int)Sensor::ACTIVITY) {parseActivity(ts, data);}
|
||
|
||
// TODO: this is a hack...
|
||
// the loop is called one additional time after the last entry
|
||
// and keeps the entries of entry
|
||
|
||
}
|
||
|
||
inp.close();
|
||
|
||
}
|
||
|
||
void parseLinearAcceleration(const uint64_t ts, const std::string& data){
|
||
|
||
const auto pos1 = data.find(';');
|
||
const auto pos2 = data.find(';', pos1+1);
|
||
|
||
const std::string x = data.substr(0, pos1);
|
||
const std::string y = data.substr(pos1+1, pos2-pos1-1);
|
||
const std::string z = data.substr(pos2+1);
|
||
|
||
TS<LinearAccelerationData> elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z)));
|
||
lin_acc.push_back(elem);
|
||
entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));
|
||
|
||
}
|
||
|
||
void parseGravity(const uint64_t ts, const std::string& data){
|
||
|
||
GravityData gravData;
|
||
|
||
const auto pos1 = data.find(';');
|
||
const auto pos2 = data.find(';', pos1+1);
|
||
|
||
gravData.x = std::stof(data.substr(0, pos1));
|
||
gravData.y = std::stof(data.substr(pos1+1, pos2-pos1-1));
|
||
gravData.z = std::stof(data.substr(pos2+1));
|
||
|
||
TS<GravityData> elem(ts, gravData);
|
||
gravity.push_back(elem);
|
||
entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);}
|
||
|
||
}
|
||
|
||
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);
|
||
|
||
const AccelerometerData accData(std::stof(x), std::stof(y), std::stof(z));
|
||
TS<AccelerometerData> elem(ts, accData);
|
||
acc.push_back(elem);
|
||
entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);}
|
||
|
||
}
|
||
|
||
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);
|
||
|
||
const GyroscopeData gyroData(std::stof(x), std::stof(y), std::stof(z));
|
||
TS<GyroscopeData> elem(ts, gyroData);
|
||
gyro.push_back(elem);
|
||
entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);}
|
||
|
||
}
|
||
|
||
void parseWiFi(const uint64_t ts, const std::string& data) {
|
||
|
||
WiFiMeasurements wifi;
|
||
Splitter s(data, sep);
|
||
|
||
//if (s.empty()) {return;}
|
||
|
||
// the -1 is due to some old files containing a trailing ";" resulting in one additional stray column
|
||
for (size_t i = 0; i < s.size()-1; i += 3) {
|
||
|
||
const std::string mac = s.get(i+0);
|
||
const float freq = s.getFloat(i+1);
|
||
const float rssi = s.getFloat(i+2);
|
||
|
||
// append AP to current scan-entry
|
||
WiFiMeasurement e(AccessPoint(mac), rssi, freq, Timestamp::fromMS(ts));
|
||
wifi.entries.push_back(e);
|
||
|
||
}
|
||
|
||
// add new wifi reading
|
||
this->wifi.push_back(TS<WiFiMeasurements>(ts, wifi));
|
||
entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);}
|
||
|
||
}
|
||
|
||
void parseWifiFtm(const uint64_t ts, const std::string& data) {
|
||
|
||
Splitter s(data, sep);
|
||
|
||
const int successMeas = s.getInteger(0);
|
||
const std::string mac = s.get(1);
|
||
const float dist = s.getFloat(2) / 1000; // mm -> m
|
||
const float distStdDev = s.getFloat(3) / 1000;
|
||
const float rssi = s.getFloat(4);
|
||
|
||
const int numAttempts = !s.isEmpty(5) ? s.getInteger(5) : 0;
|
||
const int numSuccess = !s.isEmpty(6) ? s.getInteger(6) : 0;
|
||
|
||
// append AP to current scan-entry
|
||
if (successMeas)
|
||
{
|
||
WiFiMeasurement meas(AccessPoint(mac), rssi, Timestamp::fromMS(ts), dist, distStdDev, numAttempts, numSuccess);
|
||
|
||
wifiFtm.push_back(TS<WiFiMeasurement>(ts, meas));
|
||
entries.push_back(Entry(Sensor::WIFI_FTM, ts, this->wifiFtm.size() - 1));
|
||
}
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);}
|
||
|
||
}
|
||
|
||
void parseBeacons(const uint64_t ts, const std::string& data) {
|
||
|
||
const auto pos1 = data.find(';');
|
||
const auto pos2 = data.find(';', pos1+1);
|
||
const auto pos3 = data.find(';', pos2+1);
|
||
|
||
const std::string mac = data.substr(0, pos1);
|
||
const std::string rssi = data.substr(pos1+1, pos2);
|
||
const std::string txp = data.substr(pos2+1, pos3);
|
||
|
||
//yes the timestamp is redundant here, but in case of multiusage...
|
||
TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
|
||
beacon.push_back(e);
|
||
entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
|
||
|
||
}
|
||
|
||
void parseGroundTruth(const uint64_t ts, const std::string& data) {
|
||
|
||
const auto pos1 = data.find(';');
|
||
std::string gtIndex = data.substr(0, pos1);
|
||
|
||
TS<int> elem(ts, std::stoi(gtIndex));
|
||
groundTruth.push_back(elem);
|
||
|
||
entries.push_back(Entry(Sensor::GROUND_TRUTH, ts, groundTruth.size() - 1));
|
||
|
||
}
|
||
|
||
void parseActivity(const uint64_t ts, const std::string& data) {
|
||
|
||
const auto pos1 = data.find(';');
|
||
const auto pos2 = data.find(';', pos1+1);
|
||
|
||
const std::string string = data.substr(0, pos1);
|
||
const std::string id = data.substr(pos1+1, pos2);
|
||
|
||
TS<int> elem(ts, std::stoi(id));
|
||
activity.push_back(elem);
|
||
entries.push_back(Entry(Sensor::ACTIVITY, ts, activity.size()-1));
|
||
|
||
}
|
||
|
||
void parseBarometer(const uint64_t ts, const std::string& data) {
|
||
|
||
BarometerData baro;
|
||
Splitter s(data, sep);
|
||
|
||
baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN);
|
||
|
||
TS<BarometerData> elem(ts, baro);
|
||
barometer.push_back(elem);
|
||
entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);}
|
||
|
||
}
|
||
|
||
void parseCompass(const uint64_t ts, 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);
|
||
|
||
TS<CompassData> elem(ts, compass);
|
||
this->compass.push_back(elem);
|
||
entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}
|
||
|
||
}
|
||
|
||
void parseMagnetometer(const uint64_t ts, const std::string& data) {
|
||
|
||
MagnetometerData mag;
|
||
Splitter s(data, sep);
|
||
|
||
mag.x = s.has(0) ? (s.getFloat(0)) : (NAN);
|
||
mag.y = s.has(1) ? (s.getFloat(1)) : (NAN);
|
||
mag.z = s.has(2) ? (s.getFloat(2)) : (NAN);
|
||
|
||
TS<MagnetometerData> elem(ts, mag);
|
||
this->magnetometer.push_back(elem);
|
||
entries.push_back(Entry(Sensor::MAGNETOMETER, ts, this->magnetometer.size()-1));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}
|
||
|
||
}
|
||
|
||
/** 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));
|
||
|
||
// inform listener
|
||
//if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);}
|
||
|
||
}
|
||
|
||
|
||
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
|