diff --git a/Source/Drivers/Kinect2/BaseKinect2Stream.cpp b/Source/Drivers/Kinect2/BaseKinect2Stream.cpp index 85976905..9dcff74e 100755 --- a/Source/Drivers/Kinect2/BaseKinect2Stream.cpp +++ b/Source/Drivers/Kinect2/BaseKinect2Stream.cpp @@ -7,9 +7,10 @@ using namespace kinect2_device; BaseKinect2Stream::BaseKinect2Stream(Kinect2StreamImpl* pStreamImpl) : m_pStreamImpl(pStreamImpl) + , m_running(false) + , m_mirroring(false) + , m_frameIdx(0) { - m_running = false; - m_frameIdx = 0; m_cropping.enabled = FALSE; pStreamImpl->addStream(this); } @@ -77,6 +78,18 @@ OniStatus BaseKinect2Stream::getProperty(int propertyId, void* data, int* pDataS status = GetVideoMode((OniVideoMode*)data); } break; + case ONI_STREAM_PROPERTY_MIRRORING: + if (*pDataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniBool)); + status = ONI_STATUS_ERROR; + } + else + { + *(static_cast(data)) = m_mirroring; + status = ONI_STATUS_OK; + } + break; default: status = ONI_STATUS_NOT_SUPPORTED; break; @@ -106,6 +119,16 @@ OniStatus BaseKinect2Stream::setProperty(int propertyId, const void* data, int d } status = SetVideoMode((OniVideoMode*)data); } + else if (propertyId == ONI_STREAM_PROPERTY_MIRRORING) + { + if (dataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniBool)); + status = ONI_STATUS_ERROR; + } + m_mirroring = *(static_cast(data)) == TRUE; + status = ONI_STATUS_OK; + } return status; } @@ -118,6 +141,7 @@ OniBool BaseKinect2Stream::isPropertySupported(int propertyId) case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: case ONI_STREAM_PROPERTY_VERTICAL_FOV: case ONI_STREAM_PROPERTY_VIDEO_MODE: + case ONI_STREAM_PROPERTY_MIRRORING: status = TRUE; break; default: diff --git a/Source/Drivers/Kinect2/BaseKinect2Stream.h b/Source/Drivers/Kinect2/BaseKinect2Stream.h index 1d7fc541..a3e7a43c 100755 --- a/Source/Drivers/Kinect2/BaseKinect2Stream.h +++ b/Source/Drivers/Kinect2/BaseKinect2Stream.h @@ -34,6 +34,7 @@ namespace kinect2_device Kinect2StreamImpl *m_pStreamImpl; OniVideoMode m_videoMode; OniCropping m_cropping; + bool m_mirroring; bool m_running; int m_frameIdx; }; diff --git a/Source/Drivers/Kinect2/ColorKinect2Stream.cpp b/Source/Drivers/Kinect2/ColorKinect2Stream.cpp index 7a63354d..31cdbe70 100755 --- a/Source/Drivers/Kinect2/ColorKinect2Stream.cpp +++ b/Source/Drivers/Kinect2/ColorKinect2Stream.cpp @@ -51,13 +51,28 @@ void ColorKinect2Stream::frameReady(void* data, int width, int height, double ti const int frameY = pFrame->cropOriginY * yStride; const int frameWidth = pFrame->width * xStride; const int frameHeight = pFrame->height * yStride; - for (int y = frameY; y < frameY + frameHeight; y += yStride) { - for (int x = frameX; x < frameX + frameWidth; x += xStride) { - RGBQUAD* iter = data_in + (y*width + x); - data_out->b = iter->rgbBlue; - data_out->r = iter->rgbRed; - data_out->g = iter->rgbGreen; - data_out++; + if (!m_mirroring) + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX; x < frameX + frameWidth; x += xStride) { + RGBQUAD* iter = data_in + (y*width + x); + data_out->b = iter->rgbBlue; + data_out->r = iter->rgbRed; + data_out->g = iter->rgbGreen; + data_out++; + } + } + } + else + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX + frameWidth - 1; x >= frameX; x -= xStride) { + RGBQUAD* iter = data_in + (y*width + x); + data_out->b = iter->rgbBlue; + data_out->r = iter->rgbRed; + data_out->g = iter->rgbGreen; + data_out++; + } } } diff --git a/Source/Drivers/Kinect2/DepthKinect2Stream.cpp b/Source/Drivers/Kinect2/DepthKinect2Stream.cpp index 68c40b68..31ada71c 100755 --- a/Source/Drivers/Kinect2/DepthKinect2Stream.cpp +++ b/Source/Drivers/Kinect2/DepthKinect2Stream.cpp @@ -6,7 +6,8 @@ using namespace oni::driver; using namespace kinect2_device; #define DEFAULT_FPS 30 -#define DEVICE_MAX_DEPTH_VAL 10000 +#define DEVICE_MIN_DEPTH_VAL 500 +#define DEVICE_MAX_DEPTH_VAL 9500 #define FILTER_RELIABLE_DEPTH_VALUE(VALUE) (((VALUE) < DEVICE_MAX_DEPTH_VAL) ? (VALUE) : 0) DepthKinect2Stream::DepthKinect2Stream(Kinect2StreamImpl* pStreamImpl) @@ -69,6 +70,13 @@ OniStatus DepthKinect2Stream::getProperty(int propertyId, void* data, int* pData OniStatus status = ONI_STATUS_NOT_SUPPORTED; switch (propertyId) { + case ONI_STREAM_PROPERTY_MIN_VALUE: + { + XnInt * val = (XnInt *)data; + *val = DEVICE_MIN_DEPTH_VAL; + status = ONI_STATUS_OK; + break; + } case ONI_STREAM_PROPERTY_MAX_VALUE: { XnInt * val = (XnInt *)data; @@ -76,13 +84,6 @@ OniStatus DepthKinect2Stream::getProperty(int propertyId, void* data, int* pData status = ONI_STATUS_OK; break; } - case ONI_STREAM_PROPERTY_MIRRORING: - { - XnBool * val = (XnBool *)data; - *val = TRUE; - status = ONI_STATUS_OK; - break; - } default: status = BaseKinect2Stream::getProperty(propertyId, data, pDataSize); break; @@ -96,9 +97,8 @@ OniBool DepthKinect2Stream::isPropertySupported(int propertyId) OniBool status = FALSE; switch (propertyId) { + case ONI_STREAM_PROPERTY_MIN_VALUE: case ONI_STREAM_PROPERTY_MAX_VALUE: - case ONI_STREAM_PROPERTY_MIRRORING: - status = TRUE; default: status = BaseKinect2Stream::isPropertySupported(propertyId); break; @@ -129,11 +129,25 @@ void DepthKinect2Stream::copyDepthPixelsStraight(const UINT16* data_in, int widt const int frameHeight = pFrame->height * yStride; unsigned short* data_out = (unsigned short*) pFrame->data; - for (int y = frameY; y < frameY + frameHeight; y += yStride) { - for (int x = frameX; x < frameX + frameWidth; x += xStride) { - unsigned short* iter = const_cast(data_in + (y*width + x)); - *data_out = FILTER_RELIABLE_DEPTH_VALUE(*iter); - data_out++; + if (!m_mirroring) + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX; x < frameX + frameWidth; x += xStride) { + unsigned short* iter = const_cast(data_in + (y*width + x)); + *data_out = FILTER_RELIABLE_DEPTH_VALUE(*iter); + data_out++; + } + } + } + else + { + // to mirror the image, we run through every line backwards + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX + frameWidth - 1; x >= frameX; x -= xStride) { + unsigned short* iter = const_cast(data_in + (y*width + x)); + *data_out = FILTER_RELIABLE_DEPTH_VALUE(*iter); + data_out++; + } } } } @@ -168,44 +182,79 @@ void DepthKinect2Stream::copyDepthPixelsWithImageRegistration(const UINT16* data const ColorSpacePoint* mappedCoordsIter = m_colorSpaceCoords; for (int y = 0; y < height; y++) { for (int x = 0; x < width; x++) { - const float fX = mappedCoordsIter->X*xFactor; - const float fY = mappedCoordsIter->Y*yFactor; - const int cx = static_cast(fX + 0.5f); - const int cy = static_cast(fY + 0.5f); - if (cx >= 0 && cy >= 0 && cx < width && cy < height) { - unsigned short* iter = const_cast(data_in + (y*width + x)); - const unsigned short d = FILTER_RELIABLE_DEPTH_VALUE(*iter); - unsigned short* const p = data_out + cx + cy * width; - if (*p == 0 || *p > d) *p = d; + if ( mappedCoordsIter->X >= 0.f && mappedCoordsIter->X < 1920.0f + && mappedCoordsIter->Y >= 0.f && mappedCoordsIter->Y < 1080.0f) + { + const float fX = mappedCoordsIter->X*xFactor; + const float fY = mappedCoordsIter->Y*yFactor; + const int cx = static_cast(fX + 0.5f); + const int cy = static_cast(fY + 0.5f); + if (cx >= 0 && cy >= 0 && cx < width && cy < height) { + unsigned short* iter = const_cast(data_in + (y*width + x)); + const unsigned short d = FILTER_RELIABLE_DEPTH_VALUE(*iter); + unsigned short* const p = data_out + cx + cy * width; + if (*p == 0 || *p > d) *p = d; + } } - mappedCoordsIter++; + ++mappedCoordsIter; } } // Fill vertical gaps caused by the difference in the aspect ratio between depth and color resolutions data_out = (unsigned short*) pFrame->data; - for (int y = frameY; y < frameY + frameHeight; y += yStride) { - for (int x = frameX; x < frameX + frameWidth; x += xStride) { - unsigned short* iter = const_cast(m_registeredDepthMap + (y*width + x)); - if (*iter == 0) { - unsigned short davg = 0; - int dw = 0; - for (int ky = max(y - 1, 0); ky <= y + 1 && ky < height; ky++) { - unsigned short* kiter = const_cast(m_registeredDepthMap + (ky*width + x)); - if (*kiter != 0) { - davg += *kiter; - dw += abs(ky - y); + // we only need to handle mirroring in this second loop, because only here we actually write to the OpenNi frame + if (!m_mirroring) + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX; x < frameX + frameWidth; x += xStride) { + unsigned short* iter = const_cast(m_registeredDepthMap + (y*width + x)); + if (*iter == 0) { + unsigned short davg = 0; + int dw = 0; + for (int ky = max(y - 1, 0); ky <= y + 1 && ky < height; ky++) { + unsigned short* kiter = const_cast(m_registeredDepthMap + (ky*width + x)); + if (*kiter != 0) { + davg += *kiter; + dw += abs(ky - y); + } + } + *data_out = davg; + if (dw) { + *data_out /= dw; } } - *data_out = davg; - if (dw) { - *data_out /= dw; + else { + *data_out = FILTER_RELIABLE_DEPTH_VALUE(*iter); } + ++data_out; } - else { - *data_out = FILTER_RELIABLE_DEPTH_VALUE(*iter); + } + } + else + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX + frameWidth - 1; x >= frameX; x -= xStride) { + unsigned short* iter = const_cast(m_registeredDepthMap + (y*width + x)); + if (*iter == 0) { + unsigned short davg = 0; + int dw = 0; + for (int ky = max(y - 1, 0); ky <= y + 1 && ky < height; ky++) { + unsigned short* kiter = const_cast(m_registeredDepthMap + (ky*width + x)); + if (*kiter != 0) { + davg += *kiter; + dw += abs(ky - y); + } + } + *data_out = davg; + if (dw) { + *data_out /= dw; + } + } + else { + *data_out = FILTER_RELIABLE_DEPTH_VALUE(*iter); + } + ++data_out; } - data_out++; } } } diff --git a/Source/Drivers/Kinect2/IRKinect2Stream.cpp b/Source/Drivers/Kinect2/IRKinect2Stream.cpp index 3d06940e..b1c20193 100755 --- a/Source/Drivers/Kinect2/IRKinect2Stream.cpp +++ b/Source/Drivers/Kinect2/IRKinect2Stream.cpp @@ -51,11 +51,24 @@ void IRKinect2Stream::frameReady(void* data, int width, int height, double times const int frameY = pFrame->cropOriginY * yStride; const int frameWidth = pFrame->width * xStride; const int frameHeight = pFrame->height * yStride; - for (int y = frameY; y < frameY + frameHeight; y += yStride) { - for (int x = frameX; x < frameX + frameWidth; x += xStride) { - unsigned short* iter = data_in + (y*width + x); - *data_out = *iter; - data_out++; + if (!m_mirroring) + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX; x < frameX + frameWidth; x += xStride) { + unsigned short* iter = data_in + (y*width + x); + *data_out = *iter; + data_out++; + } + } + } + else + { + for (int y = frameY; y < frameY + frameHeight; y += yStride) { + for (int x = frameX + frameWidth - 1; x >= frameX; x -= xStride) { + unsigned short* iter = data_in + (y*width + x); + *data_out = *iter; + data_out++; + } } } diff --git a/Source/Drivers/Kinect2/Kinect2Driver.cpp b/Source/Drivers/Kinect2/Kinect2Driver.cpp index d440e9b1..a6ce76cc 100755 --- a/Source/Drivers/Kinect2/Kinect2Driver.cpp +++ b/Source/Drivers/Kinect2/Kinect2Driver.cpp @@ -40,11 +40,15 @@ OniStatus Kinect2Driver::initialize(DeviceConnectedCallback connectedCallback, } // Wait some time to let the sensor initialize - Sleep(500); - BOOLEAN available; - hr = pKinectSensor->get_IsAvailable(&available); - if (FAILED(hr) || !available) { + int count = 0; + while ((FAILED(pKinectSensor->get_IsAvailable(&available)) || !available) && count < 10) + { + Sleep(100); + ++count; + } + + if (!available) { pKinectSensor->Close(); pKinectSensor->Release(); return ONI_STATUS_NO_DEVICE; diff --git a/Source/Drivers/Kinect2/Kinect2StreamImpl.cpp b/Source/Drivers/Kinect2/Kinect2StreamImpl.cpp index d2e33e48..61b31e76 100755 --- a/Source/Drivers/Kinect2/Kinect2StreamImpl.cpp +++ b/Source/Drivers/Kinect2/Kinect2StreamImpl.cpp @@ -1,8 +1,6 @@ #include "Kinect2StreamImpl.h" #include "BaseKinect2Stream.h" -#include - using namespace oni::driver; using namespace kinect2_device; using namespace xnl; @@ -131,6 +129,11 @@ void Kinect2StreamImpl::mainLoop() m_running = TRUE; while (m_running) { int width, height; + + // wait for new frame available notification + HANDLE hEvents[] = { reinterpret_cast(m_hWaitable) }; + WaitForMultipleObjects(ARRAYSIZE(hEvents), hEvents, true, INFINITE); + void* data = populateFrameBuffer(width, height); LARGE_INTEGER qpc = {0}; @@ -318,6 +321,10 @@ void Kinect2StreamImpl::openFrameReader() m_pFrameReader.color->Release(); m_pFrameReader.color = NULL; } + else + { + m_pFrameReader.color->SubscribeFrameArrived(&m_hWaitable); + } } if (frameSource) { frameSource->Release(); @@ -332,6 +339,10 @@ void Kinect2StreamImpl::openFrameReader() m_pFrameReader.depth->Release(); m_pFrameReader.depth = NULL; } + else + { + m_pFrameReader.depth->SubscribeFrameArrived(&m_hWaitable); + } } if (frameSource) { frameSource->Release(); @@ -346,6 +357,10 @@ void Kinect2StreamImpl::openFrameReader() m_pFrameReader.infrared->Release(); m_pFrameReader.infrared = NULL; } + else + { + m_pFrameReader.infrared->SubscribeFrameArrived(&m_hWaitable); + } } if (frameSource) { frameSource->Release(); @@ -356,14 +371,17 @@ void Kinect2StreamImpl::openFrameReader() void Kinect2StreamImpl::closeFrameReader() { if (m_sensorType == ONI_SENSOR_COLOR && m_pFrameReader.color) { + m_pFrameReader.color->UnsubscribeFrameArrived(m_hWaitable); m_pFrameReader.color->Release(); m_pFrameReader.color = NULL; } else if (m_sensorType == ONI_SENSOR_DEPTH && m_pFrameReader.depth) { + m_pFrameReader.depth->UnsubscribeFrameArrived(m_hWaitable); m_pFrameReader.depth->Release(); m_pFrameReader.depth = NULL; } else if (m_pFrameReader.infrared) { // ONI_SENSOR_IR + m_pFrameReader.infrared->UnsubscribeFrameArrived(m_hWaitable); m_pFrameReader.infrared->Release(); m_pFrameReader.infrared = NULL; } @@ -376,71 +394,102 @@ void* Kinect2StreamImpl::populateFrameBuffer(int& buffWidth, int& buffHeight) if (m_sensorType == ONI_SENSOR_COLOR) { if (m_pFrameReader.color && m_pFrameBuffer.color) { - buffWidth = 1920; - buffHeight = 1080; - - IColorFrame* frame = NULL; - HRESULT hr = m_pFrameReader.color->AcquireLatestFrame(&frame); - if (SUCCEEDED(hr)) { - ColorImageFormat imageFormat = ColorImageFormat_None; - hr = frame->get_RawColorImageFormat(&imageFormat); - if (SUCCEEDED(hr)) { - if (imageFormat == ColorImageFormat_Bgra) { - RGBQUAD* data; - UINT bufferSize; - frame->AccessRawUnderlyingBuffer(&bufferSize, reinterpret_cast(&data)); - memcpy(m_pFrameBuffer.color, data, 1920*1080*sizeof(RGBQUAD)); + IColorFrameArrivedEventArgs* pColorArgs = nullptr; + HRESULT hr = m_pFrameReader.color->GetFrameArrivedEventData(m_hWaitable, &pColorArgs); + if (SUCCEEDED(hr)){ + // Reference + IColorFrameReference* pColorReference = nullptr; + hr = pColorArgs->get_FrameReference(&pColorReference); + if (SUCCEEDED(hr)){ + // Frame + IColorFrame* frame = nullptr; + hr = pColorReference->AcquireFrame(&frame); + if (SUCCEEDED(hr)) { + ColorImageFormat imageFormat = ColorImageFormat_None; + hr = frame->get_RawColorImageFormat(&imageFormat); + if (SUCCEEDED(hr)) { + buffWidth = 1920; + buffHeight = 1080; + + if (imageFormat == ColorImageFormat_Bgra) { + RGBQUAD* data; + UINT bufferSize; + frame->AccessRawUnderlyingBuffer(&bufferSize, reinterpret_cast(&data)); + memcpy(m_pFrameBuffer.color, data, buffWidth * buffHeight * sizeof(RGBQUAD)); + } + else { + frame->CopyConvertedFrameDataToArray(buffWidth * buffHeight * sizeof(RGBQUAD), reinterpret_cast(m_pFrameBuffer.color), ColorImageFormat_Bgra); + } + } } - else { - frame->CopyConvertedFrameDataToArray(1920*1080*sizeof(RGBQUAD), reinterpret_cast(m_pFrameBuffer.color), ColorImageFormat_Bgra); + if (frame) { + frame->Release(); } + + return reinterpret_cast(m_pFrameBuffer.color); } } - if (frame) { - frame->Release(); - } - - return reinterpret_cast(m_pFrameBuffer.color); } } else if (m_sensorType == ONI_SENSOR_DEPTH) { if (m_pFrameReader.depth && m_pFrameBuffer.depth) { - buffWidth = 512; - buffHeight = 424; - - IDepthFrame* frame = NULL; - HRESULT hr = m_pFrameReader.depth->AcquireLatestFrame(&frame); - if (SUCCEEDED(hr)) { - UINT16* data; - UINT bufferSize; - frame->AccessUnderlyingBuffer(&bufferSize, &data); - memcpy(m_pFrameBuffer.depth, data, 512*424*sizeof(UINT16)); - } - if (frame) { - frame->Release(); - } + IDepthFrameArrivedEventArgs * pArgs = nullptr; + HRESULT hr = m_pFrameReader.depth->GetFrameArrivedEventData(m_hWaitable, &pArgs); + if (SUCCEEDED(hr)){ + // Reference + IDepthFrameReference* pReference = nullptr; + hr = pArgs->get_FrameReference(&pReference); + if (SUCCEEDED(hr)){ + // Frame + IDepthFrame* frame = nullptr; + hr = pReference->AcquireFrame(&frame); + if (SUCCEEDED(hr)) { + buffWidth = 512; + buffHeight = 424; + TIMESPAN time; + frame->get_RelativeTime(&time); + UINT16* data; + UINT bufferSize; + frame->AccessUnderlyingBuffer(&bufferSize, &data); + memcpy(m_pFrameBuffer.depth, data, buffWidth * buffHeight * sizeof(UINT16)); + } + if (frame) { + frame->Release(); + } - return reinterpret_cast(m_pFrameBuffer.depth); + return reinterpret_cast(m_pFrameBuffer.depth); + } + } } } else { // ONI_SENSOR_IR if (m_pFrameReader.infrared && m_pFrameBuffer.infrared) { - buffWidth = 512; - buffHeight = 424; - - IInfraredFrame* frame = NULL; - HRESULT hr = m_pFrameReader.infrared->AcquireLatestFrame(&frame); - if (SUCCEEDED(hr)) { - UINT16* data; - UINT bufferSize; - frame->AccessUnderlyingBuffer(&bufferSize, &data); - memcpy(m_pFrameBuffer.infrared, data, 512*424*sizeof(UINT16)); - } - if (frame) { - frame->Release(); - } + IInfraredFrameArrivedEventArgs * pArgs = nullptr; + HRESULT hr = m_pFrameReader.infrared->GetFrameArrivedEventData(m_hWaitable, &pArgs); + if (SUCCEEDED(hr)){ + // Reference + IInfraredFrameReference* pReference = nullptr; + hr = pArgs->get_FrameReference(&pReference); + if (SUCCEEDED(hr)){ + // Frame + IInfraredFrame* frame = nullptr; + hr = pReference->AcquireFrame(&frame); + if (SUCCEEDED(hr)) { + buffWidth = 512; + buffHeight = 424; + + UINT16* data; + UINT bufferSize; + frame->AccessUnderlyingBuffer(&bufferSize, &data); + memcpy(m_pFrameBuffer.infrared, data, buffWidth * buffHeight * sizeof(UINT16)); + } + if (frame) { + frame->Release(); + } - return reinterpret_cast(m_pFrameBuffer.infrared); + return reinterpret_cast(m_pFrameBuffer.infrared); + } + } } } diff --git a/Source/Drivers/Kinect2/Kinect2StreamImpl.h b/Source/Drivers/Kinect2/Kinect2StreamImpl.h index aef7a776..9551b755 100755 --- a/Source/Drivers/Kinect2/Kinect2StreamImpl.h +++ b/Source/Drivers/Kinect2/Kinect2StreamImpl.h @@ -4,12 +4,7 @@ #include "BaseKinect2Stream.h" #include "XnList.h" -struct IKinectSensor; -struct ICoordinateMapper; -struct IFrameDescription; -struct IColorFrameReader; -struct IDepthFrameReader; -struct IInfraredFrameReader; +#include namespace kinect2_device { @@ -72,6 +67,8 @@ namespace kinect2_device OniVideoMode m_videoMode; xnl::List m_streamList; + WAITABLE_HANDLE m_hWaitable; + // Thread bool m_running; LONGLONG m_perfCounter;