Skip to content

Commit

Permalink
RGB Camera support (reflection-only)
Browse files Browse the repository at this point in the history
  • Loading branch information
@KimihikoAkayasaki
KimihikoAkayasaki committed Oct 18, 2024
1 parent 89df3b9 commit 2e7d4b4
Show file tree
Hide file tree
Showing 3 changed files with 190 additions and 0 deletions.
36 changes: 36 additions & 0 deletions KinectHandler/KinectHandler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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^>;
Expand Down Expand Up @@ -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();
Expand Down
133 changes: 133 additions & 0 deletions KinectHandler/KinectWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand All @@ -39,6 +40,7 @@ class KinectWrapper

inline static bool initialized_ = false;
bool skeleton_tracked_ = false;
bool rgb_stream_enabled_ = false;

void updater()
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -246,6 +298,7 @@ class KinectWrapper
if (!initialized_) return 1;

initializeSkeleton();
initializeColor();

// Recreate the updater thread
if (!updater_thread_)
Expand Down Expand Up @@ -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
}
}
}
}
}
Expand All @@ -319,6 +390,7 @@ class KinectWrapper
{
// Protect from null call
terminateSkeleton();
terminateColor();

return [&, this]
{
Expand Down Expand Up @@ -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_;
}
};
21 changes: 21 additions & 0 deletions plugin_KinectOne/KinectOne.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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
Expand All @@ -56,6 +60,8 @@ public class KinectOne : KinectHandler.KinectHandler, ITrackingDevice
public void OnLoad()
{
PluginLoaded = true;

CameraImage = new WriteableBitmap(CameraImageWidth, CameraImageHeight);
}

public void Initialize()
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand Down

0 comments on commit 2e7d4b4

Please sign in to comment.