From bdfbc131851154ee1f8e1aa3609edfc9c9e40397 Mon Sep 17 00:00:00 2001 From: ZappoMan Date: Tue, 28 Mar 2017 15:09:03 -0700 Subject: [PATCH 1/3] add support for hand rotations to the kinect plugin, add notes on mapping from kinect orientations to our frame orientations --- plugins/hifiKinect/src/KinectPlugin.cpp | 101 +++++++++++++++++++----- 1 file changed, 82 insertions(+), 19 deletions(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 0bff69ed57..2b9691cfe2 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -397,29 +397,92 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { joints[j].Position.Y, joints[j].Position.Z }; - // Kinect Documentation is unclear on what these orientations are, are they absolute? - // or are the relative to the parent bones. It appears as if it has changed between the - // older 1.x SDK and the 2.0 sdk - // - // https://social.msdn.microsoft.com/Forums/en-US/31c9aff6-7dab-433d-9af9-59942dfd3d69/kinect-v20-preview-sdk-jointorientation-vs-boneorientation?forum=kinectv2sdk - // seems to suggest these are absolute... - // "These quaternions are absolute, so you can take a mesh in local space, transform it by the quaternion, - // and it will match the exact orientation of the bone. If you want relative orientation quaternion, you - // can multiply the absolute quaternion by the inverse of the parent joint's quaternion." - // - // - Bone direction(Y green) - always matches the skeleton. - // - Normal(Z blue) - joint roll, perpendicular to the bone - // - Binormal(X orange) - perpendicular to the bone and normal - - glm::quat jointOrientation { jointOrientations[j].Orientation.x, + // This is the rotation in the kinect camera/sensor frame... we adjust that in update... + // NOTE: glm::quat(W!!!, x, y, z)... not (x,y,z,w)!!! + glm::quat jointOrientation { jointOrientations[j].Orientation.w, + jointOrientations[j].Orientation.x, jointOrientations[j].Orientation.y, - jointOrientations[j].Orientation.z, - jointOrientations[j].Orientation.w }; + jointOrientations[j].Orientation.z }; // filling in the _joints data... if (joints[j].TrackingState != TrackingState_NotTracked) { _joints[j].position = jointPosition; + + // Kinect Documentation... + // + // https://social.msdn.microsoft.com/Forums/en-US/31c9aff6-7dab-433d-9af9-59942dfd3d69/kinect-v20-preview-sdk-jointorientation-vs-boneorientation?forum=kinectv2sdk + // seems to suggest these are absolute... + // "These quaternions are absolute, so you can take a mesh in local space, transform it by the quaternion, + // and it will match the exact orientation of the bone. If you want relative orientation quaternion, you + // can multiply the absolute quaternion by the inverse of the parent joint's quaternion." + // + // This is consistent with our findings, but does not include "enough information" + // - Bone direction(Y green) - always matches the skeleton. + // - Normal(Z blue) - joint roll, perpendicular to the bone + // - Binormal(X orange) - perpendicular to the bone and normal + + // NOTE: Common notation of vectors on paper... + // (+) is the back of the arrow - this vector is pointing into the page + // (o) is the point of the arrow - this vector is pointing out of the page + // + + // From ABOVE the kinect coordinate frame looks like this: + // + // Assuming standing facing the kinect camera + // Right Hand with fingers pointing up (green/y) + // thumb pointing behind body (blue/z) + // palm facing the head (point out back of my hand, red/x) + // + // The identity rotation relative to the cameras frame... (the joint data from SDK) + // + // y | | | | + // | | | | | + // | | | + // z----(o) \ |right| + // x \_ | + // | | + // | | + // + // Expected... identity rotation for left hand..... [to be verified] + // Left Hand with fingers pointing up (green/y) + // thumb pointing forward (blue/z) + // palm facing outward away from head (point out back of my hand, red/x) + // + // Our desired coordinate system... + // "the local coordinate of the palm in our system"... + // + // From ABOVE the hand canonical axes look like this: + // + // + // | | | | y | | | | + // | | | | | | | | | + // | | | | | + // |left | / x----(+) \ |right| + // | _/ z \_ | + // | | | | + // | | | | + // + // Right hand rule... make the hitch hiking sign... + // thumb points in direction of the axis you want to rotate around + // fisted fingers curl in positive rotation direction.... + // + // To transform from Kinect to our RIGHT Hand.... Negative 90 deg around Y + // + // FIXME -- Double check if JointType_HandRight vs JointType_WristRight is actually + // the joint we want to be using!! + // //_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; + } 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; + } else { + _joints[j].orientation = jointOrientation; + } + } } } @@ -541,8 +604,8 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC continue; } - // FIXME - determine the correct orientation transform - glm::quat rot = joints[i].orientation; + // Note: we want our rotations presenting in the AVATAR frame, so we need to adjust that here. + glm::quat rot = controllerToAvatarRotation * joints[i].orientation; if (i < prevJoints.size()) { linearVel = (pos - (prevJoints[i].position * METERS_PER_CENTIMETER)) / deltaTime; // m/s From 80fb7bc28706b38ce8a42597c3b7d82c8fa1f05f Mon Sep 17 00:00:00 2001 From: ZappoMan Date: Tue, 28 Mar 2017 18:42:15 -0700 Subject: [PATCH 2/3] add extra debugging to the kinect plugin --- plugins/hifiKinect/src/KinectPlugin.cpp | 23 +++++++++++++++++++++-- plugins/hifiKinect/src/KinectPlugin.h | 1 + 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 2b9691cfe2..19b5c94297 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -239,6 +239,14 @@ void KinectPlugin::init() { auto preference = new CheckPreference(KINECT_PLUGIN, "Enabled", getter, setter); preferences->addPreference(preference); } + { + auto debugGetter = [this]()->bool { return _enabled; }; + auto debugSetter = [this](bool value) { + _debug = value; saveSettings(); + }; + auto preference = new CheckPreference(KINECT_PLUGIN, "Extra Debugging", debugGetter, debugSetter); + preferences->addPreference(preference); + } } bool KinectPlugin::isSupported() const { @@ -389,9 +397,11 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { if (SUCCEEDED(hr)) { auto jointCount = _countof(joints); - //qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount; + if (_debug) { + qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount; + } + for (int j = 0; j < jointCount; ++j) { - //QString jointName = kinectJointNames[joints[j].JointType]; glm::vec3 jointPosition { joints[j].Position.X, joints[j].Position.Y, @@ -404,6 +414,14 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { jointOrientations[j].Orientation.y, jointOrientations[j].Orientation.z }; + if (_debug) { + QString jointName = kinectJointNames[joints[j].JointType]; + qDebug() << __FUNCTION__ << "joint[" << j << "]:" << jointName + << "position:" << jointPosition + << "orientation:" << jointOrientation + << "isTracked:" << joints[j].TrackingState != TrackingState_NotTracked; + } + // filling in the _joints data... if (joints[j].TrackingState != TrackingState_NotTracked) { _joints[j].position = jointPosition; @@ -545,6 +563,7 @@ void KinectPlugin::saveSettings() const { settings.beginGroup(idString); { settings.setValue(QString("enabled"), _enabled); + settings.setValue(QString("extraDebug"), _debug); } settings.endGroup(); } diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index b10698fa31..90794fa6b0 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -89,6 +89,7 @@ protected: static const char* KINECT_ID_STRING; bool _enabled { false }; + bool _debug { false }; mutable bool _initialized { false }; // copy of data directly from the KinectDataReader SDK From 9d3e8818f78438615a3f1993020c29daaa53f210 Mon Sep 17 00:00:00 2001 From: ZappoMan Date: Tue, 28 Mar 2017 18:49:36 -0700 Subject: [PATCH 3/3] fix build --- plugins/hifiKinect/src/KinectPlugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 19b5c94297..8313ea0d49 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -398,7 +398,7 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { if (SUCCEEDED(hr)) { auto jointCount = _countof(joints); if (_debug) { - qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount; + qDebug() << __FUNCTION__ << "nBodyCount:" << bodyCount << "body:" << i << "jointCount:" << jointCount; } for (int j = 0; j < jointCount; ++j) { @@ -419,7 +419,7 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { qDebug() << __FUNCTION__ << "joint[" << j << "]:" << jointName << "position:" << jointPosition << "orientation:" << jointOrientation - << "isTracked:" << joints[j].TrackingState != TrackingState_NotTracked; + << "isTracked:" << (joints[j].TrackingState != TrackingState_NotTracked); } // filling in the _joints data...