Indoor/sensors/imu/PoseDetection.h

#ifndef INDOOR_IMU_POSEDETECTION_H
#define INDOOR_IMU_POSEDETECTION_H

#include "AccelerometerData.h"

#include "../../data/Timestamp.h"

#include "../../math/MovingStdDevTS.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingMedianTS.h"
#include "../../math/Matrix3.h"

#include "../../geo/Point3.h"

//#include <eigen3/Eigen/Dense>

#include "PoseDetectionPlot.h"
#include "PoseProvider.h"

/**
 * estimate the smartphones world-pose,
 * based on the accelerometer's data
 */
class PoseDetection : public PoseProvider {


	/** live-pose-estimation using moving average of the smartphone's accelerometer */
	struct EstMovingAverage {

		// average the accelerometer
		MovingAverageTS<AccelerometerData> avg;

		EstMovingAverage(const Timestamp window) :
		    avg(window, AccelerometerData()) {

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

		}

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

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

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

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

//			// rotate average-accelerometer into (0,0,1)
//			//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
//			const Vector3 zAxis(0,0,1);
//			const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis);
//			//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized());	// INVERSE
//			//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();
//			const Vector3 aligned = (rotMat * avg).normalized();
//			Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted");
//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

//			return rotMat;

//		}

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

			// https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
			// get the current acceleromter average
			const AccelerometerData avgAcc = avg.get();
			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
			const Point3 vZ = Point3(avgAcc.x, avgAcc.y, avgAcc.z).normalized();
			const Point3 vX = cross(Point3(0,1,0), vZ).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x);
			//const Point3 v2 = cross(v3, vx).normalized();
			const Point3 vY = cross(vZ, vX).normalized();

			Matrix3 rotMat({
			    vX.x, vY.x, vZ.x,
			    vX.y, vY.y, vZ.y,
			    vX.z, vY.z, vZ.z,
			});

			// above transposed = inverse matrix = undo rotation
			rotMat = rotMat.transposed();

//			// https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
//			// get the current acceleromter average
//			const AccelerometerData avgAcc = avg.get();
//			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
//			const Point3 vZ = Point3(-avgAcc.x, -avgAcc.y, -avgAcc.z).normalized();
//			const Point3 vX = cross(vZ, Point3(0,1,0)).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x);
//			//const Point3 v2 = cross(v3, vx).normalized();
//			const Point3 vY = cross(vX, vZ).normalized();

//			Matrix3 rotMat({
//			    vX.x, vY.x, vZ.x,
//			    vX.y, vY.y, vZ.y,
//			    vX.z, vY.z, vZ.z,
//			});

//			//rotMat = Matrix3::getRotationDeg(180, 0, 0) * rotMat;
//			//rotMat = Matrix3::getRotationDeg(0, 0, 180) * rotMat;

//			// above transposed = inverse matrix = undo rotation
//			//rotMat = rotMat.transposed();

			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
			const Vector3 zAxis(0,0,1);
			const Vector3 inp(avgAcc.x, avgAcc.y, avgAcc.z);
			const Vector3 aligned = (rotMat * inp).normalized();
			Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted");
			//Assert::isTrue((aligned-zAxis).norm() < 0.10f, "deviation too high");

			return rotMat;

		}

	};

//	/** live-pose-estimation using moving median of the smartphone's accelerometer */
//	struct EstMovingMedian {

//		// median the accelerometer
//		MovingMedianTS<float> medianX;
//		MovingMedianTS<float> medianY;
//		MovingMedianTS<float> medianZ;

//		EstMovingMedian(const Timestamp window) :
//			medianX(window), medianY(window), medianZ(window) {

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

//		}

//		/** add the given accelerometer reading */
//		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
//			medianX.add(ts, acc.x);
//			medianY.add(ts, acc.y);
//			medianZ.add(ts, acc.z);
//		}

//		AccelerometerData getBase() const {
//			return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
//		}

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

//			const Vector3 base(medianX.get(), medianY.get(), medianZ.get());

//			// rotate average-accelerometer into (0,0,1)
//			const Vector3 zAxis(0,0,1);
//			const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);

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

//			return rotMat;

//		}

//	};


private:

	struct {
		Matrix3 rotationMatrix = Matrix3::identity();
		float curSigma = 0;
		bool isKnown = false;
		Timestamp lastEstimation;
	} orientation;

	/** how the pose is estimated */
	//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
	EstMovingAverage est;
	//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));

	MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);

	#ifdef WITH_DEBUG_PLOT
	    int plotLimit = 0;
		PoseDetectionPlot plot;
	#endif


public:

	/** ctor */
	PoseDetection(const Timestamp delay = Timestamp::fromMS(450)) : est(delay) {
        #ifdef WITH_DEBUG_PLOT
		    plot.setName("PoseDetection1");
        #endif
	}

	/** get the smartphone's rotation matrix */
	const Matrix3& getMatrix() const override {
		return orientation.rotationMatrix;
	}

	/** is the pose known and stable? */
	bool isKnown() const override {
		return orientation.isKnown;
	}

	Matrix3 getMatrixGyro() const {
		return Matrix3::identity();
	}

	Matrix3 getMatrixAcc() const {
		return orientation.rotationMatrix;
	}

	/** current uncertainty */
	float getSigma() const {
		return orientation.curSigma;
	}


	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {

		// uncertainty
		const Vector3 curAcc = Vector3(acc.x, acc.y, acc.z);
		float angleDiff = std::acos(curAcc.normalized().dot(Vector3(0,0,1)));
		if (!std::isnan(angleDiff)) {
			stdDevForSigma.add(ts, angleDiff);
			orientation.curSigma = stdDevForSigma.get()*1;
		}

		// add accelerometer data
		est.addAcc(ts, acc);

		// update (if needed)
		orientation.rotationMatrix = est.get();
		orientation.isKnown = true;
		orientation.lastEstimation = ts;

		// debug-plot (if configured)
		#ifdef WITH_DEBUG_PLOT
		    if (++plotLimit > 5) {
				plot.add(ts, est.getBase(), orientation.rotationMatrix);
				plotLimit = 0;
			}
		#endif

	}

public:


	/** get a matrix that rotates the vector "from" into the vector "to" */
	static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {

		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another

		const Vector3 v = from.cross(to) / from.cross(to).norm();
		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );

		Matrix3 A({
			0.0f,	-v.z,	v.y,
			v.z,	0.0f,	-v.x,
			-v.y,	v.x,	0.0f
		});

		return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle)));

	}

};



#endif // INDOOR_IMU_POSEDETECTION_H