many small changes, added filereader with beacons, added motion detection stuff, testcases

sensors/imu/GravityData.h

@@ -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

sensors/imu/LinearAccelerationData.h

@@ -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

sensors/imu/MotionDetection.h

@@ -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