Indoor/sensors/imu/TurnDetection.h

#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