Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-04-06 16:42:50 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

made requested changes in PR10147

Browse source
This commit is contained in:
Al Bernstein 2017-04-06 16:18:40 -07:00
parent e6e5224647
commit de1fdfacca
3 changed files with 22 additions and 53 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
libraries/shared/src/SimpleMovingAverage.h
View file

@ -61,7 +61,7 @@ public:
const float WEIGHTING = 1.0f / (float)MAX_NUM_SAMPLES;
const float ONE_MINUS_WEIGHTING = 1.0f - WEIGHTING;
std::atomic<int> numSamples{ 0 };
std::atomic<T> 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 T, int MAX_NUM_SAMPLES> class ThreadSafeMovingAverage {

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

@ -21,6 +21,7 @@
#include <Preferences.h>
#include <SettingHandle.h>
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;
}
}

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

@ -23,6 +23,7 @@
// Kinect Header files
#include <Kinect.h>
#include <SimpleMovingAverage.h>
// Safe release for interfaces
template<class Interface> 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<glm::quat, 2> _LeftHandOrientationAverage;
MovingAverage<glm::quat, 2> _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
};