Skip to content

Commit 015755d

Browse files
meruunemeruune
committed
Camera streams finally work ^^
1 parent 2e7d4b4 commit 015755d

File tree

2 files changed

+87
-105
lines changed

2 files changed

+87
-105
lines changed

‎KinectHandler/KinectHandler.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ namespace KinectHandler
5656
{
5757
if (!IsInitialized || !kinect_->camera_enabled()) return __nullptr;
5858
const auto& [unmanagedBuffer, size] = kinect_->color_buffer();
59-
if (size <= 0) return __nullptr;
59+
if (!unmanagedBuffer || size <= 0) return __nullptr;
6060

6161
auto data = gcnew array<byte>(size); // Managed image placeholder
6262
Marshal::Copy(IntPtr(unmanagedBuffer), data, 0, size);

‎KinectHandler/KinectWrapper.h‎

Lines changed: 86 additions & 104 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,7 @@ inline void (*status_changed_event)();
1717
class KinectWrapper
1818
{
1919
IKinectSensor* kinectSensor = nullptr;
20-
IBodyFrameReader* bodyFrameReader = nullptr;
21-
IColorFrameReader* colorFrameReader = nullptr;
22-
IDepthFrameReader* depthFrameReader = nullptr;
20+
IMultiSourceFrameReader* multiFrameReader = nullptr;
2321
IMultiSourceFrame* multiFrame = nullptr;
2422
ICoordinateMapper* coordMapper = nullptr;
2523
BOOLEAN isTracking = false;
@@ -29,9 +27,7 @@ class KinectWrapper
2927
IBody* kinectBodies[BODY_COUNT] = {nullptr};
3028

3129
WAITABLE_HANDLE h_statusChangedEvent;
32-
WAITABLE_HANDLE h_bodyFrameEvent;
33-
WAITABLE_HANDLE h_colorFrameEvent;
34-
bool newBodyFrameArrived = false;
30+
WAITABLE_HANDLE h_multiFrameEvent;
3531

3632
std::array<JointOrientation, JointType_Count> bone_orientations_;
3733
std::array<Joint, JointType_Count> skeleton_positions_;
@@ -48,17 +44,28 @@ class KinectWrapper
4844
while (true)update();
4945
}
5046

51-
void updateSkeletalData()
47+
void updateFrameData(IMultiSourceFrameArrivedEventArgs* args)
5248
{
53-
if (!bodyFrameReader)return; // Give up already
49+
if (!multiFrameReader) return; // Give up already
50+
51+
// Acquire the multi-source frame reference
52+
IMultiSourceFrameReference* frameReference = nullptr;
53+
args->get_FrameReference(&frameReference);
54+
55+
IMultiSourceFrame* multiFrame = nullptr;
56+
frameReference->AcquireFrame(&multiFrame);
57+
58+
if (!multiFrame) return;
59+
60+
// Get the body frame and process it
61+
IBodyFrameReference* bodyFrameReference = nullptr;
62+
multiFrame->get_BodyFrameReference(&bodyFrameReference);
5463

5564
IBodyFrame* bodyFrame = nullptr;
56-
bodyFrameReader->AcquireLatestFrame(&bodyFrame);
65+
bodyFrameReference->AcquireFrame(&bodyFrame);
5766

5867
if (!bodyFrame) return;
59-
6068
bodyFrame->GetAndRefreshBodyData(BODY_COUNT, kinectBodies);
61-
newBodyFrameArrived = true;
6269
if (bodyFrame) bodyFrame->Release();
6370

6471
// We have the frame, now parse it
@@ -75,28 +82,32 @@ class KinectWrapper
7582

7683
// Copy joint positions & orientations
7784
std::copy(std::begin(joints), std::end(joints),
78-
skeleton_positions_.begin());
85+
skeleton_positions_.begin());
7986
std::copy(std::begin(boneOrientations), std::end(boneOrientations),
80-
bone_orientations_.begin());
87+
bone_orientations_.begin());
8188

8289
break;
8390
}
8491
skeleton_tracked_ = false;
8592
}
86-
}
8793

88-
void updateColorData()
89-
{
90-
if (!colorFrameReader)return; // Give up already
94+
// Don't process color if not requested
95+
if (!camera_enabled()) return;
96+
97+
// Get the color frame and process it
98+
IColorFrameReference* colorFrameReference = nullptr;
99+
multiFrame->get_ColorFrameReference(&colorFrameReference);
91100

92101
IColorFrame* colorFrame = nullptr;
93-
colorFrameReader->AcquireLatestFrame(&colorFrame);
102+
colorFrameReference->AcquireFrame(&colorFrame);
94103

95104
if (!colorFrame) return;
96105
ResetBuffer(CameraBufferSize()); // Allocate buffer for image for copy
97106

98107
colorFrame->CopyConvertedFrameDataToArray(
99108
CameraBufferSize(), color_buffer_, ColorImageFormat_Bgra);
109+
110+
if (colorFrame) colorFrame->Release();
100111
}
101112

102113
bool initKinect()
@@ -114,6 +125,11 @@ class KinectWrapper
114125
BOOLEAN available = false;
115126
kinectSensor->get_IsAvailable(&available);
116127

128+
#ifdef _DEBUG
129+
// Emulation support bypass
130+
available = true;
131+
#endif
132+
117133
// Check the sensor (just in case)
118134
if (FAILED(hr_open) || !available || kinectSensor == nullptr)
119135
{
@@ -130,76 +146,38 @@ class KinectWrapper
130146
return false;
131147
}
132148

133-
void initializeSkeleton()
134-
{
135-
if (bodyFrameReader)
136-
bodyFrameReader->Release();
137-
138-
IBodyFrameSource* bodyFrameSource;
139-
kinectSensor->get_BodyFrameSource(&bodyFrameSource);
140-
bodyFrameSource->OpenReader(&bodyFrameReader);
141-
142-
// Newfangled event based frame capture
143-
// https://github.com/StevenHickson/PCL_Kinect2SDK/blob/master/src/Microsoft_grabber2.cpp
144-
h_bodyFrameEvent = (WAITABLE_HANDLE)CreateEvent(nullptr, FALSE, FALSE, nullptr);
145-
bodyFrameReader->SubscribeFrameArrived(&h_bodyFrameEvent);
146-
if (bodyFrameSource) bodyFrameSource->Release();
147-
}
148-
149-
void initializeColor()
149+
void initializeFrameReader()
150150
{
151-
if (colorFrameReader)
152-
colorFrameReader->Release();
151+
if (multiFrameReader)
152+
multiFrameReader->Release();
153153

154-
IColorFrameSource* colorFrameSource;
155-
kinectSensor->get_ColorFrameSource(&colorFrameSource);
156-
colorFrameSource->OpenReader(&colorFrameReader);
154+
kinectSensor->OpenMultiSourceFrameReader(
155+
FrameSourceTypes_Body | FrameSourceTypes_Color, &multiFrameReader);
157156

158157
// Newfangled event based frame capture
159158
// https://github.com/StevenHickson/PCL_Kinect2SDK/blob/master/src/Microsoft_grabber2.cpp
160-
h_colorFrameEvent = (WAITABLE_HANDLE)CreateEvent(nullptr, FALSE, FALSE, nullptr);
161-
colorFrameReader->SubscribeFrameArrived(&h_colorFrameEvent);
162-
if (colorFrameSource) colorFrameSource->Release();
159+
h_multiFrameEvent = (WAITABLE_HANDLE)CreateEvent(nullptr, FALSE, FALSE, nullptr);
160+
multiFrameReader->SubscribeMultiSourceFrameArrived(&h_multiFrameEvent);
163161
}
164162

165-
void terminateSkeleton()
163+
void terminateMultiFrame()
166164
{
167-
if (!bodyFrameReader)return; // No need to do anything
168-
if (FAILED(bodyFrameReader->UnsubscribeFrameArrived(h_bodyFrameEvent)))
165+
if (!multiFrameReader)return; // No need to do anything
166+
if (FAILED(multiFrameReader->UnsubscribeMultiSourceFrameArrived(h_multiFrameEvent)))
169167
{
170168
throw std::exception("Couldn't unsubscribe frame!");
171169
}
172170
__try
173171
{
174-
CloseHandle((HANDLE)h_bodyFrameEvent);
175-
bodyFrameReader->Release();
172+
CloseHandle((HANDLE)h_multiFrameEvent);
173+
multiFrameReader->Release();
176174
}
177175
__except (EXCEPTION_EXECUTE_HANDLER)
178176
{
179177
// ignored
180178
}
181-
h_bodyFrameEvent = NULL;
182-
bodyFrameReader = nullptr;
183-
}
184-
185-
void terminateColor()
186-
{
187-
if (!colorFrameReader)return; // No need to do anything
188-
if (FAILED(colorFrameReader->UnsubscribeFrameArrived(h_colorFrameEvent)))
189-
{
190-
throw std::exception("Couldn't unsubscribe frame!");
191-
}
192-
__try
193-
{
194-
CloseHandle((HANDLE)h_colorFrameEvent);
195-
colorFrameReader->Release();
196-
}
197-
__except (EXCEPTION_EXECUTE_HANDLER)
198-
{
199-
// ignored
200-
}
201-
h_colorFrameEvent = NULL;
202-
colorFrameReader = nullptr;
179+
h_multiFrameEvent = NULL;
180+
multiFrameReader = nullptr;
203181
}
204182

205183
HRESULT kinect_status_result()
@@ -277,28 +255,35 @@ class KinectWrapper
277255
public:
278256
bool is_initialized()
279257
{
258+
#ifdef _DEBUG
259+
return true;
260+
#else
280261
return initialized_;
262+
#endif
281263
}
282264

283265
HRESULT status_result()
284266
{
267+
#ifdef _DEBUG
268+
return 0;
269+
#else
285270
switch (kinect_status_result())
286271
{
287272
case S_OK: return 0;
288273
case S_FALSE: return 1;
289274
default: return -1;
290275
}
276+
#endif
291277
}
292278

293279
int initialize()
294280
{
295281
try
296282
{
297283
initialized_ = initKinect();
298-
if (!initialized_) return 1;
284+
if (!is_initialized()) return 1;
299285

300-
initializeSkeleton();
301-
initializeColor();
286+
initializeFrameReader();
302287

303288
// Recreate the updater thread
304289
if (!updater_thread_)
@@ -324,56 +309,50 @@ class KinectWrapper
324309
{
325310
BOOLEAN isAvailable = false;
326311
args->get_IsAvailable(&isAvailable);
312+
313+
#ifdef _DEBUG
314+
// Emulation support bypass
315+
isAvailable = true;
316+
#endif
317+
327318
initialized_ = isAvailable; // Update the status
328319
status_changed_event(); // Notify the CLR listener
329320
}
330321
}
331322
});
332323

333324
// Update (only if the sensor is on and its status is ok)
334-
if (initialized_ && kinectSensor)
325+
if (is_initialized() && kinectSensor)
335326
{
336327
BOOLEAN isAvailable = false;
337328
HRESULT kinectStatus = kinectSensor->get_IsAvailable(&isAvailable);
329+
330+
#ifdef _DEBUG
331+
// Emulation support bypass
332+
isAvailable = true;
333+
#endif
334+
338335
if (kinectStatus == S_OK)
339336
{
340337
// NEW ARRIVED FRAMES ------------------------
341338
MSG msg;
342339
while (PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE)) // Unnecessary?
343340
DispatchMessage(&msg);
344341

345-
if (h_bodyFrameEvent)
346-
if (HANDLE handles[] = {reinterpret_cast<HANDLE>(h_bodyFrameEvent)};
342+
if (h_multiFrameEvent)
343+
if (HANDLE handles[] = {reinterpret_cast<HANDLE>(h_multiFrameEvent)};
347344
// Wait for a frame to arrive, give up after >3s of nothing
348345
MsgWaitForMultipleObjects(_countof(handles), handles,
349-
false, 3000, QS_ALLINPUT) == WAIT_OBJECT_0)
346+
false, 100, QS_ALLINPUT) == WAIT_OBJECT_0)
350347
{
351-
IBodyFrameArrivedEventArgs* pArgs = nullptr;
352-
if (bodyFrameReader &&
353-
SUCCEEDED(bodyFrameReader->GetFrameArrivedEventData(h_bodyFrameEvent, &pArgs)))
348+
IMultiSourceFrameArrivedEventArgs* pArgs = nullptr;
349+
if (multiFrameReader &&
350+
SUCCEEDED(multiFrameReader->GetMultiSourceFrameArrivedEventData(h_multiFrameEvent, &pArgs)))
354351
{
355-
[&,this](IBodyFrameReader& sender, IBodyFrameArrivedEventArgs& eventArgs)
352+
[&,this](IMultiSourceFrameReader& sender, IMultiSourceFrameArrivedEventArgs& eventArgs)
356353
{
357-
updateSkeletalData();
358-
}(*bodyFrameReader, *pArgs);
359-
pArgs->Release(); // Release the frame
360-
}
361-
}
362-
363-
if (h_colorFrameEvent && rgb_stream_enabled_)
364-
if (HANDLE handles[] = {reinterpret_cast<HANDLE>(h_colorFrameEvent)};
365-
// Wait for a frame to arrive, give up after >3s of nothing
366-
MsgWaitForMultipleObjects(_countof(handles), handles,
367-
false, 3000, QS_ALLINPUT) == WAIT_OBJECT_0)
368-
{
369-
IColorFrameArrivedEventArgs* pArgs = nullptr;
370-
if (colorFrameReader &&
371-
SUCCEEDED(colorFrameReader->GetFrameArrivedEventData(h_colorFrameEvent, &pArgs)))
372-
{
373-
[&,this](IColorFrameReader& sender, IColorFrameArrivedEventArgs& eventArgs)
374-
{
375-
updateColorData();
376-
}(*colorFrameReader, *pArgs);
354+
updateFrameData(&eventArgs);
355+
}(*multiFrameReader, *pArgs);
377356
pArgs->Release(); // Release the frame
378357
}
379358
}
@@ -389,8 +368,7 @@ class KinectWrapper
389368
if (kinectSensor)
390369
{
391370
// Protect from null call
392-
terminateSkeleton();
393-
terminateColor();
371+
terminateMultiFrame();
394372

395373
return [&, this]
396374
{
@@ -486,8 +464,11 @@ class KinectWrapper
486464

487465
std::pair<int, int> MapCoordinate(const CameraSpacePoint& skeletonPoint)
488466
{
467+
auto _skeletonPoint = skeletonPoint;
468+
if (_skeletonPoint.Z < 0) _skeletonPoint.Z = 0.1f;
469+
489470
ColorSpacePoint spacePoint; // Holds the mapped coordinate
490-
const auto& result = coordMapper->MapCameraPointToColorSpace(skeletonPoint, &spacePoint);
471+
const auto& result = coordMapper->MapCameraPointToColorSpace(_skeletonPoint, &spacePoint);
491472

492473
return SUCCEEDED(result) && !std::isnan(spacePoint.X) && !std::isnan(spacePoint.Y)
493474
? std::make_pair(spacePoint.X, spacePoint.Y) // Send the mapped ones
@@ -501,6 +482,7 @@ class KinectWrapper
501482

502483
BYTE* ResetBuffer(UINT size)
503484
{
485+
size_in_bytes_last_ = size;
504486
if (!color_buffer_ || size_in_bytes_ != size)
505487
{
506488
if (color_buffer_)

0 commit comments

Comments
 (0)