RGB Camera support (reflection-only)

Author: meruune

KinectHandler/KinectHandler.h

@@ -52,6 +52,17 @@ namespace KinectHandler
             // implemented in the c# handler
         }
 
+        array<BYTE>^ GetImageBuffer()
+        {
+            if (!IsInitialized || !kinect_->camera_enabled()) return __nullptr;
+            const auto& [unmanagedBuffer, size] = kinect_->color_buffer();
+            if (size <= 0) return __nullptr;
+
+            auto data = gcnew array<byte>(size); // Managed image placeholder
+            Marshal::Copy(IntPtr(unmanagedBuffer), data, 0, size);
+            return data; // Return managed array of bytes for our camera image
+        }
+
         List<KinectJoint^>^ GetTrackedKinectJoints()
         {
             if (!IsInitialized) return gcnew List<KinectJoint^>;
@@ -102,11 +113,36 @@ namespace KinectHandler
             int get() { return kinect_->status_result(); }
         }
 
+        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(CameraSpacePoint{position.X, position.Y, position.Z});
+
+            return Drawing::Size(width, height);
+        }
+
         int InitializeKinect()
         {
             return kinect_->initialize();

KinectHandler/KinectWrapper.h

@@ -30,6 +30,7 @@ class KinectWrapper
 
     WAITABLE_HANDLE h_statusChangedEvent;
     WAITABLE_HANDLE h_bodyFrameEvent;
+    WAITABLE_HANDLE h_colorFrameEvent;
     bool newBodyFrameArrived = false;
 
     std::array<JointOrientation, JointType_Count> bone_orientations_;
@@ -39,6 +40,7 @@ class KinectWrapper
 
     inline static bool initialized_ = false;
     bool skeleton_tracked_ = false;
+    bool rgb_stream_enabled_ = false;
 
     void updater()
     {
@@ -83,6 +85,20 @@ class KinectWrapper
         }
     }
 
+    void updateColorData()
+    {
+        if (!colorFrameReader)return; // Give up already
+
+        IColorFrame* colorFrame = nullptr;
+        colorFrameReader->AcquireLatestFrame(&colorFrame);
+
+        if (!colorFrame) return;
+        ResetBuffer(CameraBufferSize()); // Allocate buffer for image for copy
+
+        colorFrame->CopyConvertedFrameDataToArray(
+            CameraBufferSize(), color_buffer_, ColorImageFormat_Bgra);
+    }
+
     bool initKinect()
     {
         // Get a working Kinect Sensor
@@ -130,6 +146,22 @@ class KinectWrapper
         if (bodyFrameSource) bodyFrameSource->Release();
     }
 
+    void initializeColor()
+    {
+        if (colorFrameReader)
+            colorFrameReader->Release();
+
+        IColorFrameSource* colorFrameSource;
+        kinectSensor->get_ColorFrameSource(&colorFrameSource);
+        colorFrameSource->OpenReader(&colorFrameReader);
+
+        // Newfangled event based frame capture
+        // https://github.com/StevenHickson/PCL_Kinect2SDK/blob/master/src/Microsoft_grabber2.cpp
+        h_colorFrameEvent = (WAITABLE_HANDLE)CreateEvent(nullptr, FALSE, FALSE, nullptr);
+        colorFrameReader->SubscribeFrameArrived(&h_colorFrameEvent);
+        if (colorFrameSource) colorFrameSource->Release();
+    }
+
     void terminateSkeleton()
     {
         if (!bodyFrameReader)return; // No need to do anything
@@ -150,6 +182,26 @@ class KinectWrapper
         bodyFrameReader = nullptr;
     }
 
+    void terminateColor()
+    {
+        if (!colorFrameReader)return; // No need to do anything
+        if (FAILED(colorFrameReader->UnsubscribeFrameArrived(h_colorFrameEvent)))
+        {
+            throw std::exception("Couldn't unsubscribe frame!");
+        }
+        __try
+        {
+            CloseHandle((HANDLE)h_colorFrameEvent);
+            colorFrameReader->Release();
+        }
+        __except (EXCEPTION_EXECUTE_HANDLER)
+        {
+            // ignored
+        }
+        h_colorFrameEvent = NULL;
+        colorFrameReader = nullptr;
+    }
+
     HRESULT kinect_status_result()
     {
         BOOLEAN avail;
@@ -246,6 +298,7 @@ class KinectWrapper
             if (!initialized_) return 1;
 
             initializeSkeleton();
+            initializeColor();
 
             // Recreate the updater thread
             if (!updater_thread_)
@@ -306,6 +359,24 @@ class KinectWrapper
                             pArgs->Release(); // Release the frame
                         }
                     }
+
+                if (h_colorFrameEvent && rgb_stream_enabled_)
+                    if (HANDLE handles[] = {reinterpret_cast<HANDLE>(h_colorFrameEvent)};
+                        // Wait for a frame to arrive, give up after >3s of nothing
+                        MsgWaitForMultipleObjects(_countof(handles), handles,
+                                                  false, 3000, QS_ALLINPUT) == WAIT_OBJECT_0)
+                    {
+                        IColorFrameArrivedEventArgs* pArgs = nullptr;
+                        if (colorFrameReader &&
+                            SUCCEEDED(colorFrameReader->GetFrameArrivedEventData(h_colorFrameEvent, &pArgs)))
+                        {
+                            [&,this](IColorFrameReader& sender, IColorFrameArrivedEventArgs& eventArgs)
+                            {
+                                updateColorData();
+                            }(*colorFrameReader, *pArgs);
+                            pArgs->Release(); // Release the frame
+                        }
+                    }
             }
         }
     }
@@ -319,6 +390,7 @@ class KinectWrapper
             {
                 // Protect from null call
                 terminateSkeleton();
+                terminateColor();
 
                 return [&, this]
                 {
@@ -376,13 +448,74 @@ class KinectWrapper
         return skeleton_positions_;
     }
 
+    std::tuple<BYTE*, int> color_buffer()
+    {
+        return std::make_tuple(color_buffer_, size_in_bytes_last_);
+    }
+
     bool skeleton_tracked()
     {
         return skeleton_tracked_;
     }
 
+    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<JointType>(kinectJointType));
     }
+
+    std::pair<int, int> CameraImageSize()
+    {
+        return std::make_pair(1920, 1080);
+    }
+
+    unsigned long CameraBufferSize()
+    {
+        const auto& [width, height] = CameraImageSize();
+        return width * height * 4;
+    }
+
+    std::pair<int, int> MapCoordinate(const CameraSpacePoint& skeletonPoint)
+    {
+        ColorSpacePoint spacePoint; // Holds the mapped coordinate
+        const auto& result = coordMapper->MapCameraPointToColorSpace(skeletonPoint, &spacePoint);
+
+        return SUCCEEDED(result) && !std::isnan(spacePoint.X) && !std::isnan(spacePoint.Y)
+                   ? std::make_pair(spacePoint.X, spacePoint.Y) // Send the mapped ones
+                   : std::make_pair(-1, -1); // Unknown coordinates - fall back to default drawing
+    }
+
+private:
+    DWORD size_in_bytes_ = 0;
+    DWORD size_in_bytes_last_ = 0;
+    BYTE* color_buffer_ = nullptr;
+
+    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_;
+    }
 };

plugin_KinectOne/KinectOne.cs

@@ -4,9 +4,12 @@
 using System;
 using System.Collections.ObjectModel;
 using System.ComponentModel.Composition;
+using System.Drawing;
 using System.Linq;
 using System.Numerics;
+using System.Runtime.InteropServices.WindowsRuntime;
 using Amethyst.Plugins.Contract;
+using Microsoft.UI.Xaml.Media.Imaging;
 
 // To learn more about WinUI, the WinUI project structure,
 // and more about our project templates, see: http://aka.ms/winui-project-info.
@@ -35,6 +38,7 @@ public class KinectOne : KinectHandler.KinectHandler, ITrackingDevice
     public bool IsFlipSupported => true;
     public bool IsAppOrientationSupported => true;
     public object SettingsInterfaceRoot => null;
+    public WriteableBitmap CameraImage { get; set; }
 
     public ObservableCollection<TrackedJoint> TrackedJoints { get; } =
         // Prepend all supported joints to the joints list
@@ -56,6 +60,8 @@ public class KinectOne : KinectHandler.KinectHandler, ITrackingDevice
     public void OnLoad()
     {
         PluginLoaded = true;
+
+        CameraImage = new WriteableBitmap(CameraImageWidth, CameraImageHeight);
     }
 
     public void Initialize()
@@ -104,6 +110,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)
@@ -119,6 +135,11 @@ public override void StatusChangedHandler()
         // Request a refresh of the status UI
         Host?.RefreshStatusInterface();
     }
+
+    public Func<BitmapSource> GetCameraImage => () => CameraImage;
+    public Func<bool> GetIsCameraEnabled => () => IsCameraEnabled;
+    public Action<bool> SetIsCameraEnabled => value => IsCameraEnabled = value;
+    public Func<Vector3, Size> MapCoordinateDelegate => MapCoordinate;
 }
 
 internal static class PoseUtils