many small changes, added filereader with beacons, added motion detection stuff, testcases
This commit is contained in:
toni
53
sensors/imu/GravityData.h
Normal file
53
sensors/imu/GravityData.h
Normal file
@@ -0,0 +1,53 @@
|
||||
#ifndef GRAVITYDATA_H
|
||||
#define GRAVITYDATA_H
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
|
||||
/** data received from an accelerometer sensor */
|
||||
struct GravityData {
|
||||
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
GravityData() : x(0), y(0), z(0) {;}
|
||||
|
||||
GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
|
||||
|
||||
float magnitude() const {
|
||||
return std::sqrt( x*x + y*y + z*z );
|
||||
}
|
||||
|
||||
GravityData& operator += (const GravityData& o) {
|
||||
this->x += o.x;
|
||||
this->y += o.y;
|
||||
this->z += o.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
GravityData& operator -= (const GravityData& o) {
|
||||
this->x -= o.x;
|
||||
this->y -= o.y;
|
||||
this->z -= o.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
GravityData operator - (const GravityData& o) const {
|
||||
return GravityData(x-o.x, y-o.y, z-o.z);
|
||||
}
|
||||
|
||||
GravityData operator / (const float val) const {
|
||||
return GravityData(x/val, y/val, z/val);
|
||||
}
|
||||
|
||||
std::string asString() const {
|
||||
std::stringstream ss;
|
||||
ss << "(" << x << "," << y << "," << z << ")";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // GRAVITYDATA_H
|
||||
53
sensors/imu/LinearAccelerationData.h
Normal file
53
sensors/imu/LinearAccelerationData.h
Normal file
@@ -0,0 +1,53 @@
|
||||
#ifndef LINEARACCELERATIONDATA_H
|
||||
#define LINEARACCELERATIONDATA_H
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
|
||||
/** data received from an accelerometer sensor */
|
||||
struct LinearAccelerationData {
|
||||
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
LinearAccelerationData() : x(0), y(0), z(0) {;}
|
||||
|
||||
LinearAccelerationData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
|
||||
|
||||
float magnitude() const {
|
||||
return std::sqrt( x*x + y*y + z*z );
|
||||
}
|
||||
|
||||
LinearAccelerationData& operator += (const LinearAccelerationData& o) {
|
||||
this->x += o.x;
|
||||
this->y += o.y;
|
||||
this->z += o.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
LinearAccelerationData& operator -= (const LinearAccelerationData& o) {
|
||||
this->x -= o.x;
|
||||
this->y -= o.y;
|
||||
this->z -= o.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
LinearAccelerationData operator - (const LinearAccelerationData& o) const {
|
||||
return LinearAccelerationData(x-o.x, y-o.y, z-o.z);
|
||||
}
|
||||
|
||||
LinearAccelerationData operator / (const float val) const {
|
||||
return LinearAccelerationData(x/val, y/val, z/val);
|
||||
}
|
||||
|
||||
std::string asString() const {
|
||||
std::stringstream ss;
|
||||
ss << "(" << x << "," << y << "," << z << ")";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // LINEARACCELERATIONDATA_H
|
||||
163
sensors/imu/MotionDetection.h
Normal file
163
sensors/imu/MotionDetection.h
Normal file
@@ -0,0 +1,163 @@
|
||||
#ifndef MOTIONDETECTION_H
|
||||
#define MOTIONDETECTION_H
|
||||
|
||||
#include "GravityData.h"
|
||||
#include "LinearAccelerationData.h"
|
||||
#include "../../data/Timestamp.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../misc/Debug.h"
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
|
||||
|
||||
|
||||
#include "../../Assertions.h"
|
||||
|
||||
class MotionDetection {
|
||||
|
||||
private:
|
||||
|
||||
bool newAccelerationMeasurementArrived = false;
|
||||
bool newGravityMeasurementArrived = false;
|
||||
|
||||
Eigen::Vector3f curGravity;
|
||||
Eigen::Vector3f curLinearAcceleration;
|
||||
|
||||
//fast algo
|
||||
Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix
|
||||
|
||||
int numMeasurementsPerInterval, updateCnt;
|
||||
int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms
|
||||
|
||||
struct Motion{
|
||||
Eigen::Vector2f vec = Eigen::Vector2f::Identity();
|
||||
Timestamp lastEstimation;
|
||||
};
|
||||
|
||||
Motion curMotion;
|
||||
Motion prevMotion;
|
||||
|
||||
const char* name = "MotionDetection";
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) {
|
||||
;
|
||||
}
|
||||
|
||||
void addGravity(const Timestamp& ts, const GravityData& grav){
|
||||
|
||||
curGravity << grav.x, grav.y, grav.z;
|
||||
newGravityMeasurementArrived = true;
|
||||
|
||||
updateProjectionVectorFast(ts);
|
||||
}
|
||||
|
||||
void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) {
|
||||
|
||||
curLinearAcceleration << acc.x, acc.y, acc.z;
|
||||
newAccelerationMeasurementArrived = true;
|
||||
|
||||
updateProjectionVectorFast(ts);
|
||||
}
|
||||
|
||||
/** return the current motion axis
|
||||
* NOTE: if no data is available, this vector is the Identity
|
||||
*/
|
||||
Eigen::Vector2f getCurrentMotionAxis(){
|
||||
return curMotion.vec;
|
||||
}
|
||||
|
||||
/** returns the radians between [-pi, pi] between successive motion axis estimations */
|
||||
float getMotionChangeInRad(){
|
||||
//TODO: put this in an EigenHelper Class within geo
|
||||
const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x();
|
||||
const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f));
|
||||
|
||||
//nan?
|
||||
if(std::isnan(ang)){
|
||||
Log::add(name, "The motion change angle is nAn, this is not correct!");
|
||||
}
|
||||
|
||||
if(updateCnt < 2){
|
||||
return 0;
|
||||
}
|
||||
|
||||
return ang;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
FRIEND_TEST(MotionDetection, motionAngle);
|
||||
FRIEND_TEST(MotionDetection, motionAxis);
|
||||
|
||||
Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){
|
||||
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covarianceMatrix);
|
||||
return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue
|
||||
}
|
||||
|
||||
void updateProjectionVectorFast(const Timestamp& ts){
|
||||
|
||||
//make sure we have both measurements for calculation
|
||||
if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){
|
||||
|
||||
numMeasurementsPerInterval++;
|
||||
|
||||
//project acc into perpendicular plane of grav (using standard vector projection)
|
||||
Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity;
|
||||
|
||||
//if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this
|
||||
if(proj.isZero()){
|
||||
proj = curLinearAcceleration;
|
||||
Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?");
|
||||
}
|
||||
|
||||
//we are only interested in x,y
|
||||
Eigen::Vector2f vec;
|
||||
vec << proj.x(), proj.y();
|
||||
|
||||
// sum this up for later averaging.
|
||||
sumProjectedCov += vec*vec.transpose();
|
||||
|
||||
// start with the first available timestamp
|
||||
if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;}
|
||||
|
||||
//update the motion axis depending on the update interval
|
||||
if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){
|
||||
|
||||
prevMotion = curMotion;
|
||||
|
||||
//calculate the average of the coveriance matrix
|
||||
Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval;
|
||||
|
||||
curMotion.vec = updateMotionAxis(Q);
|
||||
curMotion.lastEstimation = ts;
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
newGravityMeasurementArrived = false;
|
||||
newAccelerationMeasurementArrived = false;
|
||||
}
|
||||
//do nothing
|
||||
}
|
||||
|
||||
void reset(){
|
||||
numMeasurementsPerInterval = 0;
|
||||
sumProjectedCov = Eigen::Matrix2f::Zero();
|
||||
++updateCnt;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // MOTIONDETECTION_H
|
||||
299
sensors/offline/FileReader.h
Normal file
299
sensors/offline/FileReader.h
Normal file
@@ -0,0 +1,299 @@
|
||||
#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 "../../geo/Point2.h"
|
||||
#include "../../grid/factory/v2/GridFactory.h"
|
||||
#include "../../grid/factory/v2/Importance.h"
|
||||
#include "../../floorplan/v2/Floorplan.h"
|
||||
|
||||
class FileReader {
|
||||
|
||||
public:
|
||||
|
||||
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) {;}
|
||||
};
|
||||
|
||||
enum class Sensor {
|
||||
ACC,
|
||||
GYRO,
|
||||
WIFI,
|
||||
POS,
|
||||
BARO,
|
||||
BEACON,
|
||||
LIN_ACC,
|
||||
GRAVITY,
|
||||
};
|
||||
|
||||
/** 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) {;}
|
||||
};
|
||||
|
||||
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;
|
||||
|
||||
/** ALL entries */
|
||||
std::vector<Entry> entries;
|
||||
|
||||
public:
|
||||
|
||||
FileReader(const std::string& file) {
|
||||
parse(file);
|
||||
}
|
||||
|
||||
const std::vector<Entry>& getEntries() const {return entries;}
|
||||
|
||||
|
||||
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<BarometerData>>& getBarometer() const {return barometer;}
|
||||
|
||||
const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
|
||||
|
||||
const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
|
||||
|
||||
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()) {
|
||||
|
||||
uint64_t ts;
|
||||
char delim;
|
||||
int idx = -1;
|
||||
std::string data;
|
||||
|
||||
inp >> ts;
|
||||
inp >> delim;
|
||||
inp >> idx;
|
||||
inp >> delim;
|
||||
inp >> data;
|
||||
|
||||
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);}
|
||||
|
||||
// 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){
|
||||
|
||||
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<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 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);
|
||||
|
||||
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 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);
|
||||
|
||||
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));
|
||||
|
||||
}
|
||||
|
||||
void parseWiFi(const uint64_t ts, const std::string& data) {
|
||||
|
||||
std::string tmp = data;
|
||||
|
||||
// add new wifi reading
|
||||
wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements()));
|
||||
entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1));
|
||||
|
||||
// process all APs
|
||||
while(!tmp.empty()) {
|
||||
|
||||
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);
|
||||
|
||||
tmp = tmp.substr(pos3);
|
||||
assert(tmp[0] == ';'); tmp = tmp.substr(1);
|
||||
|
||||
// append AP to current scan-entry
|
||||
WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
|
||||
wifi.back().data.entries.push_back(e);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
void parseBarometer(const uint64_t ts, const std::string& data) {
|
||||
|
||||
const auto pos1 = data.find(';');
|
||||
|
||||
const std::string hPa = data.substr(0, pos1);
|
||||
|
||||
TS<BarometerData> elem(ts, BarometerData(std::stof(hPa)));
|
||||
barometer.push_back(elem);
|
||||
entries.push_back(Entry(Sensor::BARO, ts, barometer.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
|
||||
Reference in New Issue
Block a user