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

many changes

Browse Source 
added new helper class for 3x3 matrices and vec3
added magnetometer data
added compass detection
refactored pose-estimation (single class)
refactored debug plots (move to own class)
minor changes
This commit is contained in:
FrankE
2017-10-31 19:38:08 +01:00
parent d97b325650
commit 284c6b11a6
14 changed files with 1377 additions and 579 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
sensors/offline/FileReader.h
View File

@@ -16,6 +16,7 @@
#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"
@@ -51,6 +52,7 @@ namespace Offline {
std::vector<TS<GravityData>> gravity;
std::vector<TS<GPSData>> gps;
std::vector<TS<CompassData>> compass;
std::vector<TS<MagnetometerData>> magnetometer;
/** all entries in linear order as they appeared while recording */
std::vector<Entry> entries;
@@ -88,6 +90,7 @@ namespace Offline {
barometer.clear();
lin_acc.clear();
gravity.clear();
magnetometer.clear();
}
const std::vector<Entry>& getEntries() const {return entries;}
@@ -113,6 +116,8 @@ namespace Offline {
const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
const std::vector<TS<MagnetometerData>>& getMagnetometer() const {return magnetometer;}
/** get an interpolateable ground-truth based on the time-clicks during recording */
GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {
@@ -147,7 +152,7 @@ namespace Offline {
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);}
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file: " + file);}
while(!inp.eof() && !inp.bad()) {
@@ -172,6 +177,7 @@ namespace Offline {
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);}
// TODO: this is a hack...
// the loop is called one additional time after the last entry
@@ -343,6 +349,24 @@ namespace Offline {
}
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) {