From 52e7ff9a68439a4ccc2ae9b64b7c61d0c2535a62 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Tue, 2 Jul 2013 14:51:27 -0700 Subject: [PATCH] Working on filling in missing joint data. --- interface/src/Application.cpp | 2 +- interface/src/Avatar.cpp | 74 ++++++++++++++++++++++++++++-- interface/src/Webcam.cpp | 65 ++++++++++++++------------ interface/src/Webcam.h | 2 + libraries/avatars/src/AvatarData.h | 13 ++++++ 5 files changed, 122 insertions(+), 34 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index cf3ef70090..2deec90108 100755 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -362,7 +362,7 @@ void Application::paintGL() { glEnable(GL_LINE_SMOOTH); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - float headCameraScale = _serialHeadSensor.isActive() ? _headCameraPitchYawScale : 1.0f; + float headCameraScale = (_serialHeadSensor.isActive() || _webcam.isActive()) ? _headCameraPitchYawScale : 1.0f; if (_myCamera.getMode() == CAMERA_MODE_MIRROR) { _myCamera.setTightness (100.0f); diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 52f3e98a70..230c1f3ec4 100755 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -284,11 +284,11 @@ void Avatar::reset() { _hand.reset(); } -// Update avatar head rotation with sensor data +// Update avatar state with sensor data void Avatar::updateFromGyrosAndOrWebcam() { - const float AMPLIFY_PITCH = 2.f; - const float AMPLIFY_YAW = 2.f; - const float AMPLIFY_ROLL = 2.f; + const float AMPLIFY_PITCH = 1.f; + const float AMPLIFY_YAW = 1.f; + const float AMPLIFY_ROLL = 1.f; SerialInterface* gyros = Application::getInstance()->getSerialHeadSensor(); Webcam* webcam = Application::getInstance()->getWebcam(); @@ -301,6 +301,15 @@ void Avatar::updateFromGyrosAndOrWebcam() { estimatedPosition = webcam->getEstimatedPosition(); estimatedRotation = webcam->getEstimatedRotation(); + // compute and store the joint rotations + const JointVector& joints = webcam->getEstimatedJoints(); + _joints.clear(); + for (int i = 0; i < NUM_AVATAR_JOINTS; i++) { + if (joints[i].isValid) { + JointData data = { i, joints[i].position, joints[i].orientation }; + _joints.push_back(data); + } + } } else { return; } @@ -487,6 +496,63 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { // update avatar skeleton _skeleton.update(deltaTime, getOrientation(), _position); + + // apply joint data (if any) to skeleton + for (vector::iterator it = _joints.begin(); it != _joints.end(); it++) { + _skeleton.joint[it->jointID].absoluteRotation = orientation * it->rotation; + _skeleton.joint[it->jointID].position = _position + orientation * it->position; + + AvatarJointID derivedJointID = AVATAR_JOINT_NULL; + AvatarJointID secondJointID = (AvatarJointID)it->jointID; + float proportion = 0.5f; + switch (it->jointID) { + case AVATAR_JOINT_NECK_BASE: + secondJointID = AVATAR_JOINT_TORSO; + derivedJointID = AVATAR_JOINT_CHEST; + break; + + case AVATAR_JOINT_HEAD_BASE: + derivedJointID = AVATAR_JOINT_HEAD_TOP; + break; + + case AVATAR_JOINT_LEFT_SHOULDER: + secondJointID = AVATAR_JOINT_CHEST; + derivedJointID = AVATAR_JOINT_LEFT_COLLAR; + break; + + case AVATAR_JOINT_LEFT_WRIST: + derivedJointID = AVATAR_JOINT_LEFT_FINGERTIPS; + break; + + case AVATAR_JOINT_RIGHT_SHOULDER: + secondJointID = AVATAR_JOINT_CHEST; + derivedJointID = AVATAR_JOINT_RIGHT_COLLAR; + break; + + case AVATAR_JOINT_RIGHT_WRIST: + derivedJointID = AVATAR_JOINT_RIGHT_FINGERTIPS; + break; + + case AVATAR_JOINT_LEFT_HEEL: + derivedJointID = AVATAR_JOINT_LEFT_TOES; + break; + + case AVATAR_JOINT_RIGHT_HIP: + secondJointID = AVATAR_JOINT_LEFT_HIP; + derivedJointID = AVATAR_JOINT_PELVIS; + break; + + case AVATAR_JOINT_RIGHT_HEEL: + derivedJointID = AVATAR_JOINT_RIGHT_TOES; + break; + } + if (derivedJointID != AVATAR_JOINT_NULL) { + _skeleton.joint[derivedJointID].position = glm::mix(_skeleton.joint[it->jointID].position, + _skeleton.joint[secondJointID].position, proportion); + _skeleton.joint[derivedJointID].absoluteRotation = safeMix(_skeleton.joint[it->jointID].absoluteRotation, + _skeleton.joint[secondJointID].absoluteRotation, proportion); + } + } //determine the lengths of the body springs now that we have updated the skeleton at least once if (!_ballSpringsInitialized) { diff --git a/interface/src/Webcam.cpp b/interface/src/Webcam.cpp index 4788472555..885ffa0514 100644 --- a/interface/src/Webcam.cpp +++ b/interface/src/Webcam.cpp @@ -122,6 +122,7 @@ void Webcam::renderPreview(int screenWidth, int screenHeight) { glDisable(GL_TEXTURE_2D); } + glColor3f(1.0f, 1.0f, 1.0f); glBegin(GL_LINE_LOOP); Point2f facePoints[4]; _faceRect.points(facePoints); @@ -203,9 +204,12 @@ void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const Rota if (!_joints.isEmpty()) { _estimatedJoints.resize(NUM_AVATAR_JOINTS); for (int i = 0; i < NUM_AVATAR_JOINTS; i++) { + if (!_joints[i].isValid) { + continue; + } const float JOINT_SMOOTHING = 0.95f; - _estimatedJoints[i].position = glm::mix(_joints[i].position - _joints[AVATAR_JOINT_TORSO].position, - _estimatedJoints[i].position, JOINT_SMOOTHING); + _estimatedJoints[i].isValid = true; + _estimatedJoints[i].position = glm::mix(_joints[i].position, _estimatedJoints[i].position, JOINT_SMOOTHING); _estimatedJoints[i].orientation = safeMix(_joints[i].orientation, _estimatedJoints[i].orientation, JOINT_SMOOTHING); } _estimatedRotation = safeEulerAngles(_estimatedJoints[AVATAR_JOINT_HEAD_BASE].orientation); @@ -258,43 +262,35 @@ FrameGrabber::~FrameGrabber() { } } -void FrameGrabber::reset() { - _searchWindow = cv::Rect(0, 0, 0, 0); -} - #ifdef HAVE_OPENNI static AvatarJointID xnToAvatarJoint(XnSkeletonJoint joint) { switch (joint) { case XN_SKEL_HEAD: return AVATAR_JOINT_HEAD_BASE; case XN_SKEL_NECK: return AVATAR_JOINT_NECK_BASE; case XN_SKEL_TORSO: return AVATAR_JOINT_TORSO; - case XN_SKEL_WAIST: return AVATAR_JOINT_PELVIS; - case XN_SKEL_LEFT_COLLAR: return AVATAR_JOINT_LEFT_COLLAR; + case XN_SKEL_LEFT_SHOULDER: return AVATAR_JOINT_LEFT_SHOULDER; case XN_SKEL_LEFT_ELBOW: return AVATAR_JOINT_LEFT_ELBOW; - case XN_SKEL_LEFT_WRIST: return AVATAR_JOINT_LEFT_WRIST; - case XN_SKEL_LEFT_HAND: return AVATAR_JOINT_NULL; - case XN_SKEL_LEFT_FINGERTIP: return AVATAR_JOINT_LEFT_FINGERTIPS; - case XN_SKEL_RIGHT_COLLAR: return AVATAR_JOINT_RIGHT_COLLAR; + case XN_SKEL_LEFT_HAND: return AVATAR_JOINT_LEFT_WRIST; + case XN_SKEL_RIGHT_SHOULDER: return AVATAR_JOINT_RIGHT_SHOULDER; case XN_SKEL_RIGHT_ELBOW: return AVATAR_JOINT_RIGHT_ELBOW; - case XN_SKEL_RIGHT_WRIST: return AVATAR_JOINT_RIGHT_WRIST; - case XN_SKEL_RIGHT_HAND: return AVATAR_JOINT_NULL; - case XN_SKEL_RIGHT_FINGERTIP: return AVATAR_JOINT_RIGHT_FINGERTIPS; + case XN_SKEL_RIGHT_HAND: return AVATAR_JOINT_RIGHT_WRIST; + case XN_SKEL_LEFT_HIP: return AVATAR_JOINT_LEFT_HIP; case XN_SKEL_LEFT_KNEE: return AVATAR_JOINT_LEFT_KNEE; - case XN_SKEL_LEFT_ANKLE: return AVATAR_JOINT_LEFT_HEEL; - case XN_SKEL_LEFT_FOOT: return AVATAR_JOINT_LEFT_TOES; + case XN_SKEL_LEFT_FOOT: return AVATAR_JOINT_LEFT_HEEL; + case XN_SKEL_RIGHT_HIP: return AVATAR_JOINT_RIGHT_HIP; case XN_SKEL_RIGHT_KNEE: return AVATAR_JOINT_RIGHT_KNEE; - case XN_SKEL_RIGHT_ANKLE: return AVATAR_JOINT_RIGHT_HEEL; - case XN_SKEL_RIGHT_FOOT: return AVATAR_JOINT_RIGHT_TOES; + case XN_SKEL_RIGHT_FOOT: return AVATAR_JOINT_RIGHT_HEEL; + + default: return AVATAR_JOINT_NULL; } } static glm::vec3 xnToGLM(const XnVector3D& vector) { - const float METERS_PER_MM = 1.0f / 1000.0f; - return glm::vec3(vector.X, vector.Y, -vector.Z) * METERS_PER_MM; + return glm::vec3(vector.X, vector.Y, vector.Z); } static glm::quat xnToGLM(const XnMatrix3X3& matrix) { @@ -302,12 +298,12 @@ static glm::quat xnToGLM(const XnMatrix3X3& matrix) { matrix.elements[0], matrix.elements[3], matrix.elements[6], matrix.elements[1], matrix.elements[4], matrix.elements[7], matrix.elements[2], matrix.elements[5], matrix.elements[8])); - return glm::quat(rotation.w, rotation.x, rotation.y, -rotation.z); + return glm::quat(rotation.w, rotation.x, rotation.y, rotation.z); } static void XN_CALLBACK_TYPE newUser(UserGenerator& generator, XnUserID id, void* cookie) { printLog("Found user %d.\n", id); - generator.GetSkeletonCap().RequestCalibration(id, true); + generator.GetSkeletonCap().RequestCalibration(id, false); } static void XN_CALLBACK_TYPE lostUser(UserGenerator& generator, XnUserID id, void* cookie) { @@ -331,6 +327,16 @@ static void XN_CALLBACK_TYPE calibrationCompleted(SkeletonCapability& capability } #endif +void FrameGrabber::reset() { + _searchWindow = cv::Rect(0, 0, 0, 0); + +#ifdef HAVE_OPENNI + if (_userGenerator.IsValid() && _userGenerator.GetSkeletonCap().IsTracking(_userID)) { + _userGenerator.GetSkeletonCap().RequestCalibration(_userID, true); + } +#endif +} + void FrameGrabber::grabFrame() { if (!(_initialized || init())) { return; @@ -348,10 +354,10 @@ void FrameGrabber::grabFrame() { Mat depth = Mat(_depthMetaData.YRes(), _depthMetaData.XRes(), CV_16UC1, (void*)_depthGenerator.GetDepthMap()); depth.convertTo(_grayDepthFrame, CV_8UC1, 256.0 / 2048.0); - XnUserID userID; + _userID = 0; XnUInt16 userCount = 1; - _userGenerator.GetUsers(&userID, userCount); - if (userCount > 0 && _userGenerator.GetSkeletonCap().IsTracking(userID)) { + _userGenerator.GetUsers(&_userID, userCount); + if (userCount > 0 && _userGenerator.GetSkeletonCap().IsTracking(_userID)) { joints.resize(NUM_AVATAR_JOINTS); const int MAX_ACTIVE_JOINTS = 16; XnSkeletonJoint activeJoints[MAX_ACTIVE_JOINTS]; @@ -363,10 +369,11 @@ void FrameGrabber::grabFrame() { if (avatarJoint == AVATAR_JOINT_NULL) { continue; } - _userGenerator.GetSkeletonCap().GetSkeletonJoint(userID, activeJoints[i], transform); + _userGenerator.GetSkeletonCap().GetSkeletonJoint(_userID, activeJoints[i], transform); XnVector3D projected; _depthGenerator.ConvertRealWorldToProjective(1, &transform.position.position, &projected); - Joint joint = { true, xnToGLM(transform.position.position), + const float METERS_PER_MM = 1.0f / 1000.0f; + Joint joint = { true, xnToGLM(transform.position.position) * METERS_PER_MM, xnToGLM(transform.orientation.orientation), xnToGLM(projected) }; joints[avatarJoint] = joint; @@ -451,7 +458,7 @@ bool FrameGrabber::init() { _userGenerator.GetSkeletonCap().RegisterToCalibrationStart(calibrationStarted, 0, calibrationStartCallback); _userGenerator.GetSkeletonCap().RegisterToCalibrationComplete(calibrationCompleted, 0, calibrationCompleteCallback); - _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_UPPER); + _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); _xnContext.StartGeneratingAll(); return true; diff --git a/interface/src/Webcam.h b/interface/src/Webcam.h index b5d52dc3a2..424728faa3 100644 --- a/interface/src/Webcam.h +++ b/interface/src/Webcam.h @@ -45,6 +45,7 @@ public: const bool isActive() const { return _active; } const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; } const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; } + const JointVector& getEstimatedJoints() const { return _estimatedJoints; } void reset(); void renderPreview(int screenWidth, int screenHeight); @@ -117,6 +118,7 @@ private: xn::UserGenerator _userGenerator; xn::DepthMetaData _depthMetaData; xn::ImageMetaData _imageMetaData; + XnUserID _userID; #endif }; diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index d3e6a40a7e..90d437f253 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -11,6 +11,7 @@ #include #include +#include #include #include @@ -36,6 +37,8 @@ enum KeyState DELETE_KEY_DOWN }; +class JointData; + class AvatarData : public AgentData { public: AvatarData(Agent* owningAgent = NULL); @@ -132,14 +135,24 @@ protected: bool _wantDelta; bool _wantOcclusionCulling; + std::vector _joints; + HeadData* _headData; HandData* _handData; + private: // privatize the copy constructor and assignment operator so they cannot be called AvatarData(const AvatarData&); AvatarData& operator= (const AvatarData&); }; +class JointData { +public: + + int jointID; + glm::vec3 position; + glm::quat rotation; +}; // These pack/unpack functions are designed to start specific known types in as efficient a manner // as possible. Taking advantage of the known characteristics of the semantic types.