huge commit

- worked on about everything
- grid walker using plugable modules
- wifi models
- new distributions
- worked on geometric data-structures
- added typesafe timestamps
- worked on grid-building
- added sensor-classes
- added sensor analysis (step-detection, turn-detection)
- offline data reader
- many test-cases

sensors/imu/AccelerometerData.h

@@ -0,0 +1,23 @@
+#ifndef ACCELEROMETERDATA_H
+#define ACCELEROMETERDATA_H
+
+#include <cmath>
+
+/** data received from an accelerometer sensor */
+struct AccelerometerData {
+
+	float x;
+	float y;
+	float z;
+
+	AccelerometerData() : x(0), y(0), z(0) {;}
+
+	AccelerometerData(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 );
+	}
+
+};
+
+#endif // ACCELEROMETERDATA_H

sensors/imu/GyroscopeData.h

@@ -0,0 +1,23 @@
+#ifndef GYROSCOPEDATA_H
+#define GYROSCOPEDATA_H
+
+#include <cmath>
+
+/** data received from a gyroscope sensor */
+struct GyroscopeData {
+
+	float x;
+	float y;
+	float z;
+
+	GyroscopeData() : x(0), y(0), z(0) {;}
+
+	GyroscopeData(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 );
+	}
+
+};
+
+#endif // GYROSCOPEDATA_H

sensors/imu/StepDetection.h

@@ -0,0 +1,112 @@
+#ifndef STEPDETECTION_H
+#define STEPDETECTION_H
+
+
+#include "AccelerometerData.h"
+#include "../../data/Timestamp.h"
+
+#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"
+
+
+/**
+ * simple step detection based on accelerometer magnitude.
+ * magnitude > threshold? -> step!
+ * block for several msec until detecting the next one
+ */
+class StepDetection {
+
+private:
+
+	/** low pass acc-magnitude */
+	float avg1 = 0;
+
+	/** even-more low-pass acc-magnitude */
+	float avg2 = 0;
+
+private:
+
+	class Stepper {
+
+	private:
+
+		/** block for 300 ms after every step */
+		const Timestamp blockTime = Timestamp::fromMS(300);
+
+		/** the threshold for detecting a spike as step */
+		const float threshold = 0.17;
+
+		/** block until the given timestamp before detecting additional steps */
+		Timestamp blockUntil;
+
+
+	public:
+
+		/** is the given (relative!) magnitude (mag - ~9.81) a step? */
+		bool isStep(const Timestamp ts, const float mag) {
+
+			// still blocking
+			if (ts < blockUntil) {
+				return false;
+			}
+
+			// threshold reached? -> step!
+			if (mag > threshold) {
+
+				// block x milliseconds until detecting the next step
+				blockUntil = ts + blockTime;
+
+				// we have a step
+				return true;
+
+			}
+
+			// no step
+			return false;
+
+		}
+
+
+	};
+
+	Stepper stepper;
+
+public:
+
+	/** does the given data indicate a step? */
+	bool isStep(const Timestamp ts, const AccelerometerData& acc) {
+
+		avg1 = avg1 * 0.91 + acc.magnitude() * 0.09;	// short-time average	[filtered steps]
+		avg2 = avg2 * 0.97 + acc.magnitude() * 0.03;	// long-time average	[gravity]
+
+		// average maginitude must be > 9.0 to be stable enough to proceed
+		if (avg2 > 9) {
+
+			// gravity-free magnitude
+			const float avg = avg1 - avg2;
+
+			// detect steps
+			return stepper.isStep(ts, avg);
+
+		} else {
+
+			return false;
+
+		}
+
+	}
+
+
+};
+
+
+#endif // STEPDETECTION_H

sensors/imu/TurnDetection.h

@@ -0,0 +1,493 @@
+#ifndef TURNDETECTION_H
+#define TURNDETECTION_H
+
+#include "GyroscopeData.h"
+#include "AccelerometerData.h"
+#include "../../data/Timestamp.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 TurnDetection {
+
+private:
+
+	//std::vector<AccelerometerData> accData;
+	std::vector<GyroscopeData> gyroData;
+
+	Timestamp lastGyro;
+	Timestamp lastRotMatEst;
+
+	Eigen::Matrix3f rotMat;
+	Eigen::Vector3f avgAcc;
+
+	Eigen::Vector3f leGyro;
+
+public:
+
+	/** ctor */
+	TurnDetection() : rotMat(Eigen::Matrix3f::Identity()) {
+
+	}
+
+	float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
+
+		if (lastGyro.isZero()) {lastGyro = ts;}
+
+
+		// TESTING!
+		gyroData.push_back(gyro);
+		static Eigen::Matrix3f rotMat = Eigen::Matrix3f::Identity();
+		if (gyroData.size() > 25) {
+			Eigen::Vector3f sum = Eigen::Vector3f::Zero();
+			int cnt = 0;
+			for (GyroscopeData gd : gyroData) {
+				//if (gd.z > gd.x && gd.z > gd.y) {
+					Eigen::Vector3f vec; vec << (gd.x), (gd.y), (gd.z);
+					sum += vec;
+					++cnt;
+				//}
+			}
+			gyroData.clear();
+			if (cnt > 10) {
+				Eigen::Vector3f z; z << 0,0, sum(2) < 0 ? -1 : +1;
+				rotMat = getRotationMatrix(sum.normalized(), z.normalized());
+			}
+		}
+
+		// current gyro-reading as vector
+		Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
+		leGyro = vec;
+
+		// current value, rotated into the new coordinate system
+		Eigen::Vector3f curVec = rotMat * vec;
+
+		// previous value
+		static Eigen::Vector3f oldVec = curVec;
+
+		// time-difference between previous and current value
+		const Timestamp diff = ts - lastGyro;
+		lastGyro = ts;
+
+		// area
+		Eigen::Vector3f area = Eigen::Vector3f::Zero();
+		if (!diff.isZero()) {
+			area = (oldVec * diff.sec()) +					// squared region
+					((curVec - oldVec) * 0.5 * diff.sec());	// triangle region to the next (enhances the quality)
+		}
+
+		// update the old value
+		oldVec = curVec;
+
+		const float delta = area(2);// * 0.8;
+
+
+
+
+		static int i = 0; ++i;
+		if (i % 50 == 0) {
+
+		static K::Gnuplot gp;
+
+			gp << "set view equal xyz\n";
+			gp << "set xrange[-1:+1]\n";
+			gp << "set yrange[-1:+1]\n";
+			gp << "set zrange[-1:+1]\n";
+
+			K::GnuplotSplot plot;
+			K::GnuplotSplotElementLines lines; plot.add(&lines);
+
+			K::GnuplotPoint3 p0(0,0,0);
+			//K::GnuplotPoint3 pO(vec(0), vec(1), vec(2));
+
+			K::GnuplotPoint3 px(rotMat(0,0), rotMat(1,0), rotMat(2,0)); //px = px * eval(0);
+			K::GnuplotPoint3 py(rotMat(0,1), rotMat(1,1), rotMat(2,1)); //py = py * eval(1);
+			K::GnuplotPoint3 pz(rotMat(0,2), rotMat(1,2), rotMat(2,2)); //pz = pz * eval(2);
+
+			lines.addSegment(p0, px*0.15);
+			lines.addSegment(p0, py*0.4);
+			lines.addSegment(p0, pz*1.0);
+
+			Eigen::Vector3f ori = leGyro;
+			Eigen::Vector3f re = rotMat * leGyro;
+			Eigen::Vector3f avg = est.lastAvg * 0.3;
+
+			gp << "set arrow 1 from 0,0,0 to " << avg(0) << "," << avg(1) << "," << avg(2) << " lw 2\n";
+
+			gp << "set arrow 2 from 0,0,0 to " << ori(0) << "," << ori(1) << "," << ori(2) << " lw 1 dashtype 2 \n";
+			gp << "set arrow 3 from 0,0,0 to " << re(0) << "," << re(1) << "," << re(2) << " lw 1\n";
+
+//			gp << "set arrow 2 from 0,0,0 to " << vec(0) << "," << vec(1) << "," << vec(2) << "\n";
+//			gp << "set arrow 3 from 0,0,0 to " << nVec(0) << "," << nVec(1) << "," << nVec(2) << "\n";
+
+			gp.draw(plot);
+			//gp.flush();
+
+		}
+
+
+
+		static Eigen::Vector3f sum = Eigen::Vector3f::Zero();
+		sum += area;
+
+		if (i % 30 == 0) {
+
+			static int idx = 0;
+			static K::Gnuplot gp2;
+			gp2 << "set arrow 1 from 0,0 to 10000,0\n";
+			gp2 << "set arrow 2 from 0,5 to 10000,5\n";
+			gp2 << "set arrow 3 from 0,10 to 10000,10\n";
+
+			K::GnuplotPlot plot2;
+			static K::GnuplotPlotElementLines linesX; plot2.add(&linesX);
+			static K::GnuplotPlotElementLines linesY; plot2.add(&linesY);
+			static K::GnuplotPlotElementLines linesZ; plot2.add(&linesZ);
+
+			//linesX.add(K::GnuplotPoint2(idx, sum(0) + 0));
+			//linesY.add(K::GnuplotPoint2(idx, sum(1) + 5));
+			linesZ.add(K::GnuplotPoint2(idx, sum(2) + 10));
+			++idx;
+
+			gp2.draw(plot2);
+			//gp2.flush();
+
+		}
+
+		return delta;
+
+	}
+
+
+
+	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
+
+		// add accelerometer data
+		//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
+		est.add((acc.x), (acc.y), (acc.z));
+
+		// start with the first available timestamp
+		if (lastRotMatEst.isZero()) {lastRotMatEst = ts;}
+
+		// if we have at-least 1 sec of acc-data, re-calculate the current smartphone holding
+		if (ts - lastRotMatEst > Timestamp::fromMS(500)) {
+			rotMat = est.get();
+			lastRotMatEst = ts;
+		}
+
+	}
+
+private:
+
+//	/** estimate the smartphones current holding position */
+//	void estimateHolding2() {
+
+
+//		// z-axis points through the device and is the axis we are interested in
+//		// http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
+
+//		avgAcc = Eigen::Vector3f::Zero();
+
+//		for (const AccelerometerData& acc : accData) {
+//		//for (const GyroscopeData& acc : gyroData) {
+//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
+//		//	Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
+//			avgAcc += vec;
+//		}
+
+//		//avgAcc /= accData.size();
+//		avgAcc = avgAcc.normalized();
+
+//		Eigen::Vector3f rev; rev << 0,0,1;
+
+//		rotMat = getRotationMatrix(avgAcc, rev);
+
+
+//		//Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
+//		//Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
+////		Eigen::Vector3f re = rotMat * avgAcc;
+////		Eigen::Vector3f diff = rev-re;
+////		Assert::isTrue(diff.norm() < 0.001, "rotation error");
+
+//	}
+
+public:
+
+	/** get a matrix that rotates the vector "from" into the vector "to" */
+	static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
+
+		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
+
+		const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
+		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
+
+		Eigen::Matrix3f A; A <<
+			0,		-x(2),	x(1),
+			x(2),	0,		-x(0),
+			-x(1),	x(0),	0;
+
+		return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
+
+	}
+
+
+	struct XYZ {
+
+		Eigen::Vector3f lastAvg;
+		Eigen::Vector3f avg;
+		int cnt;
+
+		XYZ() {
+			reset();
+		}
+
+		void add(const float x, const float y, const float z) {
+
+			Eigen::Vector3f vec; vec << x,y,z;
+			avg += vec;
+			++cnt;
+
+		}
+
+		Eigen::Matrix3f get() {
+
+			avg/= cnt;
+			lastAvg = avg;
+
+			// rotate average accelerometer into (0,0,1)
+			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+			const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
+
+			// sanity check
+			Eigen::Vector3f aligned = (rotMat * avg).normalized();
+			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+			reset();
+
+			return rotMat;
+
+		}
+
+		void reset() {
+			cnt = 0;
+			avg = Eigen::Vector3f::Zero();
+		}
+
+
+	} est;
+
+
+//	struct RotationMatrixEstimationUsingAccAngle {
+
+//		Eigen::Vector3f lastAvg;
+//		Eigen::Vector3f avg;
+//		int cnt;
+
+//		RotationMatrixEstimationUsingAccAngle() {
+//			reset();
+//		}
+
+//		void add(const float x, const float y, const float z) {
+
+//			Eigen::Vector3f vec; vec << x,y,z;
+//			avg += vec;
+//			++cnt;
+
+//		}
+
+//		void reset() {
+//			cnt = 0;
+//			avg = Eigen::Vector3f::Zero();
+//		}
+
+//		Eigen::Matrix3f get() {
+
+//			// http://www.hobbytronics.co.uk/accelerometer-info
+
+//			avg /= cnt;
+//			lastAvg = avg;
+
+//			//const float mag = avg.norm();
+
+//			const float baseX = 0;
+//			const float baseY = 0;
+//			const float baseZ = 0;	// mag?
+
+//			const float x = avg(0) - baseX;
+//			const float y = avg(1) - baseY;
+//			const float z = avg(2) - baseZ;
+
+//			const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
+//			const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
+
+//			const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0);		// TODO -ax or +ax?
+
+//			// sanity check
+//			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+//			Eigen::Vector3f aligned = (rotMat * avg).normalized();
+//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+//			//			int i = 0; (void) i;
+
+//			reset();
+//			return rotMat;
+
+//		}
+
+//	} est;
+
+
+	/** get a rotation matrix for the given x,y,z rotation (in radians) */
+	static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
+		const float g = x; const float b = y; const float a = z;
+		const float a11 = std::cos(a)*std::cos(b);
+		const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
+		const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
+		const float a21 = std::sin(a)*std::cos(b);
+		const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
+		const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
+		const float a31 = -std::sin(b);
+		const float a32 = std::cos(b)*std::sin(g);
+		const float a33 = std::cos(b)*std::cos(g);
+		Eigen::Matrix3f m;
+		m <<
+			a11,	a12,	a13,
+			a21,	a22,	a23,
+			a31,	a32,	a33;
+		;
+		return m;
+	}
+
+
+//	struct PCA {
+
+//		Eigen::Vector3f avg;
+//		Eigen::Vector3f lastAvg;
+//		Eigen::Matrix3f covar;
+//		int cnt = 0;
+
+//		PCA() {
+//			reset();
+//		}
+
+//		void add(const float x, const float y, const float z) {
+
+//			Eigen::Vector3f vec; vec << x,y,z;
+//			avg += vec;
+//			covar += vec*vec.transpose();
+//			++cnt;
+
+//		}
+
+//		Eigen::Matrix3f get() {
+
+//			avg /= cnt;
+//			covar /= cnt;
+//			lastAvg = avg;
+
+//			std::cout << avg << std::endl;
+
+//			Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
+//			for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
+
+//			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
+//			solver.eigenvalues();
+//			solver.eigenvectors();
+
+//			const auto eval = solver.eigenvalues();
+//			const auto evec = solver.eigenvectors();
+//			Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
+
+//			Eigen::Matrix3f rotMat;
+//			rotMat.col(0) = evec.col(0);
+//			rotMat.col(1) = evec.col(1);
+//			rotMat.col(2) = evec.col(2);		// 0,0,1 (z-axis) belongs to the strongest eigenvalue
+//			rotMat.transposeInPlace();
+
+//			//Eigen::Vector3f gy; gy << 0, 30, 30;
+//			Eigen::Vector3f avg1 = rotMat * avg;
+//			int i = 0; (void) i;
+
+//			reset();
+
+//			return rotMat;
+
+//		}
+
+//		void reset() {
+//			cnt = 0;
+//			avg = Eigen::Vector3f::Zero();
+//			covar = Eigen::Matrix3f::Zero();
+//		}
+
+
+//	} pca1;
+
+
+
+
+//	/** estimate the smartphones current holding position */
+//	void estimateHolding() {
+
+//		Eigen::Vector3f avg = Eigen::Vector3f::Zero();
+//		Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
+
+//		for (const AccelerometerData& acc : accData) {
+////		for (const GyroscopeData& acc : gyroData) {
+//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
+////			Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
+//			avg += vec;
+//			covar += vec * vec.transpose();
+//		}
+
+//		avg /= accData.size();		// TODO
+//		covar /= accData.size();	//TODO
+
+//		avgAcc = avg.normalized();
+
+
+
+
+
+
+
+
+
+////		static K::Gnuplot gp;
+////		gp << "set view equal xyz\n";
+////		gp << "set xrange[-1:+1]\n";
+////		gp << "set yrange[-1:+1]\n";
+////		gp << "set zrange[-1:+1]\n";
+
+////		K::GnuplotSplot plot;
+////		K::GnuplotSplotElementLines lines; plot.add(&lines);
+
+////		K::GnuplotPoint3 p0(0,0,0);
+////		K::GnuplotPoint3 px(evec(0,0), evec(1,0), evec(2,0)); //px = px * eval(0);
+////		K::GnuplotPoint3 py(evec(0,1), evec(1,1), evec(2,1)); //py = py * eval(1);
+////		K::GnuplotPoint3 pz(evec(0,2), evec(1,2), evec(2,2)); //pz = pz * eval(2);
+
+////		K::GnuplotPoint3 pa(avg(0), avg(1), avg(2));
+
+////		lines.addSegment(p0, px);
+////		lines.addSegment(p0, py);
+////		lines.addSegment(p0, pz);
+////		lines.addSegment(p0, pa);
+
+////		gp.draw(plot);
+////		gp.flush();
+
+//	}
+
+};
+
+#endif // TURNDETECTION_H