diff --git a/KinectHandler/KinectHandler.h b/KinectHandler/KinectHandler.h index bf97422..e0fd41e 100644 --- a/KinectHandler/KinectHandler.h +++ b/KinectHandler/KinectHandler.h @@ -14,119 +14,155 @@ using namespace Amethyst::Plugins::Contract; namespace KinectHandler { - public ref class KinectJoint sealed - { - public: - KinectJoint(const int role) - { - JointRole = role; - } - - property Vector3 Position; - property Quaternion Orientation; - - property int TrackingState; - property int JointRole; - }; - - delegate void FunctionToCallDelegate(); - - public ref class KinectHandler - { - private: - KinectWrapper* kinect_; - FunctionToCallDelegate^ function_; - - public: - KinectHandler() : kinect_(new KinectWrapper()) - { - function_ = gcnew FunctionToCallDelegate(this, &KinectHandler::StatusChangedHandler); - pin_ptr tmp = &function_; // Pin the function delegate - - status_changed_event = static_cast( - Marshal::GetFunctionPointerForDelegate(function_).ToPointer()); - } - - virtual void StatusChangedHandler() - { - // implemented in the c# handler - } - - List^ GetTrackedKinectJoints() - { - if (!IsInitialized) return gcnew List; - - const auto& positions = kinect_->skeleton_positions(); - const auto& orientations = kinect_->bone_orientations(); - const auto& states = kinect_->tracking_states(); - - auto trackedKinectJoints = gcnew List; - for each (auto v in Enum::GetValues()) - { - if (v == TrackedJointType::JointHandTipLeft || - v == TrackedJointType::JointHandTipRight || - v == TrackedJointType::JointThumbLeft || - v == TrackedJointType::JointThumbRight || - v == TrackedJointType::JointNeck || - v == TrackedJointType::JointManual) - continue; // Skip unsupported joints - - auto joint = gcnew KinectJoint(static_cast(v)); - - joint->TrackingState = - states[kinect_->KinectJointType(static_cast(v))]; - - joint->Position = Vector3( - positions[kinect_->KinectJointType(static_cast(v))].x, - positions[kinect_->KinectJointType(static_cast(v))].y, - positions[kinect_->KinectJointType(static_cast(v))].z); - - joint->Orientation = Quaternion( - orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.x, - orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.y, - orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.z, - orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.w); - - trackedKinectJoints->Add(joint); - } - - return trackedKinectJoints; - } - - property bool IsInitialized - { - bool get() { return kinect_->is_initialized(); } - } - - property bool IsSkeletonTracked - { - bool get() { return kinect_->skeleton_tracked(); } - } - - property int DeviceStatus - { - int get() { return kinect_->status_result(); } - } - - property int ElevationAngle - { - int get() { return kinect_->elevation_angle(); } - void set(const int value) { kinect_->elevation_angle(value); } - } - - property bool IsSettingsDaemonSupported - { - bool get() { return DeviceStatus == 0; } - } - - int InitializeKinect() - { - return kinect_->initialize(); - } - - int ShutdownKinect() - { - return kinect_->shutdown(); - } - }; + public ref class KinectJoint sealed + { + public: + KinectJoint(const int role) + { + JointRole = role; + } + + property Vector3 Position; + property Quaternion Orientation; + + property int TrackingState; + property int JointRole; + }; + + delegate void FunctionToCallDelegate(); + + public ref class KinectHandler + { + private: + KinectWrapper* kinect_; + FunctionToCallDelegate^ function_; + + public: + KinectHandler() : kinect_(new KinectWrapper()) + { + function_ = gcnew FunctionToCallDelegate(this, &KinectHandler::StatusChangedHandler); + pin_ptr tmp = &function_; // Pin the function delegate + + status_changed_event = static_cast( + Marshal::GetFunctionPointerForDelegate(function_).ToPointer()); + } + + virtual void StatusChangedHandler() + { + // implemented in the C# handler + } + + array^ GetImageBuffer() + { + if (!IsInitialized || !kinect_->camera_enabled()) return __nullptr; + const auto& [unmanagedBuffer, size] = kinect_->color_buffer(); + if (size <= 0) return __nullptr; + + auto data = gcnew array(size); // Managed image placeholder + Marshal::Copy(IntPtr(unmanagedBuffer), data, 0, size); + return data; // Return managed array of bytes for our camera image + } + + List^ GetTrackedKinectJoints() + { + if (!IsInitialized) return gcnew List; + + const auto& positions = kinect_->skeleton_positions(); + const auto& orientations = kinect_->bone_orientations(); + const auto& states = kinect_->tracking_states(); + + auto trackedKinectJoints = gcnew List; + for each (auto v in Enum::GetValues()) + { + if (v == TrackedJointType::JointHandTipLeft || + v == TrackedJointType::JointHandTipRight || + v == TrackedJointType::JointThumbLeft || + v == TrackedJointType::JointThumbRight || + v == TrackedJointType::JointNeck || + v == TrackedJointType::JointManual) + continue; // Skip unsupported joints + + auto joint = gcnew KinectJoint(static_cast(v)); + + joint->TrackingState = + states[kinect_->KinectJointType(static_cast(v))]; + + joint->Position = Vector3( + positions[kinect_->KinectJointType(static_cast(v))].x, + positions[kinect_->KinectJointType(static_cast(v))].y, + positions[kinect_->KinectJointType(static_cast(v))].z); + + joint->Orientation = Quaternion( + orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.x, + orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.y, + orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.z, + orientations[kinect_->KinectJointType(static_cast(v))].absoluteRotation.rotationQuaternion.w); + + trackedKinectJoints->Add(joint); + } + + return trackedKinectJoints; + } + + property bool IsInitialized + { + bool get() { return kinect_->is_initialized(); } + } + + property bool IsSkeletonTracked + { + bool get() { return kinect_->skeleton_tracked(); } + } + + property int DeviceStatus + { + int get() { return kinect_->status_result(); } + } + + property int ElevationAngle + { + int get() { return kinect_->elevation_angle(); } + void set(const int value) { kinect_->elevation_angle(value); } + } + + property bool IsCameraEnabled + { + bool get() { return kinect_->camera_enabled(); } + void set(const bool value) { kinect_->camera_enabled(value); } + } + + property bool IsSettingsDaemonSupported + { + bool get() { return DeviceStatus == 0; } + } + + property int CameraImageWidth + { + int get() { return kinect_->CameraImageSize().first; } + } + + property int CameraImageHeight + { + int get() { return kinect_->CameraImageSize().second; } + } + + Drawing::Size MapCoordinate(Vector3 position) + { + if (!IsInitialized) return Drawing::Size::Empty; + const auto& [width, height] = + kinect_->MapCoordinate(_Vector4{position.X, position.Y, position.Z, 1.0f}); + + return Drawing::Size(width, height); + } + + int InitializeKinect() + { + return kinect_->initialize(); + } + + int ShutdownKinect() + { + return kinect_->shutdown(); + } + }; } diff --git a/KinectHandler/KinectWrapper.h b/KinectHandler/KinectWrapper.h index 2f0bf9f..751e356 100644 --- a/KinectHandler/KinectWrapper.h +++ b/KinectHandler/KinectWrapper.h @@ -31,6 +31,7 @@ class KinectWrapper inline static bool initialized_ = false; bool skeleton_tracked_ = false; + bool rgb_stream_enabled_ = false; void updater() { @@ -38,10 +39,35 @@ class KinectWrapper while (true)update(); } - void updateSkeletalData() + void updateSensorData() { - // Wait for a frame to arrive, give up after >3s of nothing - if (kinectSensor && kinectSensor->NuiSkeletonGetNextFrame(3000, &skeletonFrame) >= 0) + NUI_IMAGE_FRAME imageFrame{}; + + // Wait for a frame to arrive, give up if nothing + if (rgb_stream_enabled_ && kinectSensor && + kinectSensor->NuiImageStreamGetNextFrame(kinectRGBStream, 0, &imageFrame) >= 0) + { + INuiFrameTexture* pTexture = imageFrame.pFrameTexture; + + // Lock the frame data so the Kinect knows not to modify it while we are reading it + NUI_LOCKED_RECT lockedRect; + pTexture->LockRect(0, &lockedRect, nullptr, 0); + + // Make sure we've received valid data + size_in_bytes_last_ = 0; + if (lockedRect.Pitch != 0) + { + ResetBuffer(CameraBufferSize()); // Allocate buffer for image, copy to buffer + memcpy_s(color_buffer_, size_in_bytes_, lockedRect.pBits, lockedRect.size); + size_in_bytes_last_ = size_in_bytes_; // Backup for the bitmap generator + } + + pTexture->UnlockRect(0); // Unlock frame data and proceed + kinectSensor->NuiImageStreamReleaseFrame(kinectRGBStream, &imageFrame); + } + + // Wait for a frame to arrive, give up if nothing + if (kinectSensor && kinectSensor->NuiSkeletonGetNextFrame(0, &skeletonFrame) >= 0) for (auto& i : skeletonFrame.SkeletonData) { if (NUI_SKELETON_TRACKED == i.eTrackingState) @@ -62,28 +88,32 @@ class KinectWrapper } } - bool initKinect() - { + bool initKinect() + { // Register a StatusChanged event NuiSetDeviceStatusCallback(&statusCallbackEvent, nullptr); - // Get a working Kinect Sensor - int numSensors = 0; - if (NuiGetSensorCount(&numSensors) < 0 || numSensors < 1) - { - return false; - } - if (NuiCreateSensorByIndex(0, &kinectSensor) < 0) - { - return false; - } + // Get a working Kinect Sensor + int numSensors = 0; + if (NuiGetSensorCount(&numSensors) < 0 || numSensors < 1) + { + return false; + } + if (NuiCreateSensorByIndex(0, &kinectSensor) < 0) + { + return false; + } // Check the sensor (just in case) if (kinectSensor == nullptr) return false; // Initialize Sensor - HRESULT hr = kinectSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON); - kinectSensor->NuiSkeletonTrackingEnable(nullptr, 0); //NUI_SKELETON_TRACKING_FLAG_ENABLE_IN_NEAR_RANGE + HRESULT hr = kinectSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON | NUI_INITIALIZE_FLAG_USES_COLOR); + + kinectSensor->NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, resolution_, + 0, 2, nullptr, &kinectRGBStream); + + kinectSensor->NuiSkeletonTrackingEnable(nullptr, 0); return kinectSensor; } @@ -219,7 +249,7 @@ class KinectWrapper // Update (only if the sensor is on and its status is ok) if (initialized_ && kinectSensor && kinectSensor->NuiStatus() == S_OK) - updateSkeletalData(); + updateSensorData(); } int shutdown() @@ -268,6 +298,11 @@ class KinectWrapper return tracking_states_; } + std::tuple color_buffer() + { + return std::make_tuple(color_buffer_, size_in_bytes_last_); + } + bool skeleton_tracked() { return skeleton_tracked_; @@ -286,8 +321,91 @@ class KinectWrapper return angle; } + void camera_enabled(bool enabled) + { + rgb_stream_enabled_ = enabled; + } + + bool camera_enabled(void) + { + return rgb_stream_enabled_; + } + int KinectJointType(int kinectJointType) { return KinectJointTypeDictionary.at(static_cast(kinectJointType)); } + + std::pair CameraImageSize() + { + switch (resolution_) + { + case NUI_IMAGE_RESOLUTION_1280x960: + return std::make_pair(1280, 760); + case NUI_IMAGE_RESOLUTION_640x480: + return std::make_pair(640, 480); + case NUI_IMAGE_RESOLUTION_320x240: + return std::make_pair(320, 240); + case NUI_IMAGE_RESOLUTION_80x60: + return std::make_pair(80, 60); + default: + return std::make_pair(1280, 760); + } + } + + unsigned long CameraBufferSize() + { + const auto& [width,height] = CameraImageSize(); + return width * height * 4; + } + + + std::pair MapCoordinate(const _Vector4& skeletonPoint) + { + LONG x = 0, y = 0; + LONG backupX = x, backupY = y; + + USHORT depthValue = 0; + const auto& [width, height] = CameraImageSize(); + NuiTransformSkeletonToDepthImage(skeletonPoint, &x, &y, &depthValue, resolution_); + + if (FAILED(NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution( + resolution_, resolution_, nullptr, + x, y, depthValue, &x, &y))) + { + x = backupX; + y = backupY; + } + + // return std::make_pair(1000 * x / width, 1000 * y / width); + return std::make_pair(x, y); + } + +private: + DWORD size_in_bytes_ = 0; + DWORD size_in_bytes_last_ = 0; + BYTE* color_buffer_ = nullptr; + + _NUI_IMAGE_RESOLUTION resolution_ = + NUI_IMAGE_RESOLUTION_640x480; + + BYTE* ResetBuffer(UINT size) + { + if (!color_buffer_ || size_in_bytes_ != size) + { + if (color_buffer_) + { + delete[] color_buffer_; + color_buffer_ = nullptr; + } + + if (0 != size) + { + color_buffer_ = new BYTE[size]; + } + size_in_bytes_ = size; + } + + return color_buffer_; + } }; diff --git a/plugin_Kinect360/Kinect360.cs b/plugin_Kinect360/Kinect360.cs index eaf285e..f3e2b8c 100644 --- a/plugin_Kinect360/Kinect360.cs +++ b/plugin_Kinect360/Kinect360.cs @@ -4,11 +4,17 @@ using System; using System.Collections.ObjectModel; using System.ComponentModel.Composition; +using System.Drawing; +using System.IO; using System.Linq; using System.Numerics; +using System.Runtime.InteropServices.WindowsRuntime; +using System.Threading.Tasks; using Amethyst.Plugins.Contract; using Microsoft.UI.Xaml; using Microsoft.UI.Xaml.Controls; +using Microsoft.UI.Xaml.Media.Imaging; +using Windows.Storage.Streams; // To learn more about WinUI, the WinUI project structure, // and more about our project templates, see: http://aka.ms/winui-project-info. @@ -33,6 +39,7 @@ public class Kinect360 : KinectHandler.KinectHandler, ITrackingDevice private Page InterfaceRoot { get; set; } private NumberBox TiltNumberBox { get; set; } private TextBlock TiltTextBlock { get; set; } + public WriteableBitmap CameraImage { get; set; } public bool IsPositionFilterBlockingEnabled => false; public bool IsPhysicsOverrideEnabled => false; @@ -102,12 +109,19 @@ public void OnLoad() VerticalAlignment = VerticalAlignment.Center }; + CameraImage = new WriteableBitmap(CameraImageWidth, CameraImageHeight); InterfaceRoot = new Page { Content = new StackPanel { - Orientation = Orientation.Horizontal, - Children = { TiltTextBlock, TiltNumberBox } + Children = + { + new StackPanel + { + Orientation = Orientation.Horizontal, + Children = { TiltTextBlock, TiltNumberBox } + } + } } }; @@ -154,6 +168,7 @@ public void Shutdown() public void Update() { + // Update skeletal data var trackedJoints = GetTrackedKinectJoints(); trackedJoints.ForEach(x => { @@ -163,6 +178,16 @@ public void Update() TrackedJoints[trackedJoints.IndexOf(x)].Position = x.Position.Safe(); TrackedJoints[trackedJoints.IndexOf(x)].Orientation = x.Orientation.Safe(); }); + + // Update camera feed + if (!IsCameraEnabled) return; + CameraImage.DispatcherQueue.TryEnqueue(async () => + { + var buffer = GetImageBuffer(); // Read from Kinect + if (buffer is null || buffer.Length <= 0) return; + await CameraImage.PixelBuffer.AsStream().WriteAsync(buffer); + CameraImage.Invalidate(); // Enqueue for preview refresh + }); } public void SignalJoint(int jointId) @@ -182,9 +207,14 @@ public override void StatusChangedHandler() // Request a refresh of the status UI Host?.RefreshStatusInterface(); } + + public Func GetCameraImage => () => CameraImage; + public Func GetIsCameraEnabled => () => IsCameraEnabled; + public Action SetIsCameraEnabled => value => IsCameraEnabled = value; + public Func MapCoordinateDelegate => MapCoordinate; } -internal static class PoseUtils +internal static class Utils { public static Quaternion Safe(this Quaternion q) { diff --git a/plugin_Kinect360/Properties/PublishProfiles/FolderProfile.pubxml b/plugin_Kinect360/Properties/PublishProfiles/FolderProfile.pubxml index 3306ccd..f4fe6cb 100644 --- a/plugin_Kinect360/Properties/PublishProfiles/FolderProfile.pubxml +++ b/plugin_Kinect360/Properties/PublishProfiles/FolderProfile.pubxml @@ -4,7 +4,7 @@ https://go.microsoft.com/fwlink/?LinkID=208121. --> - Release + Debug x64 bin\Release\Publish FileSystem