Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-06 22:32:57 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Kinect moving average filter for hands

Browse source
This commit is contained in:
Al Bernstein 2017-04-06 11:08:09 -07:00
parent 34a2cefb98
commit 4a17112478
2 changed files with 65 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

50
plugins/hifiKinect/src/KinectPlugin.cpp
View file

@ -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;
}

17
plugins/hifiKinect/src/KinectPlugin.h
View file

@ -16,6 +16,12 @@
#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
#endif
// glm files
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
// Windows Header Files
#include <windows.h>
@ -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
};