Indoor/sensors/imu/TurnDetection.h

#ifndef TURNDETECTION_H
#define TURNDETECTION_H

#include "GyroscopeData.h"
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../geo/Point3.h"

#include <eigen3/Eigen/Dense>

#include <cmath>
#include <vector>

#ifdef WITH_DEBUG_PLOT
	#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 <KLib/misc/gnuplot/GnuplotMultiplot.h>
	#include <KLib/misc/gnuplot/GnuplotSplot.h>
	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#endif

#include "../../Assertions.h"

class TurnDetection {

private:

	#ifdef WITH_DEBUG_PLOT
		Timestamp plotRef;
		Timestamp lastPlot;

		K::Gnuplot gp1;
		K::Gnuplot gp2;

		K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,3);

		K::GnuplotPlot plotGyroRaw;
		K::GnuplotPlotElementLines lineGyroRawX;
		K::GnuplotPlotElementLines lineGyroRawY;
		K::GnuplotPlotElementLines lineGyroRawZ;

		K::GnuplotPlot plotGyroFix;
		K::GnuplotPlotElementLines lineGyroFixX;
		K::GnuplotPlotElementLines lineGyroFixY;
		K::GnuplotPlotElementLines lineGyroFixZ;

		K::GnuplotPlot plotAcc;
		K::GnuplotPlotElementLines lineAccX;
		K::GnuplotPlotElementLines lineAccY;
		K::GnuplotPlotElementLines lineAccZ;

		K::GnuplotSplot plotPose;
		K::GnuplotSplotElementLines linePose;

		float plotCurHead = 0;

	#endif


	//std::vector<GyroscopeData> gyroData;
	Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();

	Timestamp lastGyroReading;


	struct {
		Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
		bool isKnown = false;
		Timestamp lastEstimation;
	} orientation;




public:

	/** ctor */
	TurnDetection() {

		#ifdef WITH_DEBUG_PLOT

			gp1 << "set autoscale xfix\n";
			gp1 << "set view equal xyz\n";

			multiplot.add(&plotGyroRaw);
			multiplot.add(&plotGyroFix);
			multiplot.add(&plotAcc);

			plotGyroRaw.setTitle("Gyroscope (raw)");
			plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
			plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
			plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");

			plotGyroFix.setTitle("Gyroscope (fixed)");
			plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
			plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
			plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");

			plotAcc.setTitle("Accelerometer");
			plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX");
			plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY");
			plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ");

			plotPose.setTitle("Pose");
			plotPose.getView().setEnabled(false);
			plotPose.add(&linePose);
			plotPose.getAxisX().setRange(-5,+5);
			plotPose.getAxisY().setRange(-5,+5);
			plotPose.getAxisZ().setRange(-5,+5);

		#endif

	}


	// does not seem to help...
//	struct DriftEstimator {


//		MovingAverageTS<Eigen::Vector3f> avg;

//		DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
//			;
//		}

//		void removeDrift(const Timestamp ts, Eigen::Vector3f& gyro) {

//			if (gyro.norm() < 0.15) {
//				avg.add(ts, gyro);
//				gyro -= avg.get();
//			}

//		}

//	} driftEst;





	float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {


		// ignore the first reading completely, just remember its timestamp
		if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}

		// time-difference between previous and current reading
		const Timestamp curDiff = ts - lastGyroReading;
		lastGyroReading = ts;

		// fast sensors might lead to delay = 0 ms. filter those values
		if (curDiff.isZero()) {return 0.0f;}

		// ignore readings until the first orientation-estimation is available
		// otherwise we would use a wrong rotation matrix which yields wrong results!
		if (!orientation.isKnown) {return 0.0f;}

		// get the  current gyro-reading as vector
		Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;

		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
		Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
		//driftEst.removeDrift(ts, curGyro);

		// area
		const Eigen::Vector3f area =

				// Trapezoid rule (should be more accurate but does not always help?!)
				//(prevGyro * curDiff.sec()) +					// squared region
				//((curGyro - prevGyro) * 0.5 * curDiff.sec());	// triangle region to the next (enhances the quality)

				// just the rectangular region
				(prevGyro * curDiff.sec());		// BEST?!


		//}

		// update the old value
		prevGyro = curGyro;

		// rotation = z-axis only!
		const float delta = area(2);


		#ifdef WITH_DEBUG_PLOT

			plotCurHead += delta;

			if (plotRef.isZero()) {plotRef = ts;}
			const Timestamp tsPlot = (ts-plotRef);
			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);

			// raw gyro
			lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
			lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
			lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );

			// adjusted gyro
			lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(0)) );
			lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(1)) );
			lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(2)) );

			// adjusted gyro
			lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().x) );
			lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().y) );
			lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().z) );

			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {

				lastPlot = tsPlot;

				// plot 3D pose
				std::vector<Point3> pose = {
					Point3(-1, -2, -0.1), Point3(+1, -2, -0.1), Point3(+1, +2, -0.1), Point3(-1, +2, -0.1),
					Point3(-1, -2, +0.1), Point3(+1, -2, +0.1), Point3(+1, +2, +0.1), Point3(-1, +2, +0.1),
				};
				linePose.clear();
				for (const Point3 p : pose) {
					Eigen::Vector3f vec1; vec1 << p.x, p.y, p.z;
					Eigen::Vector3f vec2 = orientation.rotationMatrix * vec1;
					K::GnuplotPoint3 gp3(vec2(0), vec2(1), vec2(2));
					linePose.add(gp3);
				}

				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
				lineGyroRawX.removeIf(remove);
				lineGyroRawY.removeIf(remove);
				lineGyroRawZ.removeIf(remove);
				lineGyroFixX.removeIf(remove);
				lineGyroFixY.removeIf(remove);
				lineGyroFixZ.removeIf(remove);
				lineAccX.removeIf(remove);
				lineAccY.removeIf(remove);
				lineAccZ.removeIf(remove);

				const float ax = 0.85 + std::cos(plotCurHead)*0.1;
				const float ay = 0.85 + std::sin(plotCurHead)*0.1;
				gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
				gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";


				gp1.draw(multiplot);
				gp1.flush();

				gp2.draw(plotPose);
				gp2.flush();

				//usleep(100);

			}

		#endif


		// done
		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.addAcc(ts, acc);

		if (1 == 0) {

			// FASTER

			// start with the first available timestamp
			if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}

			// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
			if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
				orientation.rotationMatrix = est.get();
				orientation.isKnown = true;
				orientation.lastEstimation = ts;
				est.reset();
			}

		} else {

			// MORE ACCURATE

			orientation.rotationMatrix = est.get();
			orientation.isKnown = true;
			orientation.lastEstimation = 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 sum;
		int cnt;

		XYZ() {
			reset();
		}

		/** add the given accelerometer reading */
		void addAcc(const Timestamp ts, const AccelerometerData& acc) {

			// did NOT improve the result for every smartphone (only some)
			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
			//if (deltaMag > 5.0) {return;}

			// adjust sum and count (for average calculation)
			Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
			sum += vec;
			++cnt;


		}

		AccelerometerData getAvg() const {
			return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
		}

		/** get the current rotation matrix estimation */
		Eigen::Matrix3f get() const {

			// get the current acceleromter average
			const Eigen::Vector3f avg = sum / cnt;

			// rotate average accelerometer into (0,0,1)
			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
			const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);

			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
			Eigen::Vector3f aligned = (rotMat * avg).normalized();
			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

			return rotMat;

		}

		/** reset the current sum etc. */
		void reset() {
			cnt = 0;
			sum = Eigen::Vector3f::Zero();
		}


	};


	struct XYZ2 {

		// average the accelerometer
		MovingAverageTS<AccelerometerData> avg = MovingAverageTS<AccelerometerData>(Timestamp::fromMS(1250), AccelerometerData());

		XYZ2() {

			// start approximately
			addAcc(Timestamp(), AccelerometerData(0,0,9.81));

		}

		/** add the given accelerometer reading */
		void addAcc(const Timestamp ts, const AccelerometerData& acc) {

			// did NOT improve the result for every smartphone (only some)
			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
			//if (deltaMag > 5.0) {return;}

			avg.add(ts, acc);

		}

		AccelerometerData getAvg() const {
			return avg.get();
		}

		/** get the current rotation matrix estimation */
		Eigen::Matrix3f get() const {

			// get the current acceleromter average
			AccelerometerData avgAcc = getAvg();
			const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);

			// rotate average-accelerometer into (0,0,1)
			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
			const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);

			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
			Eigen::Vector3f aligned = (rotMat * avg).normalized();
			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

			return rotMat;

		}

		/** reset the current sum etc. */
		void reset() {
			;
		}

	} 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