From 4a171124786acffc9e3dee68be77ee42a2077b3d Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 11:08:09 -0700 Subject: [PATCH 1/7] Kinect moving average filter for hands --- plugins/hifiKinect/src/KinectPlugin.cpp | 50 ++++++++++++++++++++++++- plugins/hifiKinect/src/KinectPlugin.h | 17 +++++++++ 2 files changed, 65 insertions(+), 2 deletions(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 6d29a261dd..2c9568cb50 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -248,6 +248,7 @@ void KinectPlugin::init() { auto preference = new CheckPreference(KINECT_PLUGIN, "Extra Debugging", debugGetter, debugSetter); preferences->addPreference(preference); } + } bool KinectPlugin::isSupported() const { @@ -493,11 +494,15 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { //_joints[j].orientation = jointOrientation; if (joints[j].JointType == JointType_HandRight) { static const quat kinectToHandRight = glm::angleAxis(-PI / 2.0f, Vectors::UNIT_Y); - _joints[j].orientation = jointOrientation * kinectToHandRight; + // add moving average of orientation quaternion + glm::quat jointTmp = jointOrientation * kinectToHandRight; + _joints[j].orientation = QMAR(jointTmp); } else if (joints[j].JointType == JointType_HandLeft) { // To transform from Kinect to our LEFT Hand.... Postive 90 deg around Y static const quat kinectToHandLeft = glm::angleAxis(PI / 2.0f, Vectors::UNIT_Y); - _joints[j].orientation = jointOrientation * kinectToHandLeft; + // add moving average of orientation quaternion + glm::quat jointTmp = jointOrientation * kinectToHandLeft; + _joints[j].orientation = QMAL(jointTmp); } else { _joints[j].orientation = jointOrientation; } @@ -644,3 +649,44 @@ void KinectPlugin::InputDevice::clearState() { _poseStateMap[poseIndex] = controller::Pose(); } } + +glm::quat KinectPlugin::QMAL(glm::quat q) +{ + if (glm::dot(_q_barL, q) < 0) + _q_barL = (1 - _deltaL)*_q_barL + -_deltaL*q; + else + _q_barL = (1 - _deltaL)*_q_barL + _deltaL*q; + + _q_barL = glm::normalize(_q_barL); + + + if (_debug) + { + qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; + qDebug() << __FUNCTION__ << "_q_barL = " << _q_barL.x << "," << _q_barL.y << "," << _q_barL.z << "," << _q_barL.w; + } + return _q_barL; +} + + +glm::quat KinectPlugin::QMAR(glm::quat q) +{ + if (glm::dot(_q_barR, q) < 0) + _q_barR = (1 - _deltaR)*_q_barR + -_deltaR*q; + else + _q_barR = (1 - _deltaR)*_q_barR + _deltaR*q; + + _q_barR = glm::normalize(_q_barR); + + + _q_barR = -_q_barR; + + if (_debug) + { + qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; + qDebug() << __FUNCTION__ << "_q_barL = " << _q_barR.x << "," << _q_barR.y << "," << _q_barR.z << "," << _q_barR.w; + } + + return _q_barR; +} + diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 90794fa6b0..16e41cce8d 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -16,6 +16,12 @@ #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers #endif +// glm files + +#include +#include + + // Windows Header Files #include @@ -58,6 +64,14 @@ public: virtual void saveSettings() const override; virtual void loadSettings() override; +private: + // add variables for moving average + + glm::quat _q_barL; + glm::quat _q_barR; + float _deltaL = (float)0.5; + float _deltaR = (float)0.5; + protected: struct KinectJoint { @@ -113,6 +127,9 @@ protected: // Body reader mutable IBodyFrameReader* _bodyFrameReader { nullptr }; + // moving average for a quaternion + glm::quat QMAL(glm::quat q); + glm::quat QMAR(glm::quat q); #endif }; From 001c92416b45848e41a4832ab7dc753f3984de5c Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 11:23:33 -0700 Subject: [PATCH 2/7] clean ups - coding standard --- plugins/hifiKinect/src/KinectPlugin.cpp | 73 ++++++++++++------------- 1 file changed, 36 insertions(+), 37 deletions(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 2c9568cb50..165bca266c 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -248,7 +248,6 @@ void KinectPlugin::init() { auto preference = new CheckPreference(KINECT_PLUGIN, "Extra Debugging", debugGetter, debugSetter); preferences->addPreference(preference); } - } bool KinectPlugin::isSupported() const { @@ -494,15 +493,15 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { //_joints[j].orientation = jointOrientation; if (joints[j].JointType == JointType_HandRight) { static const quat kinectToHandRight = glm::angleAxis(-PI / 2.0f, Vectors::UNIT_Y); - // add moving average of orientation quaternion - glm::quat jointTmp = jointOrientation * kinectToHandRight; - _joints[j].orientation = QMAR(jointTmp); + // add moving average of orientation quaternion + glm::quat jointTmp = jointOrientation * kinectToHandRight; + _joints[j].orientation = QMAR(jointTmp); } else if (joints[j].JointType == JointType_HandLeft) { // To transform from Kinect to our LEFT Hand.... Postive 90 deg around Y static const quat kinectToHandLeft = glm::angleAxis(PI / 2.0f, Vectors::UNIT_Y); - // add moving average of orientation quaternion - glm::quat jointTmp = jointOrientation * kinectToHandLeft; - _joints[j].orientation = QMAL(jointTmp); + // add moving average of orientation quaternion + glm::quat jointTmp = jointOrientation * kinectToHandLeft; + _joints[j].orientation = QMAL(jointTmp); } else { _joints[j].orientation = jointOrientation; } @@ -652,41 +651,41 @@ void KinectPlugin::InputDevice::clearState() { glm::quat KinectPlugin::QMAL(glm::quat q) { - if (glm::dot(_q_barL, q) < 0) - _q_barL = (1 - _deltaL)*_q_barL + -_deltaL*q; - else - _q_barL = (1 - _deltaL)*_q_barL + _deltaL*q; - - _q_barL = glm::normalize(_q_barL); - - - if (_debug) - { - qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; - qDebug() << __FUNCTION__ << "_q_barL = " << _q_barL.x << "," << _q_barL.y << "," << _q_barL.z << "," << _q_barL.w; - } - return _q_barL; + if (glm::dot(_q_barL, q) < 0) { + _q_barL = (1 - _deltaL) * _q_barL + -_deltaL * q; + } + else { + _q_barL = (1 - _deltaL) * _q_barL + _deltaL * q; + } + + _q_barL = glm::normalize(_q_barL); + if (_debug) + { + qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; + qDebug() << __FUNCTION__ << "_q_barL = " << _q_barL.x << "," << _q_barL.y << "," << _q_barL.z << "," << _q_barL.w; + } + return _q_barL; } glm::quat KinectPlugin::QMAR(glm::quat q) { - if (glm::dot(_q_barR, q) < 0) - _q_barR = (1 - _deltaR)*_q_barR + -_deltaR*q; - else - _q_barR = (1 - _deltaR)*_q_barR + _deltaR*q; - - _q_barR = glm::normalize(_q_barR); - - - _q_barR = -_q_barR; - - if (_debug) - { - qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; - qDebug() << __FUNCTION__ << "_q_barL = " << _q_barR.x << "," << _q_barR.y << "," << _q_barR.z << "," << _q_barR.w; - } + if (glm::dot(_q_barR, q) < 0){ + _q_barR = (1 - _deltaR)*_q_barR + -_deltaR*q; + } + else { + _q_barR = (1 - _deltaR)*_q_barR + _deltaR*q; + } - return _q_barR; + _q_barR = glm::normalize(_q_barR); + _q_barR = -_q_barR; + + if (_debug) + { + qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; + qDebug() << __FUNCTION__ << "_q_barL = " << _q_barR.x << "," << _q_barR.y << "," << _q_barR.z << "," << _q_barR.w; + } + + return _q_barR; } From e6e522464770b0c81188f3cbd31c4f65b467dc37 Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 11:27:58 -0700 Subject: [PATCH 3/7] clean ups - coding standard --- plugins/hifiKinect/src/KinectPlugin.cpp | 14 ++++++-------- plugins/hifiKinect/src/KinectPlugin.h | 16 +++++----------- 2 files changed, 11 insertions(+), 19 deletions(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 165bca266c..d9888ca8d6 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -653,8 +653,7 @@ glm::quat KinectPlugin::QMAL(glm::quat q) { if (glm::dot(_q_barL, q) < 0) { _q_barL = (1 - _deltaL) * _q_barL + -_deltaL * q; - } - else { + } else { _q_barL = (1 - _deltaL) * _q_barL + _deltaL * q; } @@ -670,15 +669,14 @@ glm::quat KinectPlugin::QMAL(glm::quat q) glm::quat KinectPlugin::QMAR(glm::quat q) { - if (glm::dot(_q_barR, q) < 0){ - _q_barR = (1 - _deltaR)*_q_barR + -_deltaR*q; - } - else { - _q_barR = (1 - _deltaR)*_q_barR + _deltaR*q; + if (glm::dot(_q_barR, q) < 0) { + _q_barR = (1 - _deltaR) * _q_barR + -_deltaR*q; + } else { + _q_barR = (1 - _deltaR) * _q_barR + _deltaR * q; } _q_barR = glm::normalize(_q_barR); - _q_barR = -_q_barR; + _q_barR = -_q_barR; if (_debug) { diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 16e41cce8d..114d4b76aa 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -16,12 +16,6 @@ #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers #endif -// glm files - -#include -#include - - // Windows Header Files #include @@ -65,12 +59,12 @@ public: virtual void loadSettings() override; private: - // add variables for moving average + // add variables for moving average - glm::quat _q_barL; - glm::quat _q_barR; - float _deltaL = (float)0.5; - float _deltaR = (float)0.5; + glm::quat _q_barL; + glm::quat _q_barR; + float _deltaL = 0.5f; + float _deltaR = 0.5f; protected: From de1fdfacca9318359cf99f64b25fcb8d26fb7bd2 Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 16:18:40 -0700 Subject: [PATCH 4/7] made requested changes in PR10147 --- libraries/shared/src/SimpleMovingAverage.h | 7 ++- plugins/hifiKinect/src/KinectPlugin.cpp | 58 ++++++---------------- plugins/hifiKinect/src/KinectPlugin.h | 10 ++-- 3 files changed, 22 insertions(+), 53 deletions(-) diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 858ee21371..2559bb36ec 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -61,7 +61,7 @@ public: const float WEIGHTING = 1.0f / (float)MAX_NUM_SAMPLES; const float ONE_MINUS_WEIGHTING = 1.0f - WEIGHTING; std::atomic numSamples{ 0 }; - std::atomic average; + T average; void clear() { numSamples = 0; @@ -71,12 +71,15 @@ public: void addSample(T sample) { if (numSamples > 0) { - average = ((float)sample * WEIGHTING) + ((float)average * ONE_MINUS_WEIGHTING); + average = (sample * WEIGHTING) + (average * ONE_MINUS_WEIGHTING); } else { average = sample; } numSamples++; } + + + }; template class ThreadSafeMovingAverage { diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index d9888ca8d6..8f10efed86 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -21,6 +21,7 @@ #include #include + Q_DECLARE_LOGGING_CATEGORY(inputplugins) Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") @@ -494,14 +495,22 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { if (joints[j].JointType == JointType_HandRight) { static const quat kinectToHandRight = glm::angleAxis(-PI / 2.0f, Vectors::UNIT_Y); // add moving average of orientation quaternion - glm::quat jointTmp = jointOrientation * kinectToHandRight; - _joints[j].orientation = QMAR(jointTmp); + glm::quat jointSample = jointOrientation * kinectToHandRight; + if (glm::dot(jointSample, _RightHandOrientationAverage.average) < 0) { + jointSample = -jointSample; + } + _RightHandOrientationAverage.addSample(jointSample); + _joints[j].orientation = glm::normalize(_RightHandOrientationAverage.average); } else if (joints[j].JointType == JointType_HandLeft) { // To transform from Kinect to our LEFT Hand.... Postive 90 deg around Y static const quat kinectToHandLeft = glm::angleAxis(PI / 2.0f, Vectors::UNIT_Y); // add moving average of orientation quaternion - glm::quat jointTmp = jointOrientation * kinectToHandLeft; - _joints[j].orientation = QMAL(jointTmp); + glm::quat jointSample = jointOrientation * kinectToHandLeft; + if (glm::dot(jointSample, _LeftHandOrientationAverage.average) < 0) { + jointSample = -jointSample; + } + _LeftHandOrientationAverage.addSample(jointSample); + _joints[j].orientation = glm::normalize(_LeftHandOrientationAverage.average); } else { _joints[j].orientation = jointOrientation; } @@ -647,43 +656,4 @@ void KinectPlugin::InputDevice::clearState() { int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i); _poseStateMap[poseIndex] = controller::Pose(); } -} - -glm::quat KinectPlugin::QMAL(glm::quat q) -{ - if (glm::dot(_q_barL, q) < 0) { - _q_barL = (1 - _deltaL) * _q_barL + -_deltaL * q; - } else { - _q_barL = (1 - _deltaL) * _q_barL + _deltaL * q; - } - - _q_barL = glm::normalize(_q_barL); - if (_debug) - { - qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; - qDebug() << __FUNCTION__ << "_q_barL = " << _q_barL.x << "," << _q_barL.y << "," << _q_barL.z << "," << _q_barL.w; - } - return _q_barL; -} - - -glm::quat KinectPlugin::QMAR(glm::quat q) -{ - if (glm::dot(_q_barR, q) < 0) { - _q_barR = (1 - _deltaR) * _q_barR + -_deltaR*q; - } else { - _q_barR = (1 - _deltaR) * _q_barR + _deltaR * q; - } - - _q_barR = glm::normalize(_q_barR); - _q_barR = -_q_barR; - - if (_debug) - { - qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; - qDebug() << __FUNCTION__ << "_q_barL = " << _q_barR.x << "," << _q_barR.y << "," << _q_barR.z << "," << _q_barR.w; - } - - return _q_barR; -} - +} \ No newline at end of file diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 114d4b76aa..73cc9f4103 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -23,6 +23,7 @@ // Kinect Header files #include +#include // Safe release for interfaces template inline void SafeRelease(Interface *& pInterfaceToRelease) { @@ -61,10 +62,8 @@ public: private: // add variables for moving average - glm::quat _q_barL; - glm::quat _q_barR; - float _deltaL = 0.5f; - float _deltaR = 0.5f; + MovingAverage _LeftHandOrientationAverage; + MovingAverage _RightHandOrientationAverage; protected: @@ -121,9 +120,6 @@ protected: // Body reader mutable IBodyFrameReader* _bodyFrameReader { nullptr }; - // moving average for a quaternion - glm::quat QMAL(glm::quat q); - glm::quat QMAR(glm::quat q); #endif }; From e737456dcd40175c3948258d14985f4770ab7de6 Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 16:56:03 -0700 Subject: [PATCH 5/7] used thread safe moving average for the quaternions --- libraries/shared/src/SimpleMovingAverage.h | 2 +- plugins/hifiKinect/src/KinectPlugin.cpp | 8 ++++---- plugins/hifiKinect/src/KinectPlugin.h | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 2559bb36ec..783c13d4b1 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -61,7 +61,7 @@ public: const float WEIGHTING = 1.0f / (float)MAX_NUM_SAMPLES; const float ONE_MINUS_WEIGHTING = 1.0f - WEIGHTING; std::atomic numSamples{ 0 }; - T average; + std::atomic average; void clear() { numSamples = 0; diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 8f10efed86..a6bf5df9dd 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -496,21 +496,21 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { static const quat kinectToHandRight = glm::angleAxis(-PI / 2.0f, Vectors::UNIT_Y); // add moving average of orientation quaternion glm::quat jointSample = jointOrientation * kinectToHandRight; - if (glm::dot(jointSample, _RightHandOrientationAverage.average) < 0) { + if (glm::dot(jointSample, _RightHandOrientationAverage.getAverage()) < 0) { jointSample = -jointSample; } _RightHandOrientationAverage.addSample(jointSample); - _joints[j].orientation = glm::normalize(_RightHandOrientationAverage.average); + _joints[j].orientation = glm::normalize(_RightHandOrientationAverage.getAverage()); } else if (joints[j].JointType == JointType_HandLeft) { // To transform from Kinect to our LEFT Hand.... Postive 90 deg around Y static const quat kinectToHandLeft = glm::angleAxis(PI / 2.0f, Vectors::UNIT_Y); // add moving average of orientation quaternion glm::quat jointSample = jointOrientation * kinectToHandLeft; - if (glm::dot(jointSample, _LeftHandOrientationAverage.average) < 0) { + if (glm::dot(jointSample, _LeftHandOrientationAverage.getAverage()) < 0) { jointSample = -jointSample; } _LeftHandOrientationAverage.addSample(jointSample); - _joints[j].orientation = glm::normalize(_LeftHandOrientationAverage.average); + _joints[j].orientation = glm::normalize(_LeftHandOrientationAverage.getAverage()); } else { _joints[j].orientation = jointOrientation; } diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 73cc9f4103..68ba1a7b86 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -62,8 +62,8 @@ public: private: // add variables for moving average - MovingAverage _LeftHandOrientationAverage; - MovingAverage _RightHandOrientationAverage; + ThreadSafeMovingAverage _LeftHandOrientationAverage; + ThreadSafeMovingAverage _RightHandOrientationAverage; protected: From c628b23895336976b38c21eaeac6d81ed07257a2 Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 17:21:58 -0700 Subject: [PATCH 6/7] removed whitespace --- libraries/shared/src/SimpleMovingAverage.h | 3 --- plugins/hifiKinect/src/KinectPlugin.cpp | 2 -- 2 files changed, 5 deletions(-) diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 783c13d4b1..0404ab9646 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -77,9 +77,6 @@ public: } numSamples++; } - - - }; template class ThreadSafeMovingAverage { diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index a6bf5df9dd..3a36be0982 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -21,11 +21,9 @@ #include #include - Q_DECLARE_LOGGING_CATEGORY(inputplugins) Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") - const char* KinectPlugin::NAME = "Kinect"; const char* KinectPlugin::KINECT_ID_STRING = "Kinect"; From ead4793392145d83177efc89700ac4be2c1e639b Mon Sep 17 00:00:00 2001 From: Al Bernstein Date: Thu, 6 Apr 2017 17:26:12 -0700 Subject: [PATCH 7/7] removed whitespace --- plugins/hifiKinect/src/KinectPlugin.h | 1 - 1 file changed, 1 deletion(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 68ba1a7b86..158e66ee62 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -61,7 +61,6 @@ public: private: // add variables for moving average - ThreadSafeMovingAverage _LeftHandOrientationAverage; ThreadSafeMovingAverage _RightHandOrientationAverage;