From 182f4e70af265c5b132637f90bd2201f1ac0f73f Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Mon, 1 Jul 2013 15:01:36 -0700 Subject: [PATCH] More work on skeleton tracking. --- cmake/modules/FindNITE.cmake | 44 ----------- interface/CMakeLists.txt | 9 +-- interface/src/Webcam.cpp | 142 +++++++++++++++++++++++++++++++---- interface/src/Webcam.h | 19 ++++- 4 files changed, 148 insertions(+), 66 deletions(-) delete mode 100644 cmake/modules/FindNITE.cmake diff --git a/cmake/modules/FindNITE.cmake b/cmake/modules/FindNITE.cmake deleted file mode 100644 index 301ed651ee..0000000000 --- a/cmake/modules/FindNITE.cmake +++ /dev/null @@ -1,44 +0,0 @@ -# Find the NITE library -# -# You must provide an NITE_ROOT_DIR which contains lib and include directories -# -# Once done this will define -# -# NITE_FOUND - system found NITE -# NITE_INCLUDE_DIRS - the NITE include directory -# NITE_LIBRARIES - Link this to use NITE -# -# Created on 6/28/2013 by Andrzej Kapolka -# Copyright (c) 2013 High Fidelity -# - -if (NITE_LIBRARIES AND NITE_INCLUDE_DIRS) - # in cache already - set(NITE_FOUND TRUE) -else (NITE_LIBRARIES AND NITE_INCLUDE_DIRS) - find_path(NITE_INCLUDE_DIRS XnVNite.h /usr/include/nite) - - if (APPLE) - find_library(NITE_LIBRARIES libXnVNite_1_5_2.dylib /usr/lib) - elseif (UNIX) - find_library(NITE_LIBRARIES libXnVNite_1_5_2.so /usr/lib) - endif () - - if (NITE_INCLUDE_DIRS AND NITE_LIBRARIES) - set(NITE_FOUND TRUE) - endif (NITE_INCLUDE_DIRS AND NITE_LIBRARIES) - - if (NITE_FOUND) - if (NOT NITE_FIND_QUIETLY) - message(STATUS "Found NITE: ${NITE_LIBRARIES}") - endif (NOT NITE_FIND_QUIETLY) - else (NITE_FOUND) - if (NITE_FIND_REQUIRED) - message(FATAL_ERROR "Could not find NITE") - endif (NITE_FIND_REQUIRED) - endif (NITE_FOUND) - - # show the NITE_INCLUDE_DIRS and NITE_LIBRARIES variables only in the advanced view - mark_as_advanced(NITE_INCLUDE_DIRS NITE_LIBRARIES) - -endif (NITE_LIBRARIES AND NITE_INCLUDE_DIRS) diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index 98045fb63c..0b8d28f266 100755 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -96,14 +96,13 @@ find_package(OpenCV) find_package(ZLIB) find_package(UVCCameraControl) find_package(OpenNI) -find_package(NITE) # let the source know that we have OpenNI/NITE for Kinect -if (OPENNI_FOUND AND NITE_FOUND) +if (OPENNI_FOUND) add_definitions(-DHAVE_OPENNI) - include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS} ${NITE_INCLUDE_DIRS}) - target_link_libraries(${TARGET_NAME} ${OPENNI_LIBRARIES} ${NITE_LIBRARIES}) -endif (OPENNI_FOUND AND NITE_FOUND) + include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS}) + target_link_libraries(${TARGET_NAME} ${OPENNI_LIBRARIES}) +endif (OPENNI_FOUND) # include headers for interface and InterfaceConfig. include_directories( diff --git a/interface/src/Webcam.cpp b/interface/src/Webcam.cpp index ac219de06b..9b554b6044 100644 --- a/interface/src/Webcam.cpp +++ b/interface/src/Webcam.cpp @@ -22,7 +22,8 @@ using namespace cv; using namespace std; using namespace xn; -// register OpenCV matrix type with Qt metatype system +// register types with Qt metatype system +int jointVectorMetaType = qRegisterMetaType("JointVector"); int matMetaType = qRegisterMetaType("cv::Mat"); int rotatedRectMetaType = qRegisterMetaType("cv::RotatedRect"); @@ -95,9 +96,28 @@ void Webcam::renderPreview(int screenWidth, int screenHeight) { glTexCoord2f(0, 1); glVertex2f(depthLeft, top); glEnd(); + + glBindTexture(GL_TEXTURE_2D, 0); + glDisable(GL_TEXTURE_2D); + + if (!_joints.isEmpty()) { + glColor3f(1.0f, 0.0f, 0.0f); + glPointSize(4.0f); + glBegin(GL_POINTS); + float projectedScale = PREVIEW_HEIGHT / (float)_depthHeight; + foreach (const Joint& joint, _joints) { + if (joint.isValid) { + glVertex2f(depthLeft + joint.projected.x * projectedScale, + top - PREVIEW_HEIGHT + joint.projected.y * projectedScale); + } + } + glEnd(); + glPointSize(1.0f); + } + } else { + glBindTexture(GL_TEXTURE_2D, 0); + glDisable(GL_TEXTURE_2D); } - glBindTexture(GL_TEXTURE_2D, 0); - glDisable(GL_TEXTURE_2D); glBegin(GL_LINE_LOOP); Point2f facePoints[4]; @@ -124,7 +144,7 @@ Webcam::~Webcam() { delete _grabber; } -void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const RotatedRect& faceRect) { +void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const RotatedRect& faceRect, const JointVector& joints) { IplImage image = frame; glPixelStorei(GL_UNPACK_ROW_LENGTH, image.widthStep / 3); if (_frameTextureID == 0) { @@ -160,8 +180,9 @@ void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const Rota glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); glBindTexture(GL_TEXTURE_2D, 0); - // store our face rect, update our frame count for fps computation + // store our face rect and joints, update our frame count for fps computation _faceRect = faceRect; + _joints = joints; _frameCount++; const int MAX_FPS = 60; @@ -224,12 +245,80 @@ 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_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_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_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; + } +} + +static glm::vec3 xnToGLM(const XnVector3D& vector) { + return glm::vec3(vector.X, vector.Y, vector.Z); +} + +static glm::mat3 xnToGLM(const XnMatrix3X3& matrix) { + return glm::mat3( + matrix.elements[0], matrix.elements[1], matrix.elements[2], + matrix.elements[3], matrix.elements[4], matrix.elements[5], + matrix.elements[6], matrix.elements[7], matrix.elements[8]); +} + +static void XN_CALLBACK_TYPE newUser(UserGenerator& generator, XnUserID id, void* cookie) { + printLog("Found user %d.\n", id); + generator.GetSkeletonCap().RequestCalibration(id, true); +} + +static void XN_CALLBACK_TYPE lostUser(UserGenerator& generator, XnUserID id, void* cookie) { + printLog("Lost user %d.\n", id); +} + +static void XN_CALLBACK_TYPE calibrationStarted(SkeletonCapability& capability, XnUserID id, void* cookie) { + printLog("Calibration started for user %d.\n", id); +} + +static void XN_CALLBACK_TYPE calibrationCompleted(SkeletonCapability& capability, + XnUserID id, XnCalibrationStatus status, void* cookie) { + if (status == XN_CALIBRATION_STATUS_OK) { + printLog("Calibration completed for user %d.\n", id); + capability.StartTracking(id); + + } else { + printLog("Calibration failed to user %d.\n", id); + capability.RequestCalibration(id, true); + } +} +#endif + void FrameGrabber::grabFrame() { if (!(_initialized || init())) { return; } int format = GL_BGR; Mat frame; + JointVector joints; #ifdef HAVE_OPENNI if (_depthGenerator.IsValid()) { @@ -239,6 +328,31 @@ 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; + XnUInt16 userCount = 1; + _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]; + XnUInt16 activeJointCount = MAX_ACTIVE_JOINTS; + _userGenerator.GetSkeletonCap().EnumerateActiveJoints(activeJoints, activeJointCount); + XnSkeletonJointTransformation transform; + for (int i = 0; i < activeJointCount; i++) { + AvatarJointID avatarJoint = xnToAvatarJoint(activeJoints[i]); + if (avatarJoint == AVATAR_JOINT_NULL) { + continue; + } + _userGenerator.GetSkeletonCap().GetSkeletonJoint(userID, activeJoints[i], transform); + XnVector3D projected; + _depthGenerator.ConvertRealWorldToProjective(1, &transform.position.position, &projected); + Joint joint = { true, xnToGLM(transform.position.position), + glm::quat_cast(xnToGLM(transform.orientation.orientation)), + xnToGLM(projected) }; + joints[avatarJoint] = joint; + } + } } #endif @@ -289,18 +403,10 @@ void FrameGrabber::grabFrame() { _searchWindow = faceRect.boundingRect(); } QMetaObject::invokeMethod(Application::getInstance()->getWebcam(), "setFrame", - Q_ARG(cv::Mat, frame), Q_ARG(int, format), Q_ARG(cv::Mat, _grayDepthFrame), Q_ARG(cv::RotatedRect, faceRect)); + Q_ARG(cv::Mat, frame), Q_ARG(int, format), Q_ARG(cv::Mat, _grayDepthFrame), + Q_ARG(cv::RotatedRect, faceRect), Q_ARG(JointVector, joints)); } -#ifdef HAVE_OPENNI -static void XN_CALLBACK_TYPE newUser(UserGenerator& generator, XnUserID id, void* cookie) { - printLog("Found user %d.\n", id); -} -static void XN_CALLBACK_TYPE lostUser(UserGenerator& generator, XnUserID id, void* cookie) { - printLog("Lost user %d.\n", id); -} -#endif - bool FrameGrabber::init() { _initialized = true; @@ -321,8 +427,12 @@ bool FrameGrabber::init() { _imageGenerator.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); _imageGenerator.GetMetaData(_imageMetaData); - XnCallbackHandle userCallbacks; + XnCallbackHandle userCallbacks, calibrationStartCallback, calibrationCompleteCallback; _userGenerator.RegisterUserCallbacks(newUser, lostUser, 0, userCallbacks); + _userGenerator.GetSkeletonCap().RegisterToCalibrationStart(calibrationStarted, 0, calibrationStartCallback); + _userGenerator.GetSkeletonCap().RegisterToCalibrationComplete(calibrationCompleted, 0, calibrationCompleteCallback); + + _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_UPPER); _xnContext.StartGeneratingAll(); return true; diff --git a/interface/src/Webcam.h b/interface/src/Webcam.h index 89ece788c7..99ec424853 100644 --- a/interface/src/Webcam.h +++ b/interface/src/Webcam.h @@ -12,8 +12,10 @@ #include #include #include +#include #include +#include #include @@ -28,6 +30,9 @@ class QImage; struct CvCapture; class FrameGrabber; +class Joint; + +typedef QVector JointVector; class Webcam : public QObject { Q_OBJECT @@ -47,7 +52,8 @@ public: public slots: void setEnabled(bool enabled); - void setFrame(const cv::Mat& video, int format, const cv::Mat& depth, const cv::RotatedRect& faceRect); + void setFrame(const cv::Mat& video, int format, const cv::Mat& depth, + const cv::RotatedRect& faceRect, const JointVector& joints); private: @@ -64,6 +70,7 @@ private: GLuint _depthTextureID; cv::RotatedRect _faceRect; cv::RotatedRect _initialFaceRect; + JointVector _joints; long long _startTimestamp; int _frameCount; @@ -112,6 +119,16 @@ private: #endif }; +class Joint { +public: + + bool isValid; + glm::vec3 position; + glm::quat orientation; + glm::vec3 projected; +}; + +Q_DECLARE_METATYPE(JointVector) Q_DECLARE_METATYPE(cv::Mat) Q_DECLARE_METATYPE(cv::RotatedRect)