Indoor/sensors/imu/StepDetection4.h

#ifndef STEPDETECTION4_H
#define STEPDETECTION4_H


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

#include "PoseDetection.h"

#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/GnuplotPlotElementPoints.h>
#endif

#ifdef WITH_DEBUG_OUTPUT
	#include <fstream>
#endif

#include "../../Assertions.h"
#include "../../math/dsp/iir/BiQuad.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/DelayBuffer.h"


/**
 * step detection based on accelerometer data,
 * un-rotated using pose-detection
 * interpolated to a fixed frequency
 * passed through IIR filter
 * searching for zero crossings that follow a peak value
 * that is above a certain threshold
 */
class StepDetection4 {

	static constexpr float gravity = 9.81;
	static constexpr float stepRate_hz = 2.0;
	static constexpr int sRate_hz = 100;
	static constexpr int every_ms = 1000 / sRate_hz;
	static constexpr float threshold = 1.0;

	float max = 0;
	Timestamp maxTS;

private:

	PoseDetection* pose;
	FixedFrequencyInterpolator<Vector3> interpol;
	IIR::BiQuad<float> biquad;
	DelayBuffer<float> delay;


	#ifdef WITH_DEBUG_PLOT
		K::Gnuplot gp;
		K::GnuplotPlot plot;
		K::GnuplotPlotElementLines lineRaw;
		K::GnuplotPlotElementLines lineFiltered;
		K::GnuplotPlotElementPoints pointDet;
		Timestamp plotRef;
		Timestamp lastPlot;
	#endif

	#ifdef WITH_DEBUG_OUTPUT
		std::ofstream outFiltered;
		std::ofstream outSteps;
	#endif


public:

	/** ctor */
	StepDetection4(PoseDetection* pose) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {

		plot.getKey().setVisible(true);
		lineRaw.setTitle("unrotated Z");
		lineFiltered.setTitle("IIR filtered");

		biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
		biquad.preFill(gravity);

		#ifdef WITH_DEBUG_PLOT
			gp << "set autoscale xfix\n";
			plot.setTitle("Step Detection");
			plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
			plot.add(&lineFiltered); lineFiltered.getStroke().getColor().setHexStr("#000000");
			plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
		#endif

		#ifdef WITH_DEBUG_OUTPUT
			outFiltered = std::ofstream("/tmp/sd2_filtered.dat");
			outSteps = std::ofstream("/tmp/sd2_steps.dat");
		#endif

	}

	/** does the given data indicate a step? */
	bool add(const Timestamp ts, const AccelerometerData& _acc) {

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

		// get the current accs-reading as vector
		const Vector3 vec(_acc.x, _acc.y, _acc.z);

		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
		const Vector3 acc = pose->getMatrix() * vec;

		// will be set when a step was detected
		bool gotStep = false;

		// accel-data incoming on a fixed sampling rate (needed for FIR to work)
		// NOTE!!!! MIGHT TRIGGER MORE THAN ONCE PER add() !!!
		auto onResample = [&] (const Timestamp ts, const Vector3 data) {

			bool step = false;
			const float mag = data.z;

			// apply filter
			const float fMag = biquad.filter(mag);

			// history buffer
			float fMagOld = delay.add(fMag);

			// zero crossing?
			float tmp = max;
			if (fMagOld > 0 && fMag < 0) {
				if (max > threshold) {
					step = true;
					gotStep = true;
				}
				delay.setAll(0);
				max = 0;
			}

			// track maximum value
			if (fMag > max) {max = fMag; maxTS = ts;}

			#ifdef WITH_DEBUG_OUTPUT
				if (step) {
					std::cout << ts.ms() << std::endl;
					outSteps << maxTS.ms() << " " << tmp << "\n";
					outSteps.flush();
				}
				outFiltered << ts.ms() << " " << fMag << "\n";
			#endif

			#ifdef WITH_DEBUG_PLOT

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

				lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), data.z) );
				lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );

				if (step) {
					pointDet.add( K::GnuplotPoint2((maxTS-plotRef).ms(), tmp) );
				}

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

					lastPlot = tsPlot;
					auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
					lineRaw.removeIf(remove);
					lineFiltered.removeIf(remove);
					pointDet.removeIf(remove);

					gp.draw(plot);
					gp.flush();
					usleep(100);

				}

			#endif

		};

		// ensure fixed sampling rate for FIR freq filters to work!
		interpol.add(ts, acc, onResample);

		return gotStep;


	}

};



#endif // STEPDETECTION4_H