diff --git a/KinectV1Device.cpp b/KinectV1Device.cpp index 4990f44..3a180ae 100644 --- a/KinectV1Device.cpp +++ b/KinectV1Device.cpp @@ -10,7 +10,6 @@ namespace KinectOsvr { std::map windowMap; - typedef HRESULT(_stdcall *NuiGetSensorCountType)(int*); typedef HRESULT(_stdcall *NuiCreateSensorByIndexType)(int, INuiSensor**); typedef HRESULT(_stdcall *NuiSkeletonCalculateBoneOrientationsType)(NUI_SKELETON_DATA*, NUI_SKELETON_BONE_ORIENTATION*); @@ -98,6 +97,11 @@ namespace KinectOsvr { m_trackedBodyChanged = true; } + void KinectV1Device::recenter() + { + m_firstUpdate = true; + } + void KinectV1Device::ui_thread(ui_thread_data& data) { MSG msg; @@ -129,15 +133,18 @@ namespace KinectOsvr { } bool redraw = false; + bool foundBody = false; int bodies = 0; for (int i = 0; i < NUI_SKELETON_COUNT; i++) { + if (bodyStates[i] == ShouldBeTracked) { + foundBody = true; + } if (bodyStates[i] != CannotBeTracked) { bodies++; } if (bodyStates[i] != previousStates[i]) { redraw = true; - SendDlgItemMessage(hDlg, IDC_RADIO1 + i, WM_ENABLE, true, 0); EnableWindow(GetDlgItem(hDlg, IDC_RADIO1 + i), bodyStates[i] != CannotBeTracked); if (bodyStates[i] == ShouldBeTracked) { CheckRadioButton(hDlg, IDC_RADIO1, IDC_RADIO6, IDC_RADIO1 + i); @@ -146,6 +153,9 @@ namespace KinectOsvr { } } if (redraw) { + if (!foundBody) { + CheckRadioButton(hDlg, IDC_RADIO1, IDC_RADIO6, 0); + } if (bodies == 1) { SetDlgItemText(hDlg, IDC_STATIC1, "1 body detected."); } @@ -169,6 +179,9 @@ namespace KinectOsvr { case WM_COMMAND: switch (LOWORD(wParam)) { + case IDC_BUTTON1: + windowMap[hDlg]->recenter(); + break; case IDC_CHECK1: if (BN_CLICKED == HIWORD(wParam)) { windowMap[hDlg]->toggleSeatedMode(); @@ -473,12 +486,17 @@ namespace KinectOsvr { }; KinectV1Device::~KinectV1Device() { - if (m_pNuiSensor) { m_pNuiSensor->NuiShutdown(); + m_pNuiSensor->Release(); + m_pNuiSensor = NULL; + } + + if (m_hNextSkeletonEvent && (m_hNextSkeletonEvent != INVALID_HANDLE_VALUE)) + { + CloseHandle(m_hNextSkeletonEvent); } - SafeRelease(m_pNuiSensor); }; }; diff --git a/KinectV1Device.h b/KinectV1Device.h index 25ed51f..a3836b3 100644 --- a/KinectV1Device.h +++ b/KinectV1Device.h @@ -1,4 +1,5 @@ #include "stdafx.h" +#include namespace KinectOsvr { class KinectV1Device { @@ -19,6 +20,7 @@ namespace KinectOsvr { void toggleSeatedMode(); BodyTrackingState *getBodyStates(); void setTrackedBody(int i); + void recenter(); struct ui_thread_data { diff --git a/KinectV2Device.cpp b/KinectV2Device.cpp index 5a5b304..fcd4775 100644 --- a/KinectV2Device.cpp +++ b/KinectV2Device.cpp @@ -7,10 +7,18 @@ namespace KinectOsvr { + std::map windowMap2; + KinectV2Device::KinectV2Device(OSVR_PluginRegContext ctx, IKinectSensor* pKinectSensor) : m_pKinectSensor(pKinectSensor) { + m_trackingId = m_trackedBody = -1; + m_lastTrackedPosition.X = m_lastTrackedPosition.X = m_lastTrackedPosition.X = 0; + m_lastTrackedTime = { 0, 0 }; + m_firstUpdate = true; + m_trackedBodyChanged = false; + for (int i = 0; i < BODY_COUNT; i++) { - m_channels[i] = 0; + m_body_states[i] = CannotBeTracked; } HRESULT hr; @@ -38,6 +46,9 @@ namespace KinectOsvr { } SafeRelease(pBodyFrameSource); + mThreadData.kinect = this; + mThread = new std::thread(KinectV2Device::ui_thread, std::ref(mThreadData)); + /// Create the initialization options OSVR_DeviceInitOptions opts = osvrDeviceCreateInitOptions(ctx); @@ -107,6 +118,123 @@ namespace KinectOsvr { return OSVR_RETURN_SUCCESS; }; + KinectV2Device::BodyTrackingState* KinectV2Device::getBodyStates() { + return m_body_states; + } + + void KinectV2Device::setTrackedBody(int i) + { + m_trackedBody = i; + m_trackedBodyChanged = true; + } + + void KinectV2Device::ui_thread(ui_thread_data& data) + { + MSG msg; + BOOL ret; + HWND hDlg; + HINSTANCE hInst; + + hInst = GetModuleHandle("je_nourish_kinect.dll"); + hDlg = CreateDialogParam(hInst, MAKEINTRESOURCE(IDD_DIALOG1), 0, DialogProc, 0); + SetWindowText(hDlg, "OSVR Kinect V2 Config"); + ShowWindow(GetDlgItem(hDlg, IDC_CHECK1), SW_HIDE); + ShowWindow(hDlg, SW_RESTORE); + UpdateWindow(hDlg); + + windowMap2[hDlg] = data.kinect; + + KinectV2Device::BodyTrackingState previousStates[BODY_COUNT]; + KinectV2Device::BodyTrackingState* bodyStates = data.kinect->getBodyStates(); + for (int i = 0; i < BODY_COUNT; i++) { + previousStates[i] = bodyStates[i]; + } + + do { + ret = PeekMessage(&msg, 0, 0, 0, PM_REMOVE); + + if (ret) { + if (!IsDialogMessage(hDlg, &msg)) { + TranslateMessage(&msg); + DispatchMessage(&msg); + } + } + + bool redraw = false; + bool foundBody = false; + int bodies = 0; + + for (int i = 0; i < BODY_COUNT; i++) { + if (bodyStates[i] == ShouldBeTracked) { + foundBody = true; + } + if (bodyStates[i] != CannotBeTracked) { + bodies++; + } + if (bodyStates[i] != previousStates[i]) { + redraw = true; + EnableWindow(GetDlgItem(hDlg, IDC_RADIO1 + i), bodyStates[i] != CannotBeTracked); + if (bodyStates[i] == ShouldBeTracked) { + CheckRadioButton(hDlg, IDC_RADIO1, IDC_RADIO6, IDC_RADIO1 + i); + } + previousStates[i] = bodyStates[i]; + } + } + if (redraw) { + if (!foundBody) { + CheckRadioButton(hDlg, IDC_RADIO1, IDC_RADIO6, 0); + } + if (bodies == 1) { + SetDlgItemText(hDlg, IDC_STATIC1, "1 body detected."); + } + else { + SetDlgItemText(hDlg, IDC_STATIC1, (std::to_string(bodies) + " bodies detected.").c_str()); + } + UpdateWindow(hDlg); + } + + bodyStates = data.kinect->getBodyStates(); + + } while (true); + + DestroyWindow(hDlg); + } + + INT_PTR CALLBACK KinectV2Device::DialogProc(HWND hDlg, UINT uMsg, WPARAM wParam, LPARAM lParam) + { + switch (uMsg) + { + case WM_COMMAND: + switch (LOWORD(wParam)) + { + case IDC_BUTTON1: + windowMap2[hDlg]->recenter(); + break; + case IDC_RADIO1: + case IDC_RADIO2: + case IDC_RADIO3: + case IDC_RADIO4: + case IDC_RADIO5: + case IDC_RADIO6: + if (BST_CHECKED == Button_GetCheck(GetDlgItem(hDlg, LOWORD(wParam)))) { + windowMap2[hDlg]->setTrackedBody(LOWORD(wParam) - IDC_RADIO1); + } + break; + } + break; + + case WM_CLOSE: + DestroyWindow(hDlg); + return TRUE; + + case WM_DESTROY: + PostQuitMessage(0); + return TRUE; + } + + return FALSE; + } + void setupOffset(OSVR_PoseState* offset, Joint* joint, JointOrientation* jointOrientation) { osvrVec3SetX(&(offset->translation), joint->Position.X); osvrVec3SetY(&(offset->translation), joint->Position.Y); @@ -121,149 +249,241 @@ namespace KinectOsvr { osvr::util::toQuat(q.inverse(), offset->rotation); } + void KinectV2Device::recenter() + { + m_firstUpdate = true; + } + void KinectV2Device::ProcessBody(IBody** ppBodies, OSVR_TimeValue* timeValue) { if (m_pCoordinateMapper) { - for (int i = 0; i < BODY_COUNT; i++) { - IBody* pBody = ppBodies[i]; - if (pBody) + IdentifyBodies(ppBodies, timeValue); + + if (m_trackedBody >= 0) + { + IBody* pBody = ppBodies[m_trackedBody]; + + Joint joints[JointType_Count]; + JointOrientation jointOrientations[JointType_Count]; + HandState rightHandState = HandState_Unknown; + HandState leftHandState = HandState_Unknown; + + pBody->get_HandRightState(&rightHandState); + pBody->get_HandLeftState(&leftHandState); + + OSVR_ButtonState buttons[6]; + buttons[0] = rightHandState == HandState_Open; + buttons[1] = rightHandState == HandState_Closed; + buttons[2] = rightHandState == HandState_Lasso; + buttons[3] = leftHandState == HandState_Open; + buttons[4] = leftHandState == HandState_Closed; + buttons[5] = leftHandState == HandState_Lasso; + + // Send hand gestures as button presses + osvrDeviceButtonSetValues(m_dev, m_button, buttons, 6); + + HRESULT hr = pBody->GetJoints(_countof(joints), joints); + HRESULT hr2 = pBody->GetJointOrientations(_countof(jointOrientations), jointOrientations); + + if (SUCCEEDED(hr) && SUCCEEDED(hr2)) { + OSVR_PoseState poseState; + OSVR_Vec3 translation; + OSVR_Quaternion rotation; + + osvrVec3Zero(&translation); + osvrQuatSetIdentity(&rotation); - BOOLEAN bTracked = false; - HRESULT hr = pBody->get_IsTracked(&bTracked); + if (m_firstUpdate) { + m_firstUpdate = false; - if (!bTracked) { - removeBody(i); + setupOffset(&m_offset, &joints[JointType_Head], &jointOrientations[JointType_Neck]); + + osvrVec3SetX(&(m_kinectPose.translation), -joints[JointType_Head].Position.X); + osvrVec3SetY(&(m_kinectPose.translation), -joints[JointType_Head].Position.Y); + osvrVec3SetZ(&(m_kinectPose.translation), -joints[JointType_Head].Position.Z); + + Eigen::Quaterniond quaternion(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); + osvr::util::toQuat(quaternion, m_kinectPose.rotation); } - // Tracked body is the one who's been visible longest - if (SUCCEEDED(hr) && bTracked && firstBody(addBody(i))) + osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &m_kinectPose, 25, timeValue); + + for (int j = 0; j < _countof(joints); ++j) { + osvrVec3SetX(&translation, joints[j].Position.X); + osvrVec3SetY(&translation, joints[j].Position.Y); + osvrVec3SetZ(&translation, joints[j].Position.Z); + + osvrQuatSetX(&rotation, jointOrientations[j].Orientation.x); + osvrQuatSetY(&rotation, jointOrientations[j].Orientation.y); + osvrQuatSetZ(&rotation, jointOrientations[j].Orientation.z); + osvrQuatSetW(&rotation, jointOrientations[j].Orientation.w); + + // Rotate hand orientation to something more useful for OSVR + if (j == JointType_HandLeft || j == JointType_HandRight) { + boneSpaceToWorldSpace(&rotation); + } - Joint joints[JointType_Count]; - JointOrientation jointOrientations[JointType_Count]; - HandState rightHandState = HandState_Unknown; - HandState leftHandState = HandState_Unknown; - - pBody->get_HandRightState(&rightHandState); - pBody->get_HandLeftState(&leftHandState); - - OSVR_ButtonState buttons[6]; - buttons[0] = rightHandState == HandState_Open; - buttons[1] = rightHandState == HandState_Closed; - buttons[2] = rightHandState == HandState_Lasso; - buttons[3] = leftHandState == HandState_Open; - buttons[4] = leftHandState == HandState_Closed; - buttons[5] = leftHandState == HandState_Lasso; - - // Send hand gestures as button presses - osvrDeviceButtonSetValues(m_dev, m_button, buttons, 6); - - hr = pBody->GetJoints(_countof(joints), joints); - HRESULT hr2 = pBody->GetJointOrientations(_countof(jointOrientations), jointOrientations); - - if (SUCCEEDED(hr) && SUCCEEDED(hr2)) - { - OSVR_PoseState poseState; - OSVR_Vec3 translation; - OSVR_Quaternion rotation; - - osvrVec3Zero(&translation); - osvrQuatSetIdentity(&rotation); - - if (m_firstUpdate) { - m_firstUpdate = false; - - setupOffset(&m_offset, &joints[JointType_Head], &jointOrientations[JointType_Neck]); - - osvrVec3SetX(&(m_kinectPose.translation), -joints[JointType_Head].Position.X); - osvrVec3SetY(&(m_kinectPose.translation), -joints[JointType_Head].Position.Y); - osvrVec3SetZ(&(m_kinectPose.translation), -joints[JointType_Head].Position.Z); - - Eigen::Quaterniond quaternion(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); - osvr::util::toQuat(quaternion, m_kinectPose.rotation); - } - - osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &m_kinectPose, 25, timeValue); - - for (int j = 0; j < _countof(joints); ++j) - { - osvrVec3SetX(&translation, joints[j].Position.X); - osvrVec3SetY(&translation, joints[j].Position.Y); - osvrVec3SetZ(&translation, joints[j].Position.Z); - - osvrQuatSetX(&rotation, jointOrientations[j].Orientation.x); - osvrQuatSetY(&rotation, jointOrientations[j].Orientation.y); - osvrQuatSetZ(&rotation, jointOrientations[j].Orientation.z); - osvrQuatSetW(&rotation, jointOrientations[j].Orientation.w); - - // Rotate hand orientation to something more useful for OSVR - if (j == JointType_HandLeft || j == JointType_HandRight) { - boneSpaceToWorldSpace(&rotation); - } - - poseState.translation = translation; - poseState.rotation = rotation; - applyOffset(&m_offset, &poseState); - // Send pose - osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &poseState, j, timeValue); - - OSVR_AnalogState confidence = 0; - switch (joints[j].TrackingState) { - case TrackingState_Tracked: - confidence = 1; - break; - case TrackingState_Inferred: - confidence = 0.5; - break; - default: - case TrackingState_NotTracked: - confidence = 0; - break; - } - // Tracking confidence for use in smoothing plugins - osvrDeviceAnalogSetValueTimestamped(m_dev, m_analog, confidence, j, timeValue); - } + poseState.translation = translation; + poseState.rotation = rotation; + applyOffset(&m_offset, &poseState); + // Send pose + osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &poseState, j, timeValue); + + OSVR_AnalogState confidence = 0; + switch (joints[j].TrackingState) { + case TrackingState_Tracked: + confidence = 1; + break; + case TrackingState_Inferred: + confidence = 0.5; + break; + default: + case TrackingState_NotTracked: + confidence = 0; + break; } + // Tracking confidence for use in smoothing plugins + osvrDeviceAnalogSetValueTimestamped(m_dev, m_analog, confidence, j, timeValue); } } } } }; - bool KinectV2Device::firstBody(int channel) { - for (int i = 0; i < channel; i++) { - if (m_channels[i] != 0) { - return false; + void KinectV2Device::IdentifyBodies(IBody** ppBodies, OSVR_TimeValue* timeValue) { + HRESULT hr; + UINT64 trackingId; + + if (m_trackedBody >= 0) { // We're tracking a body + if (m_trackedBodyChanged) { + hr = ppBodies[m_trackedBody]->get_TrackingId(&m_trackingId); + if (SUCCEEDED(hr)) { + m_trackedBodyChanged = false; + } + else { + return; + } + } + else { + for (int i = 0; i < BODY_COUNT; ++i) { + hr = ppBodies[i]->get_TrackingId(&trackingId); + if (SUCCEEDED(hr) && trackingId == m_trackingId) { + m_trackedBody = i; + break; + } + } } - } - return true; - } - int KinectV2Device::addBody(int idx) { - for (int i = 0; i < BODY_COUNT; i++) { - if (m_channels[i] == idx) { - return i; + BOOLEAN isTracked; + hr = ppBodies[m_trackedBody]->get_IsTracked(&isTracked); + + if (SUCCEEDED(hr)) { + if (isTracked) { // Discount other bodies + for (int i = 0; i < BODY_COUNT; ++i) { + hr = ppBodies[i]->get_TrackingId(&trackingId); + if (SUCCEEDED(hr) && trackingId == m_trackingId) continue; + + hr = ppBodies[i]->get_IsTracked(&isTracked); + if (isTracked) { + m_body_states[i] = ShouldNotBeTracked; + } + else { + m_body_states[i] = CannotBeTracked; + } + } + return; + } + else { + m_body_states[m_trackedBody] = CannotBeTracked; + m_trackedBody = -1; + m_trackingId = -1; + } } } - for (int i = 0; i < BODY_COUNT; i++) { - if (!m_channels[i]) { - m_channels[i] = idx; - return i; + + // Lost tracking or haven't started yet + m_trackedBody = m_trackingId = -1; + int candidates = 0; + double confidence[BODY_COUNT]; + double timeConfidence = (timeValue->seconds - m_lastTrackedTime.seconds) / 15000.0; // If we lose tracking for a few seconds, just pick whoever's visible + + Joint joints[JointType_Count]; + CameraSpacePoint position; + float distanceFromLastPosition; + float distanceConfidence; + for (int i = 0; i < BODY_COUNT; ++i) + { + BOOLEAN trackingState; + + hr = ppBodies[i]->get_TrackingId(&trackingId); + hr = ppBodies[i]->get_IsTracked(&trackingState); + BodyTrackingState myTrackingState = m_body_states[i]; + + confidence[i] = 0.0f; + + if (trackingState) { + switch (myTrackingState) { + case CannotBeTracked: + case CanBeTracked: + m_body_states[i] = CanBeTracked; + candidates++; + + hr = ppBodies[i]->GetJoints(_countof(joints), joints); + position = joints[JointType_Head].Position; + + distanceFromLastPosition = sqrt(pow(position.X - m_lastTrackedPosition.X, 2) + + pow(position.Y - m_lastTrackedPosition.Y, 2) + pow(position.Z - m_lastTrackedPosition.Z, 2)); + distanceConfidence = 1.0f - distanceFromLastPosition / 7.0f; // Approx largest possible distance in playspace + confidence[i] = distanceConfidence + timeConfidence; + + break; + case ShouldNotBeTracked: // Ignore bodies we've previously ruled out + break; + case ShouldBeTracked: // Shouldn't be possible at this point + break; + } + } + else { + m_body_states[i] = CannotBeTracked; } } - return -1; - } - void KinectV2Device::removeBody(int idx) { - for (int i = 0; i < BODY_COUNT; i++) { - if (m_channels[i] == idx) { - m_channels[i] = 0; + switch (candidates) { + case 0: // No bodies found + break; + case 1: // Only 1 candidate (lets still wait until confidence is good enough) + default: // Multiple possible bodies, choose based on last known position + double bestConfidence = 0.0; + for (int i = 0; i < BODY_COUNT; ++i) { + if (m_body_states[i] == CanBeTracked) { + if (confidence[i] > bestConfidence) { + bestConfidence = confidence[i]; + m_trackedBody = i; + hr = ppBodies[i]->get_TrackingId(&m_trackingId); + } + } } + if (bestConfidence > 0.75) { + for (int i = 0; i < BODY_COUNT; ++i) { + if (i == m_trackedBody) { + m_body_states[i] = ShouldBeTracked; + } + else if (m_body_states[i] == CanBeTracked) { + m_body_states[i] = ShouldNotBeTracked; + } + } + } + else { + m_trackedBody = m_trackingId = -1; + } + break; } } + bool KinectV2Device::Detect(IKinectSensor** ppKinectSensor) { typedef HRESULT(_stdcall *GetDefaultKinectSensorType)(IKinectSensor**); diff --git a/KinectV2Device.h b/KinectV2Device.h index d9add97..f83fb56 100644 --- a/KinectV2Device.h +++ b/KinectV2Device.h @@ -1,4 +1,5 @@ #include "stdafx.h" +#include namespace KinectOsvr { class KinectV2Device { @@ -6,13 +7,31 @@ namespace KinectOsvr { KinectV2Device(OSVR_PluginRegContext ctx, IKinectSensor* pKinectSensor); ~KinectV2Device(); + enum BodyTrackingState { + CannotBeTracked, + CanBeTracked, + ShouldNotBeTracked, + ShouldBeTracked + }; + OSVR_ReturnCode update(); static bool Detect(IKinectSensor** ppKinectSensor); + + BodyTrackingState *getBodyStates(); + void setTrackedBody(int i); + void recenter(); + + struct ui_thread_data + { + std::mutex mutex; + KinectV2Device *kinect; + bool end = false; + }; + static void ui_thread(ui_thread_data& data); + static INT_PTR CALLBACK DialogProc(HWND hDlg, UINT uMsg, WPARAM wParam, LPARAM lParam); private: + void IdentifyBodies(IBody** ppBodies, OSVR_TimeValue* timeValue); void ProcessBody(IBody** ppBodies, OSVR_TimeValue* timeValue); - int addBody(int idx); - void removeBody(int idx); - bool firstBody(int channel); osvr::pluginkit::DeviceToken m_dev; OSVR_TrackerDeviceInterface m_tracker; @@ -23,13 +42,21 @@ namespace KinectOsvr { ICoordinateMapper* m_pCoordinateMapper; IBodyFrameReader* m_pBodyFrameReader; - int m_channels[BODY_COUNT]; - bool m_firstUpdate = true; OSVR_PoseState m_offset; OSVR_PoseState m_kinectPose; OSVR_TimeValue m_initializeTime; INT64 m_initializeOffset = 0; + + BodyTrackingState m_body_states[BODY_COUNT]; + UINT64 m_trackingId; + int m_trackedBody; + bool m_trackedBodyChanged; + CameraSpacePoint m_lastTrackedPosition; + OSVR_TimeValue m_lastTrackedTime; + + std::thread *mThread; + ui_thread_data mThreadData; }; } \ No newline at end of file diff --git a/je_nourish_kinect.aps b/je_nourish_kinect.aps index 9b69e89..aa03417 100644 Binary files a/je_nourish_kinect.aps and b/je_nourish_kinect.aps differ diff --git a/je_nourish_kinect.cpp b/je_nourish_kinect.cpp index 90920b1..e93eb71 100644 --- a/je_nourish_kinect.cpp +++ b/je_nourish_kinect.cpp @@ -17,8 +17,7 @@ namespace KinectOsvr { if (KinectV1Device::Detect(&pNuiSensor)) { m_found = true; - osvr::pluginkit::registerObjectForDeletion( - ctx, new KinectV1Device(ctx, pNuiSensor)); + osvr::pluginkit::registerObjectForDeletion(ctx, new KinectV1Device(ctx, pNuiSensor)); } } return OSVR_RETURN_SUCCESS; diff --git a/je_nourish_kinect.rc b/je_nourish_kinect.rc index 2a00107..e85d520 100644 Binary files a/je_nourish_kinect.rc and b/je_nourish_kinect.rc differ diff --git a/resource.h b/resource.h index c99d6c2..e3c5e04 100644 Binary files a/resource.h and b/resource.h differ diff --git a/stdafx.h b/stdafx.h index 499d920..e69ca55 100644 --- a/stdafx.h +++ b/stdafx.h @@ -6,9 +6,6 @@ #include #include -#include -#include - #include #include #include