From 3f26e1b63ad18ab228b2138e6ab9f4d3f068909f Mon Sep 17 00:00:00 2001 From: Thijs Wenker Date: Sat, 13 Dec 2014 23:04:19 +0100 Subject: [PATCH] RealSense Initial --- cmake/modules/FindRSSDK.cmake | 33 ++++ interface/CMakeLists.txt | 2 +- interface/src/Application.cpp | 2 + interface/src/Menu.cpp | 10 ++ interface/src/Menu.h | 2 + interface/src/devices/RealSense.cpp | 233 ++++++++++++++++++++++++++++ interface/src/devices/RealSense.h | 63 ++++++++ 7 files changed, 344 insertions(+), 1 deletion(-) create mode 100644 cmake/modules/FindRSSDK.cmake create mode 100644 interface/src/devices/RealSense.cpp create mode 100644 interface/src/devices/RealSense.h diff --git a/cmake/modules/FindRSSDK.cmake b/cmake/modules/FindRSSDK.cmake new file mode 100644 index 0000000000..c31b0efcd9 --- /dev/null +++ b/cmake/modules/FindRSSDK.cmake @@ -0,0 +1,33 @@ +# Try to find the RSSDK library +# +# You must provide a RSSDK_ROOT_DIR which contains lib and include directories +# +# Once done this will define +# +# RSSDK_FOUND - system found RSSDK +# RSSDK_INCLUDE_DIRS - the RSSDK include directory +# RSSDK_LIBRARIES - Link this to use RSSDK +# +# Created on 12/7/2014 by Thijs Wenker +# Copyright (c) 2014 High Fidelity +# + +include("${MACRO_DIR}/HifiLibrarySearchHints.cmake") +hifi_library_search_hints("rssdk") + +find_path(RSSDK_INCLUDE_DIRS pxcbase.h PATH_SUFFIXES include HINTS ${RSSDK_SEARCH_DIRS}) + +if (WIN32) + find_library(RSSDK_LIBRARY_DEBUG libpxc_d PATH_SUFFIXES lib/Win32 HINTS ${RSSDK_SEARCH_DIRS}) + find_library(RSSDK_LIBRARY_RELEASE libpxc PATH_SUFFIXES lib/Win32 HINTS ${RSSDK_SEARCH_DIRS}) +endif () + +include(SelectLibraryConfigurations) +select_library_configurations(RSSDK) + +set(RSSDK_LIBRARIES "${RSSDK_LIBRARY}") + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(RSSDK DEFAULT_MSG RSSDK_INCLUDE_DIRS RSSDK_LIBRARIES) + +mark_as_advanced(RSSDK_INCLUDE_DIRS RSSDK_LIBRARIES RSSDK_SEARCH_DIRS) diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index 38dd02c655..bc25778940 100644 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -2,7 +2,7 @@ set(TARGET_NAME interface) project(${TARGET_NAME}) # set a default root dir for each of our optional externals if it was not passed -set(OPTIONAL_EXTERNALS "Faceshift" "LibOVR" "PrioVR" "Sixense" "Visage" "LeapMotion" "RtMidi" "Qxmpp" "SDL2" "Gverb") +set(OPTIONAL_EXTERNALS "Faceshift" "LibOVR" "PrioVR" "Sixense" "Visage" "LeapMotion" "RtMidi" "Qxmpp" "SDL2" "Gverb" "RSSDK") foreach(EXTERNAL ${OPTIONAL_EXTERNALS}) string(TOUPPER ${EXTERNAL} ${EXTERNAL}_UPPERCASE) if (NOT ${${EXTERNAL}_UPPERCASE}_ROOT_DIR) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 3824185f52..08e3a78653 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -76,6 +76,7 @@ #include "Util.h" #include "devices/Leapmotion.h" +#include "devices/RealSense.h" #include "devices/MIDIManager.h" #include "devices/OculusManager.h" #include "devices/TV3DManager.h" @@ -1994,6 +1995,7 @@ void Application::init() { _visage.init(); Leapmotion::init(); + RealSense::init(); // fire off an immediate domain-server check in now that settings are loaded NodeList::getInstance()->sendDomainServerCheckIn(); diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index ae12b5ff26..37e4d96ed6 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -49,6 +49,7 @@ #include "ui/ModelsBrowser.h" #include "ui/LoginDialog.h" #include "ui/NodeBounds.h" +#include "devices/RealSense.h" #include "devices/OculusManager.h" @@ -493,6 +494,11 @@ Menu::Menu() : QMenu* leapOptionsMenu = handOptionsMenu->addMenu("Leap Motion"); addCheckableActionToQMenuAndActionHash(leapOptionsMenu, MenuOption::LeapMotionOnHMD, 0, false); +#ifdef HAVE_RSSDK + QMenu* realSenseOptionsMenu = handOptionsMenu->addMenu("RealSense"); + addActionToQMenuAndActionHash(realSenseOptionsMenu, MenuOption::LoadRSSDKFile, 0, this, SLOT(loadRSSDKFile())); +#endif + QMenu* networkMenu = developerMenu->addMenu("Network"); addCheckableActionToQMenuAndActionHash(networkMenu, MenuOption::DisableNackPackets, 0, false); addCheckableActionToQMenuAndActionHash(networkMenu, @@ -1346,6 +1352,10 @@ void Menu::showChat() { } } +void Menu::loadRSSDKFile() { + RealSense::getInstance()->loadRSSDKFile(); +} + void Menu::toggleChat() { #ifdef HAVE_QXMPP _chatAction->setEnabled(XmppClient::getInstance().getXMPPClient().isConnected()); diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 138828d3e8..4ed5be8057 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -236,6 +236,7 @@ private slots: void displayAddressNotFoundMessage(); void muteEnvironment(); void changeVSync(); + void loadRSSDKFile(); private: static Menu* _instance; @@ -414,6 +415,7 @@ namespace MenuOption { const QString LeapMotionOnHMD = "Leap Motion on HMD"; const QString LoadScript = "Open and Run Script File..."; const QString LoadScriptURL = "Open and Run Script from URL..."; + const QString LoadRSSDKFile = "Load .rssdk file"; const QString LodTools = "LOD Tools"; const QString Login = "Login"; const QString Log = "Log"; diff --git a/interface/src/devices/RealSense.cpp b/interface/src/devices/RealSense.cpp new file mode 100644 index 0000000000..08492d8377 --- /dev/null +++ b/interface/src/devices/RealSense.cpp @@ -0,0 +1,233 @@ +// +// RealSense.cpp +// interface/src/devices +// +// Created by Thijs Wenker on 12/10/14 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// +#include "RealSense.h" +#include "Menu.h" +#include "SharedUtil.h" + +const int PALMROOT_NUM_JOINTS = 2; +const int FINGER_NUM_JOINTS = 4; + +const DeviceTracker::Name RealSense::NAME = "RealSense"; + +// find the index of a joint from +// the side: true = right +// the finger & the bone: +// finger in [0..4] : bone in [0..3] a finger phalange +// [-1] up the hand branch : bone in [0..2] <=> [ hand, forearm, arm] +MotionTracker::Index evalRealSenseJointIndex(bool isRightSide, int finger, int bone) { + + MotionTracker::Index offset = 1 // start after root + + (int(isRightSide) * PXCHandData::NUMBER_OF_JOINTS) // then offset for side + + PALMROOT_NUM_JOINTS; // then add the arm/forearm/hand chain + if (finger >= 0) { + // from there go down in the correct finger and bone + return offset + (finger * FINGER_NUM_JOINTS) + bone; + } else { + // or go back up for the correct root bone + return offset - 1 - bone; + } +} + +// static +void RealSense::init() { + DeviceTracker* device = DeviceTracker::getDevice(NAME); + if (!device) { + // create a new RealSense and register it + RealSense* realSense = new RealSense(); + DeviceTracker::registerDevice(NAME, realSense); + } +} + +// static +RealSense* RealSense::getInstance() { + DeviceTracker* device = DeviceTracker::getDevice(NAME); + if (!device) { + // create a new RealSense and register it + RealSense* realSense = new RealSense(); + DeviceTracker::registerDevice(NAME, realSense); + } + return dynamic_cast< RealSense* > (device); +} + +RealSense::RealSense() : + MotionTracker(), + _active(false), + _handData(NULL) +{ + _session = PXCSession_Create(); + initSession(false, NULL); + + // Create the RealSense joint hierarchy + std::vector< Semantic > sides; + sides.push_back("joint_L_"); + sides.push_back("joint_R_"); + + std::vector< Semantic > rootBones; + rootBones.push_back("wrist"); + rootBones.push_back("hand"); + + std::vector< Semantic > fingers; + fingers.push_back("thumb"); + fingers.push_back("index"); + fingers.push_back("middle"); + fingers.push_back("ring"); + fingers.push_back("pinky"); + + std::vector< Semantic > fingerBones; + fingerBones.push_back("1"); + fingerBones.push_back("2"); + fingerBones.push_back("3"); + fingerBones.push_back("4"); + + std::vector< Index > palms; + for (unsigned int s = 0; s < sides.size(); s++) { + Index rootJoint = 0; + for (unsigned int rb = 0; rb < rootBones.size(); rb++) { + rootJoint = addJoint(sides[s] + rootBones[rb], rootJoint); + } + + // capture the hand index for debug + palms.push_back(rootJoint); + + for (unsigned int f = 0; f < fingers.size(); f++) { + for (unsigned int b = 0; b < fingerBones.size(); b++) { + rootJoint = addJoint(sides[s] + fingers[f] + fingerBones[b], rootJoint); + } + } + } +} + +RealSense::~RealSense() { + _manager->Release(); +} + +void RealSense::initSession(bool playback, QString filename) { + _active = false; + _properlyInitialized = false; + if (_handData != NULL) { + _handData->Release(); + _handController->Release(); + _session->Release(); + } + _manager = _session->CreateSenseManager(); + if (playback) { + _manager->QueryCaptureManager()->SetFileName(filename.toStdWString().c_str(), false); + } + _manager->QueryCaptureManager()->SetRealtime(!playback); + _manager->EnableHand(0); + _handController = _manager->QueryHand(); + + if (_manager->Init() == PXC_STATUS_NO_ERROR) { + _handData = _handController->CreateOutput(); + + PXCCapture::Device *device = _manager->QueryCaptureManager()->QueryDevice(); + PXCCapture::DeviceInfo dinfo; + _manager->QueryCaptureManager()->QueryDevice()->QueryDeviceInfo(&dinfo); + if (dinfo.model == PXCCapture::DEVICE_MODEL_IVCAM) + { + device->SetDepthConfidenceThreshold(1); + device->SetMirrorMode(PXCCapture::Device::MIRROR_MODE_DISABLED); + device->SetIVCAMFilterOption(6); + } + _properlyInitialized = true; + } +} + +#ifdef HAVE_RSSDK +glm::quat quatFromPXCPoint4DF32(const PXCPoint4DF32& basis) { + return glm::quat(basis.w, basis.x, basis.y, basis.z); +} + +glm::vec3 vec3FromPXCPoint3DF32(const PXCPoint3DF32& vec) { + return glm::vec3(vec.x, vec.y, vec.z); +} +#endif + +void RealSense::update() { +#ifdef HAVE_RSSDK + bool wasActive = _active; + _active = _manager->IsConnected() && _properlyInitialized; + if (_active || wasActive) { + // Go through all the joints and increment their counter since last update. + // Increment all counters once after controller first becomes inactive so that each joint reports itself as inactive. + // TODO C++11 for (auto jointIt = _jointsArray.begin(); jointIt != _jointsArray.end(); jointIt++) { + for (JointTracker::Vector::iterator jointIt = _jointsArray.begin(); jointIt != _jointsArray.end(); jointIt++) { + (*jointIt).tickNewFrame(); + } + } + + if (!_active) { + return; + } + + pxcStatus sts = _manager->AcquireFrame(true); + _handData->Update(); + PXCHandData::JointData nodes[2][PXCHandData::NUMBER_OF_JOINTS] = {}; + PXCHandData::ExtremityData extremitiesPointsNodes[2][PXCHandData::NUMBER_OF_EXTREMITIES] = {}; + for (pxcI32 i = 0; i < _handData->QueryNumberOfHands(); i++) { + PXCHandData::IHand* handData; + if (_handData->QueryHandData(PXCHandData::ACCESS_ORDER_BY_TIME, i, handData) == PXC_STATUS_NO_ERROR) { + int rightSide = handData->QueryBodySide() == PXCHandData::BODY_SIDE_RIGHT; + PXCHandData::JointData jointData; + JointTracker* parentJointTracker = _jointsArray.data(); + //Iterate Joints + int rootBranchIndex = -1; + JointTracker* palmJoint = NULL; + for (int j = 0; j < PXCHandData::NUMBER_OF_JOINTS; j++) { + handData->QueryTrackedJoint((PXCHandData::JointType)j, jointData); + nodes[i][j] = jointData; + if (j == PXCHandData::JOINT_WRIST) { + JointTracker* wrist = editJointTracker(evalRealSenseJointIndex(rightSide, rootBranchIndex, 1)); // 1 is the index of the wrist joint + wrist->editAbsFrame().setTranslation(vec3FromPXCPoint3DF32(jointData.positionWorld)); + wrist->editAbsFrame().setRotation(quatFromPXCPoint4DF32(jointData.globalOrientation)); + wrist->updateLocFromAbsTransform(parentJointTracker); + wrist->activeFrame(); + parentJointTracker = wrist; + continue; + } else if (j == PXCHandData::JOINT_CENTER) { + palmJoint = editJointTracker(evalRealSenseJointIndex(rightSide, rootBranchIndex, 0)); // 0 is the index of the palm joint + palmJoint->editAbsFrame().setTranslation(vec3FromPXCPoint3DF32(jointData.positionWorld)); + palmJoint->editAbsFrame().setRotation(quatFromPXCPoint4DF32(jointData.globalOrientation)); + palmJoint->updateLocFromAbsTransform(parentJointTracker); + palmJoint->activeFrame(); + parentJointTracker = palmJoint; + continue; + } + int finger_index = j - PALMROOT_NUM_JOINTS; + int finger = finger_index / FINGER_NUM_JOINTS; + int finger_bone = finger_index % FINGER_NUM_JOINTS; + JointTracker* ljointTracker = editJointTracker(evalRealSenseJointIndex(rightSide, finger, finger_bone)); + if (jointData.confidence > 0) { + ljointTracker->editAbsFrame().setTranslation(vec3FromPXCPoint3DF32(jointData.positionWorld)); + ljointTracker->editAbsFrame().setRotation(quatFromPXCPoint4DF32(jointData.globalOrientation)); + ljointTracker->updateLocFromAbsTransform(parentJointTracker); + ljointTracker->activeFrame(); + } + if (finger_bone == (FINGER_NUM_JOINTS - 1)) { + parentJointTracker = palmJoint; + continue; + } + parentJointTracker = ljointTracker; + } + } + } + _manager->ReleaseFrame(); +#endif // HAVE_RSSDK +} + +void RealSense::loadRSSDKFile() { + QString fileNameString = QFileDialog::getOpenFileName(Application::getInstance()->getGLWidget(), tr("Open RSSDK clip"), + NULL, + tr("RSSDK Recordings (*.rssdk)")); + if (!fileNameString.isEmpty()) { + initSession(true, fileNameString); + } +} diff --git a/interface/src/devices/RealSense.h b/interface/src/devices/RealSense.h new file mode 100644 index 0000000000..015f1e7ce6 --- /dev/null +++ b/interface/src/devices/RealSense.h @@ -0,0 +1,63 @@ +// +// RealSense.h +// interface/src/devices +// +// Created by Thijs Wenker on 12/10/14 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_RealSense_h +#define hifi_RealSense_h + +#include + +#include "MotionTracker.h" + +#ifdef HAVE_RSSDK +#include +#include +#include +#include +#include +#endif + +/// Handles interaction with the RealSense skeleton tracking suit. +class RealSense : public QObject, public MotionTracker { + Q_OBJECT + +public: + static const Name NAME; + + static void init(); + + /// RealSense MotionTracker factory + static RealSense* getInstance(); + + bool isActive() const { return _active; } + + virtual void update(); + + void loadRSSDKFile(); + +protected: + RealSense(); + virtual ~RealSense(); + + void initSession(bool playback, QString filename); + +private: +#ifdef HAVE_RSSDK + PXCSession* _session; + PXCSenseManager* _manager; + PXCHandModule* _handController; + PXCHandData* _handData; + PXCHandConfiguration* _config; +#endif + bool _properlyInitialized; + bool _active; +}; + +#endif // hifi_RealSense_h