Indoor/sensors/imu/MotionDetection.h

#ifndef MOTIONDETECTION_H
#define MOTIONDETECTION_H

#include "GravityData.h"
#include "LinearAccelerationData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../misc/Debug.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 MotionDetection {

private:

    bool newAccelerationMeasurementArrived = false;
    bool newGravityMeasurementArrived = false;

    Eigen::Vector3f curGravity;
    Eigen::Vector3f curLinearAcceleration;

    //fast algo
    Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix

    int numMeasurementsPerInterval, updateCnt;
    int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms

    struct Motion{
        Eigen::Vector2f vec = Eigen::Vector2f::Identity();
        Timestamp lastEstimation;
    };

    Motion curMotion;
    Motion prevMotion;

    const char* name = "MotionDetection";

public:

	/** ctor */
    MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) {
        ;
	}

    void addGravity(const Timestamp& ts, const GravityData& grav){

            curGravity << grav.x, grav.y, grav.z;
            newGravityMeasurementArrived = true;

            updateProjectionVectorFast(ts);
    }

    void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) {

            curLinearAcceleration << acc.x, acc.y, acc.z;
            newAccelerationMeasurementArrived = true;

            updateProjectionVectorFast(ts);
    }

    /** return the current motion axis
     * NOTE: if no data is available, this vector is the Identity
    */
    Eigen::Vector2f getCurrentMotionAxis(){
        return curMotion.vec;
    }

    /** returns the radians between [-pi, pi] between successive motion axis estimations */
    float getMotionChangeInRad(){
        //TODO: put this in an EigenHelper Class within geo
        const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x();
        const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f));

        //nan?
        if(std::isnan(ang)){
            Log::add(name, "The motion change angle is nAn, this is not correct!");
        }

        if(updateCnt < 2){
            return 0;
        }

        return ang;
    }

private:

    FRIEND_TEST(MotionDetection, motionAngle);
    FRIEND_TEST(MotionDetection, motionAxis);

    Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covarianceMatrix);
        return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue
    }

    void updateProjectionVectorFast(const Timestamp& ts){

        //make sure we have both measurements for calculation
        if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){

            numMeasurementsPerInterval++;

            //project acc into perpendicular plane of grav (using standard vector projection)
            Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity;

            //if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this
            if(proj.isZero()){
                proj = curLinearAcceleration;
                Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?");
            }

            //we are only interested in x,y
            Eigen::Vector2f vec;
            vec << proj.x(), proj.y();

            // sum this up for later averaging.
            sumProjectedCov += vec*vec.transpose();

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

            //update the motion axis depending on the update interval
            if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){

                prevMotion = curMotion;

                //calculate the average of the coveriance matrix
                Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval;

                curMotion.vec = updateMotionAxis(Q);
                curMotion.lastEstimation = ts;

                reset();
            }

            newGravityMeasurementArrived = false;
            newAccelerationMeasurementArrived = false;
        }
        //do nothing
    }

    void reset(){
        numMeasurementsPerInterval = 0;
        sumProjectedCov = Eigen::Matrix2f::Zero();
        ++updateCnt;
    }

};

#endif // MOTIONDETECTION_H