add parallel biquad filter, add virtual screen threshold as parameter

Author: cristeab

GestureAlgos.cpp

@@ -1,13 +1,15 @@
 #include <qglobal.h>
 #include <QDebug>
 #include "GestureAlgos.h"
+#include "VirtualTouchScreen.h"
 
 GestureAlgos::GestureAlgos() :
 	screen_(0, 0),
 	image_(0,0),
 	scaleFactor_(0.0), offset_(0,0),
 	KF_(cv::KalmanFilter(8, 4, 0)),
-	measurement_(cv::Mat_<float>(4,1))
+	measurement_(cv::Mat_<float>(4,1)),
+	mainWnd_(NULL)
 {
 }
 
@@ -124,10 +126,12 @@ void GestureAlgos::initBiquad()
     sos_mat_[4][0] = 1.0, sos_mat_[4][1] = 2.0, sos_mat_[4][2] = 1.0, sos_mat_[4][3] = 1.0,
 		sos_mat_[4][4] = 0.5304, sos_mat_[4][5] = 0.7165;
 	gain_ = 0.0189;
-	for (int n = 0; n < SosMat::NB_BIQUADS; ++n) {
-		biquadState[n].index = n;
-		memset(biquadState[n].mem_in, 0, sizeof(biquadState[n].mem_in));
-		memset(biquadState[n].mem_out, 0, sizeof(biquadState[n].mem_out));
+	for (int i = 0; i < 2; ++i) {
+		for (int n = 0; n < SosMat::NB_BIQUADS; ++n) {
+			biquadState[i][n].index = n;
+			memset(biquadState[i][n].mem_in, 0, sizeof(biquadState[i][n].mem_in));
+			memset(biquadState[i][n].mem_out, 0, sizeof(biquadState[i][n].mem_out));
+		}
 	}
 }
 
@@ -148,7 +152,7 @@ double GestureAlgos::biquad(BiquadState *state, double in)
 	return out;
 }
 
-void GestureAlgos::filterLowPass(qreal &depth)
+void GestureAlgos::filterLowPass(qreal &depthThumb, qreal &depthIndex)
 {
 	static bool initDone = false;
 	if (!initDone) {
@@ -157,15 +161,15 @@ void GestureAlgos::filterLowPass(qreal &depth)
 	}
 	//cascade of biquads
 	for (int n = 0; n < SosMat::NB_BIQUADS; ++n) {
-		depth = biquad(biquadState+n, depth);
+		depthThumb = biquad(&(biquadState[0][n]), depthThumb);
+		depthIndex = biquad(&(biquadState[1][n]), depthIndex);
 	}
-	depth = gain_*depth;
+	depthThumb = gain_*depthThumb;
+	depthIndex = gain_*depthIndex;
 }
 
 GestureAlgos::TouchType GestureAlgos::isTouch(qreal depthThumb, qreal depthIndex)
 {
-	//constants
-	const static qreal DEPTH_THRESHOLD = 0.45;
 	//permanent variable
 	static GestureAlgos::TouchType out = GestureAlgos::TouchType::NONE;
 
@@ -177,9 +181,11 @@ GestureAlgos::TouchType GestureAlgos::isTouch(qreal depthThumb, qreal depthIndex
 	}
 
 	//process depth information
-	if ((DEPTH_THRESHOLD >= depthThumb) || (DEPTH_THRESHOLD >= depthIndex)) {
+	if ((mainWnd_->virtualScreenThreshold_ >= depthThumb) || 
+		(mainWnd_->virtualScreenThreshold_ >= depthIndex)) {
 		//touch down
-		out = ((DEPTH_THRESHOLD >= depthThumb) && (DEPTH_THRESHOLD >= depthIndex))?
+		out = ((mainWnd_->virtualScreenThreshold_ >= depthThumb) && 
+			(mainWnd_->virtualScreenThreshold_ >= depthIndex))?
 			GestureAlgos::TouchType::DOUBLE_DOWN:GestureAlgos::TouchType::SINGLE_DOWN;
 	} else {
 		//touch up

GestureAlgos.h

@@ -4,6 +4,8 @@
 #include <QSize>
 #include "opencv2/video/tracking.hpp"
 
+class VirtualTouchScreen;
+
 class GestureAlgos
 {
 public:
@@ -29,11 +31,14 @@ class GestureAlgos
 		scaleFactor_ = scaleFactor;
 		offset_ = offset;
 	}
+	void setMainWindow(const VirtualTouchScreen *w) {
+		mainWnd_ = w;
+	}
 	//transforms to screen coordinates
 	int imageToScreen(QPointF &pt);
 	//filters
 	int filterKalman(QPointF &ptThumb, QPointF &ptIndex);
-	void filterLowPass(qreal &depth);
+	void filterLowPass(qreal &depthThumb, qreal &depthIndex);
 	//detect when the hand touches the virtual touch screen
 	//the OS decides which gesture has been made
 	enum TouchType {NONE = 0, SINGLE_DOWN, SINGLE_UP, DOUBLE_DOWN, DOUBLE_UP};
@@ -50,13 +55,14 @@ class GestureAlgos
 	enum SosMat {NB_BIQUADS = 5};
 	double sos_mat_[SosMat::NB_BIQUADS][6];
 	double gain_;
-	BiquadState biquadState[SosMat::NB_BIQUADS];
+	BiquadState biquadState[2][SosMat::NB_BIQUADS];//two parallel biquad filters
 	QSize screen_;
 	QSize image_;
 	qreal scaleFactor_;
 	QPointF offset_;
 	cv::KalmanFilter KF_;
 	cv::Mat_<float> measurement_;
+	const VirtualTouchScreen *mainWnd_;
 	GestureAlgos(const GestureAlgos&);
 	GestureAlgos& operator=(const GestureAlgos&);
 	~GestureAlgos() {};

GestureThread.cpp

@@ -134,11 +134,13 @@ void GestureThread::run()
 					mainWnd->gestureAlgos->imageToScreen(mainWnd->handSkeletonPoints_[i]);
 					depth[i] = static_cast<qreal>(fingerNode[i].positionWorld.y);					
 				}
+				//Kalman filter for coordinates
 				mainWnd->gestureAlgos->filterKalman(
 					mainWnd->handSkeletonPoints_[VirtualTouchScreen::THUMB],
 					mainWnd->handSkeletonPoints_[VirtualTouchScreen::INDEX]);
 				mainWnd->skeletonPointMutex_.unlock();
-				//TODO: low pass filter depth
+				//low pass filter for the depth
+				mainWnd->gestureAlgos->filterLowPass(depth[0], depth[1]);
 
 				//request finger position update
 				emit moveIndex();

VirtualTouchScreen.cpp

@@ -22,7 +22,8 @@ VirtualTouchScreen::VirtualTouchScreen(QWidget *parent)
 	thumbPointer(new QWidget()),
 	config(NULL),
 	handSkeletonPoints_(Hand::POINTS),
-	touch_(new TouchInputEmulator())
+	touch_(new TouchInputEmulator()),
+	virtualScreenThreshold_(0.35)
 {
 	setWindowFlags(Qt::WindowStaysOnTopHint | Qt::FramelessWindowHint);
 	thumbPointer->setWindowFlags(Qt::WindowStaysOnTopHint | Qt::FramelessWindowHint);
@@ -41,6 +42,7 @@ VirtualTouchScreen::VirtualTouchScreen(QWidget *parent)
 	gestureAlgos = GestureAlgos::instance();
 	gestureAlgos->setScreenSize(QSize(geom.width(), geom.height()));
 	gestureAlgos->setCorrectionFactors(scaleFactor, offset);
+	gestureAlgos->setMainWindow(this);
 
 	//config = new ConfigDialog(NULL, this);//screen size must be set
 

VirtualTouchScreen.h

@@ -19,6 +19,7 @@ class VirtualTouchScreen : public QMainWindow
 	Q_OBJECT
 	friend class GestureThread;
 	friend class ConfigDialog;
+	friend class GestureAlgos;
 public:
 	explicit VirtualTouchScreen(QWidget *parent = 0);
 	~VirtualTouchScreen();
@@ -52,6 +53,7 @@ public slots:
 	QVector<QPointF> handSkeletonPoints_;
 	QMutex skeletonPointMutex_;
 	TouchInputEmulator *touch_;
+	qreal virtualScreenThreshold_;
 };
 
 #endif // VirtualTouchScreen_H

tests/unit_tests.cpp

@@ -17,24 +17,24 @@ private slots:
 
 void TestGestureAlgos::imageToScreenFilterKalman()
 {
-	/*GestureAlgos *algos = GestureAlgos::instance();
+	GestureAlgos *algos = GestureAlgos::instance();
 	QVERIFY(NULL != algos);
 	QPointF pt(0, 0);
-	QVERIFY(EXIT_FAILURE == algos->filterKalman(pt));
+	QVERIFY(EXIT_FAILURE == algos->filterKalman(pt, pt));
 	QVERIFY(EXIT_FAILURE == algos->imageToScreen(pt));
 	algos->setScreenSize(QSize(1024, 768));
-	QVERIFY(EXIT_SUCCESS == algos->filterKalman(pt));
+	QVERIFY(EXIT_SUCCESS == algos->filterKalman(pt, pt));
 	QVERIFY(EXIT_FAILURE == algos->imageToScreen(pt));
 	algos->setImageSize(QSize(320, 240));
-	QVERIFY(EXIT_SUCCESS == algos->filterKalman(pt));
+	QVERIFY(EXIT_SUCCESS == algos->filterKalman(pt, pt));
 	QVERIFY(EXIT_FAILURE == algos->imageToScreen(pt));
 	algos->setCorrectionFactors(1.0, QPoint(0, 0));
-	QVERIFY(EXIT_SUCCESS == algos->filterKalman(pt));
+	QVERIFY(EXIT_SUCCESS == algos->filterKalman(pt, pt));
 	pt.setX(0);
 	pt.setY(0);
 	QVERIFY(EXIT_SUCCESS == algos->imageToScreen(pt));
 	QVERIFY(1024.0 == pt.x());
-	QVERIFY(0.0 == pt.y());*/
+	QVERIFY(0.0 == pt.y());
 }
 
 void TestGestureAlgos::filterLowPass()
@@ -46,24 +46,28 @@ void TestGestureAlgos::filterLowPass()
 	const static qreal PI = 3.1415926535;
 	const static qreal TOL = 1e-3;
 	int nbSamp = static_cast<int>(10/fn);
-	qreal val = 0;
-	qreal sum = 0;
+	qreal val[] = {0,0};
+	qreal sum[] = {0,0};
 	for (int n = 0; n < nbSamp; ++n) {
-		val = sin(2*PI*fn*n);
-		algos->filterLowPass(val);
-		sum += abs(val*val);
+		val[0] = val[1] = sin(2*PI*fn*n);
+		algos->filterLowPass(val[0], val[1]);
+		sum[0] += abs(val[0]*val[0]);
+		sum[1] += abs(val[1]*val[1]);
 	}
-	QVERIFY(fabs(48.5314 - sum) < TOL);
+	QVERIFY(fabs(48.5314 - sum[0]) < TOL);
+	QVERIFY(fabs(48.5314 - sum[1]) < TOL);
 
 	fn = 0.4;
 	nbSamp = static_cast<int>(10/fn);
-	sum = 0;
+	sum[0] = sum[1] = 0;
 	for (int n = 0; n < nbSamp; ++n) {
-		val = sin(2*PI*fn*n);
-		algos->filterLowPass(val);
-		sum += abs(val*val);
+		val[0] = val[1] = sin(2*PI*fn*n);
+		algos->filterLowPass(val[0], val[1]);
+		sum[0] += abs(val[0]*val[0]);
+		sum[1] += abs(val[1]*val[1]);
 	}
-	QVERIFY(fabs(1.58709 - sum) < TOL);
+	QVERIFY(fabs(1.58709 - sum[0]) < TOL);
+	QVERIFY(fabs(1.58709 - sum[1]) < TOL);
 }
 
 int TestGestureAlgos::getData(QVector<int> &x, QVector<int> &y, QVector<qreal> &depth,