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 <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<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() {
		;
	}


	// 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);

		// 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(acc);

		// 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();
		}

	}

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

		}

		/** 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();
		}


	} 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