diff --git a/NOTICE b/NOTICE index b8aa8b11..42de60bf 100644 --- a/NOTICE +++ b/NOTICE @@ -13,3 +13,6 @@ Your use of the library may involve use of the Microsoft Kinect for Windows Soft which is currently subject to the following license: http://www.microsoft.com/en-us/kinectforwindows/develop/sdk-eula.aspx +Your use of the library may involve use of the Intel Perceptual Computing SDK 2013, +which is currently subject to the following license: + http://software.intel.com/sites/products/vcsource/files/Intel_PCSDK_EULA.rtf diff --git a/OpenNI.sln b/OpenNI.sln index 9af176a6..59c1f7b4 100644 --- a/OpenNI.sln +++ b/OpenNI.sln @@ -120,6 +120,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PSLink", "Source\Drivers\PS {72D595BB-8C52-449B-91DB-0E9F6AEAF5BB} = {72D595BB-8C52-449B-91DB-0E9F6AEAF5BB} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PerCDevice", "Source\Drivers\PerCDevice\PerCDevice.vcxproj", "{D7E18C47-5E29-4D56-9C5B-57F7765F7B61}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -300,6 +302,14 @@ Global {5B74F010-8B79-46B5-B906-C2B56CDB3386}.Release|x64.Build.0 = Release|x64 {5B74F010-8B79-46B5-B906-C2B56CDB3386}.Release|x86.ActiveCfg = Release|Win32 {5B74F010-8B79-46B5-B906-C2B56CDB3386}.Release|x86.Build.0 = Release|Win32 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Debug|x64.ActiveCfg = Debug|x64 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Debug|x64.Build.0 = Debug|x64 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Debug|x86.ActiveCfg = Debug|Win32 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Debug|x86.Build.0 = Debug|Win32 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Release|x64.ActiveCfg = Release|x64 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Release|x64.Build.0 = Release|x64 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Release|x86.ActiveCfg = Release|Win32 + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE @@ -319,6 +329,7 @@ Global {15ECC029-90DE-4D1D-B00A-4A8E647D8C24} = {238D091D-1A85-4A61-9DCD-483768C51804} {E636BACA-795F-41CF-BC52-14C727BF014E} = {238D091D-1A85-4A61-9DCD-483768C51804} {5B74F010-8B79-46B5-B906-C2B56CDB3386} = {238D091D-1A85-4A61-9DCD-483768C51804} + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61} = {238D091D-1A85-4A61-9DCD-483768C51804} {BDA3BF24-550A-4BF9-83E5-0056134EED40} = {20285393-1DB1-4300-8AD3-30AEAE3C5DA6} {D39A4248-3985-41DE-AFD5-AEC58D29291F} = {20285393-1DB1-4300-8AD3-30AEAE3C5DA6} {D5709FB9-909D-415F-8F86-2F25BEF6CE23} = {20285393-1DB1-4300-8AD3-30AEAE3C5DA6} diff --git a/README b/README index c91e93f2..4128233d 100644 --- a/README +++ b/README @@ -11,6 +11,8 @@ Windows From: http://msdn.microsoft.com/en-us/vstudio/bb984878.aspx - Microsoft Kinect SDK v1.6 From: http://go.microsoft.com/fwlink/?LinkID=262831 +- Intel (R) Perceptual Computing SDK 2013 + From: http://registrationcenter-download.intel.com/akdlm/irc_nas/3581/intel_pc_sdk_websetup_11066.exe - Python 2.6+/3.x From: http://www.python.org/download/ - PyWin32 diff --git a/Samples/ClosestPointViewer/Viewer.cpp b/Samples/ClosestPointViewer/Viewer.cpp index 8c6e7e07..739d819a 100644 --- a/Samples/ClosestPointViewer/Viewer.cpp +++ b/Samples/ClosestPointViewer/Viewer.cpp @@ -158,7 +158,7 @@ void SampleViewer::display() for (int x = 0; x < width; ++x, ++pDepth, ++pTex) { - if (*pDepth != 0) + if ((*pDepth != 0) && (*pDepth < 10000)) { if (*pDepth == closest.Z) { diff --git a/Samples/Common/OniSampleUtilities.h b/Samples/Common/OniSampleUtilities.h index a036d9f9..5b58cb94 100644 --- a/Samples/Common/OniSampleUtilities.h +++ b/Samples/Common/OniSampleUtilities.h @@ -93,7 +93,7 @@ void calculateHistogram(float* pHistogram, int histogramSize, const openni::Vide { for (int x = 0; x < width; ++x, ++pDepth) { - if (*pDepth != 0) + if ((*pDepth != 0) && ((*pDepth) < histogramSize)) { pHistogram[*pDepth]++; nNumberOfPoints++; diff --git a/Samples/SimpleViewer/Viewer.cpp b/Samples/SimpleViewer/Viewer.cpp index aa754117..44373dbc 100644 --- a/Samples/SimpleViewer/Viewer.cpp +++ b/Samples/SimpleViewer/Viewer.cpp @@ -218,7 +218,7 @@ void SampleViewer::display() for (int x = 0; x < m_depthFrame.getWidth(); ++x, ++pDepth, ++pTex) { - if (*pDepth != 0) + if ((*pDepth != 0) && (*pDepth < MAX_DEPTH)) { int nHistValue = m_pDepthHist[*pDepth]; pTex->r = nHistValue; diff --git a/Source/Drivers/PerCDevice/PerCBaseStream.cpp b/Source/Drivers/PerCDevice/PerCBaseStream.cpp new file mode 100644 index 00000000..76e9ed40 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCBaseStream.cpp @@ -0,0 +1,194 @@ +#include "PerCDeviceEnumerator.h" +#include "PerCBaseStream.h" + +namespace perc_device +{ +PerCBaseStream::PerCBaseStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode) + : m_idxDeviceInternal(idxDeviceInternal) + , m_pxcDevice(NULL) + , m_idxStream(idxStream) + , m_pxcStream(NULL) + , m_running(false) + , m_mirror(false) + , m_videoMode(videoMode) +{ + m_cropping.enabled = 0; + createStream(); +} + +PerCBaseStream::~PerCBaseStream() +{ + stop(); +} + +OniStatus PerCBaseStream::start() +{ + if (m_running) + return ONI_STATUS_OK; + + if (!m_pxcStream.IsValid()) + return ONI_STATUS_ERROR; + + PXCCapture::VideoStream::ProfileInfo pinfo; + for (int idxProfile = 0; ; idxProfile++) + { + if (PXC_STATUS_NO_ERROR > m_pxcStream->QueryProfile(idxProfile, &pinfo)) + return ONI_STATUS_ERROR; + if ((m_videoMode.resolutionX == (int)pinfo.imageInfo.width) && (m_videoMode.resolutionY == (int)pinfo.imageInfo.height) && + (pinfo.frameRateMin.denominator * m_videoMode.fps >= pinfo.frameRateMin.numerator) && + (pinfo.frameRateMax.denominator * m_videoMode.fps <= pinfo.frameRateMax.numerator)) + break; + } + if (PXC_STATUS_NO_ERROR > m_pxcStream->SetProfile(&pinfo)) + return ONI_STATUS_ERROR; + + xnOSCreateThread(threadFunc, this, &m_threadHandle); + + return ONI_STATUS_OK; +} + +void PerCBaseStream::stop() +{ + if (!m_running) + return; + m_running = false; + if (m_threadHandle) + xnOSWaitForThreadExit(m_threadHandle, 10000); + + // Is there another way to stop stream? + // If we don't recreate device, a error may appears (or may not), then we change + // a stream profile on the running stream and then read a frame. + // Another way - set Sleep(1000) in start() after SetProfile() before start the thread + m_pxcStream.ReleaseRef(); + m_pxcDevice.ReleaseRef(); + createStream(); +} + +OniBool PerCBaseStream::isPropertySupported(int propertyId) +{ + return ((ONI_STREAM_PROPERTY_VIDEO_MODE == propertyId) || + (ONI_STREAM_PROPERTY_MIRRORING == propertyId) || + (ONI_STREAM_PROPERTY_CROPPING == propertyId)); +} +OniStatus PerCBaseStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + if (ONI_STREAM_PROPERTY_VIDEO_MODE == propertyId) + { + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, (int)sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + return GetVideoMode((OniVideoMode*)data); + } + else if (ONI_STREAM_PROPERTY_MIRRORING == propertyId) + { + if (*pDataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, (int)sizeof(OniBool)); + return ONI_STATUS_ERROR; + } + *(OniBool *)data = m_mirror; + return ONI_STATUS_OK; + } + else if (ONI_STREAM_PROPERTY_CROPPING == propertyId) + { + if (*pDataSize != sizeof(OniCropping)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, (int)sizeof(OniCropping)); + return ONI_STATUS_ERROR; + } + *(OniCropping *)data = m_cropping; + return ONI_STATUS_OK; + } + return ONI_STATUS_NOT_IMPLEMENTED; +} + +OniStatus PerCBaseStream::setProperty(int propertyId, const void* data, int dataSize) +{ + if (ONI_STREAM_PROPERTY_VIDEO_MODE == propertyId) + { + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, (int)sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + return SetVideoMode((OniVideoMode*)data); + } + else if (ONI_STREAM_PROPERTY_MIRRORING == propertyId) + { + if (dataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", dataSize, (int)sizeof(OniBool)); + return ONI_STATUS_ERROR; + } + m_mirror = *(const OniBool *)data; + return ONI_STATUS_OK; + } + else if (ONI_STREAM_PROPERTY_CROPPING == propertyId) + { + if (dataSize != sizeof(OniCropping)) + { + printf("Unexpected size: %d != %d\n", dataSize, (int)sizeof(OniCropping)); + return ONI_STATUS_ERROR; + } + m_cropping = *(OniCropping *)data; + return ONI_STATUS_OK; + } + return ONI_STATUS_NOT_IMPLEMENTED; +} + +OniBool PerCBaseStream::getFrameRect(int &left, int &top, int &right, int &bottom) +{ + left = 0; right = m_videoMode.resolutionX - 1; + top = 0; bottom = m_videoMode.resolutionY - 1; + if (0 == m_cropping.enabled) + return 1; + + top = min(m_cropping.originY, m_cropping.originY + m_cropping.height - 1); + bottom = max(m_cropping.originY, m_cropping.originY + m_cropping.height - 1); + if ((bottom < 0) || (m_videoMode.resolutionY <= top)) + return 0; + if (top < 0) + top = 0; + if (m_videoMode.resolutionY <= bottom) + bottom = m_videoMode.resolutionY - 1; + + left = min(m_cropping.originX, m_cropping.originX + m_cropping.width - 1); + right = max(m_cropping.originX, m_cropping.originX + m_cropping.width - 1); + if ((right < 0) || (m_videoMode.resolutionX <= left)) + return 0; + if (left < 0) + left = 0; + if (m_videoMode.resolutionX <= right) + right = m_videoMode.resolutionX - 1; + + return 1; +} + +void PerCBaseStream::createStream() +{ + m_pxcDevice = deviceEnumerator().getDevice(m_idxDeviceInternal); + if (!m_pxcDevice.IsValid()) + { + //TODO log + return; + } + if (PXC_STATUS_NO_ERROR > m_pxcDevice->CreateStream(m_idxStream, &m_pxcStream)) + { + //TODO log + return; + } +} + +XN_THREAD_PROC PerCBaseStream::threadFunc(XN_THREAD_PARAM pThreadParam) +{ + PerCBaseStream* pStream = (PerCBaseStream*)pThreadParam; + pStream->m_running = true; + pStream->Mainloop(); + + XN_THREAD_PROC_RETURN(XN_STATUS_OK); +} + + +}//namespace perc_device \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCBaseStream.h b/Source/Drivers/PerCDevice/PerCBaseStream.h new file mode 100644 index 00000000..906598a1 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCBaseStream.h @@ -0,0 +1,62 @@ +#ifndef _PERC_BASE_STREAM_H_ +#define _PERC_BASE_STREAM_H_ + +#include "Driver/OniDriverAPI.h" +#include "XnLib.h" +#include "XnHash.h" +#include "XnEvent.h" +#include "XnPlatform.h" + +#include "pxcsmartptr.h" +#include "pxccapture.h" + +namespace perc_device +{ +class PerCBaseStream + : public oni::driver::StreamBase +{ +public: + PerCBaseStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode); + virtual ~PerCBaseStream(); + + bool isValid() + { + return (m_pxcDevice.IsValid() && m_pxcStream.IsValid()); + } + + virtual OniStatus start(); + virtual void stop(); + + virtual OniStatus SetVideoMode(OniVideoMode*) = 0; + virtual OniStatus GetVideoMode(OniVideoMode* pVideoMode) = 0; + + virtual OniBool isPropertySupported(int /*propertyId*/); + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); + + virtual void Mainloop() = 0; +protected: + static XN_THREAD_PROC threadFunc(XN_THREAD_PARAM pThreadParam); +protected: + XnUInt32 m_idxDeviceInternal; + PXCSmartPtr m_pxcDevice; + XnUInt32 m_idxStream; + PXCSmartPtr m_pxcStream; + + void createStream(); + + bool m_running; + XN_THREAD_HANDLE m_threadHandle; +protected: + OniVideoMode m_videoMode; + + OniBool m_mirror; + OniCropping m_cropping; + OniBool getFrameRect(int &left, int &top, int &right, int &bottom); +private: + PerCBaseStream(const PerCBaseStream &); + PerCBaseStream &operator =(const PerCBaseStream &); +}; +}// namespace perc_device + +#endif //_PERC_BASE_STREAM_H_ \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCColorStream.cpp b/Source/Drivers/PerCDevice/PerCColorStream.cpp new file mode 100644 index 00000000..1f9586e8 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCColorStream.cpp @@ -0,0 +1,140 @@ +#include "PerCColorStream.h" + +namespace perc_device +{ +PerCColorStream::PerCColorStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode) + : PerCBaseStream(idxDeviceInternal, idxStream, videoMode) +{ +} + +OniStatus PerCColorStream::SetVideoMode(OniVideoMode* pVideoMode) +{ + if (m_running) + return ONI_STATUS_NOT_SUPPORTED; + m_videoMode.pixelFormat = pVideoMode->pixelFormat; + m_videoMode.fps = pVideoMode->fps; + m_videoMode.resolutionX = pVideoMode->resolutionX; + m_videoMode.resolutionY = pVideoMode->resolutionY; + return ONI_STATUS_OK; +} +OniStatus PerCColorStream::GetVideoMode(OniVideoMode* pVideoMode) +{ + pVideoMode->pixelFormat = m_videoMode.pixelFormat; + pVideoMode->fps = m_videoMode.fps; + pVideoMode->resolutionX = m_videoMode.resolutionX; + pVideoMode->resolutionY = m_videoMode.resolutionY; + return ONI_STATUS_OK; +} + +OniBool PerCColorStream::isPropertySupported(int propertyId) +{ + return PerCBaseStream::isPropertySupported(propertyId); +} + +OniStatus PerCColorStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + return PerCBaseStream::getProperty(propertyId, data, pDataSize); +} +OniStatus PerCColorStream::setProperty(int propertyId, const void* data, int dataSize) +{ + return PerCBaseStream::setProperty(propertyId, data, dataSize); +} + +void PerCColorStream::Mainloop() +{ + if (!m_pxcStream.IsValid()) + { + m_running = false; + return; + } + + int frameId = 1; + pxcU64 startTimeStamp = 0; + while (m_running) + { + PXCSmartPtr image; PXCSmartSP sp; + if (PXC_STATUS_NO_ERROR > m_pxcStream->ReadStreamAsync(&image, &sp)) + continue; + if (PXC_STATUS_NO_ERROR > sp->Synchronize()) + continue; + if (!image.IsValid()) + continue; + + int left, top, right, bottom; + if (0 == getFrameRect(left, top, right, bottom)) + continue; + + OniFrame* pFrame = getServices().acquireFrame(); + pFrame->frameIndex = frameId; + pFrame->videoMode = m_videoMode; + if (m_cropping.enabled) + { + pFrame->croppingEnabled = 1; + pFrame->cropOriginX = m_cropping.originX; + pFrame->cropOriginY = m_cropping.originY; + } + else + { + pFrame->croppingEnabled = 0; + pFrame->cropOriginX = 0; + pFrame->cropOriginY = 0; + } + + pFrame->sensorType = ONI_SENSOR_COLOR; + if (0 == startTimeStamp) + { + startTimeStamp = image->QueryTimeStamp(); + pFrame->timestamp = 0; + } + else + { + pFrame->timestamp = (uint64_t)((image->QueryTimeStamp() - startTimeStamp) / 1000000); + } + + pFrame->width = right - left + 1; + pFrame->height = bottom - top + 1; + pFrame->stride = 3 * pFrame->width; + + PXCImage::ImageData data; + image->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::COLOR_FORMAT_RGB24, &data); + //note: DepthSense throught PCSDK return color frame in RGB format + // we have to optimize this, if some change will appears in PCSDK + + if (m_mirror) + { + for (int row = 0; row < pFrame->height; row++) + { + pxcBYTE *pSrc = data.planes[0] + data.pitches[0] * (row + top) + 3 * (m_videoMode.resolutionX - 1 - left); + pxcBYTE *pDst = (pxcBYTE *)(pFrame->data) + pFrame->stride * row; + for (int col = 0; col < pFrame->width; col++, pSrc-=3, pDst+=3) + { + pDst[0] = pSrc[2]; // red + pDst[1] = pSrc[1]; // green + pDst[2] = pSrc[0]; // blue + } + } + } + else + { + for (int row = 0; row < pFrame->height; row++) + { + pxcBYTE *pSrc = data.planes[0] + data.pitches[0] * (row + top) + 3 * left; + pxcBYTE *pDst = (pxcBYTE *)(pFrame->data) + pFrame->stride * row; + for (int col = 0; col < pFrame->width; col++, pSrc+=3, pDst+=3) + { + pDst[0] = pSrc[2]; // red + pDst[1] = pSrc[1]; // green + pDst[2] = pSrc[0]; // blue + } + } + } + image->ReleaseAccess(&data); + + raiseNewFrame(pFrame); + getServices().releaseFrame(pFrame); + + frameId++; + } +} + +}// namespace perc_device diff --git a/Source/Drivers/PerCDevice/PerCColorStream.h b/Source/Drivers/PerCDevice/PerCColorStream.h new file mode 100644 index 00000000..3afe3270 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCColorStream.h @@ -0,0 +1,26 @@ +#ifndef _PERC_COLOR_STREAM_H_ +#define _PERC_COLOR_STREAM_H_ + +#include "PerCBaseStream.h" + +namespace perc_device +{ +class PerCColorStream + : public PerCBaseStream +{ +public: + PerCColorStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode); + + virtual OniStatus SetVideoMode(OniVideoMode* pVideoMode); + virtual OniStatus GetVideoMode(OniVideoMode* pVideoMode); + + virtual OniBool isPropertySupported(int propertyId); + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); +private: + void Mainloop(); +}; + +}// namespace perc_device + +#endif //_PERC_COLOR_STREAM_H_ \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDepthStream.cpp b/Source/Drivers/PerCDevice/PerCDepthStream.cpp new file mode 100644 index 00000000..d1d6bb5b --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDepthStream.cpp @@ -0,0 +1,169 @@ +#include "PerCDeviceEnumerator.h" +#include "PerCDepthStream.h" +#include "service\pxcsessionservice.h" + +namespace perc_device +{ +PerCDepthStream::PerCDepthStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode) + : PerCBaseStream(idxDeviceInternal, idxStream, videoMode) +{ + if (!m_pxcDevice.IsValid()) + return; + + PXCSession *session = deviceEnumerator().getSession(); + if (NULL == session) + return; + + //PXCSessionService *service = session->DynamicCast(); + //if (NULL != service) + //{ + // if (PXC_STATUS_NO_ERROR == service->LockSession()) + // { + // m_pxcDevice->SetProperty(PXCCapture::Device::PROPERTY_DEPTH_LOW_CONFIDENCE_VALUE, (pxcF32)-1); + // m_pxcDevice->SetProperty(PXCCapture::Device::PROPERTY_DEPTH_SATURATION_VALUE, (pxcF32)-1); + // service->UnlockSession(); + // } + //} + + pxcUID pid = 0; + m_pxcDevice->QueryPropertyAsUID(PXCCapture::Device::PROPERTY_PROJECTION_SERIALIZABLE, &pid); + session->DynamicCast()->CreateSerializable(pid, &m_pxcProjection); +} + +OniStatus PerCDepthStream::SetVideoMode(OniVideoMode* pVideoMode) +{ + if (m_running) + return ONI_STATUS_NOT_SUPPORTED; + m_videoMode.pixelFormat = pVideoMode->pixelFormat; + m_videoMode.fps = pVideoMode->fps; + m_videoMode.resolutionX = pVideoMode->resolutionX; + m_videoMode.resolutionY = pVideoMode->resolutionY; + return ONI_STATUS_OK; +} +OniStatus PerCDepthStream::GetVideoMode(OniVideoMode* pVideoMode) +{ + pVideoMode->pixelFormat = m_videoMode.pixelFormat; + pVideoMode->fps = m_videoMode.fps; + pVideoMode->resolutionX = m_videoMode.resolutionX; + pVideoMode->resolutionY = m_videoMode.resolutionY; + return ONI_STATUS_OK; +} + +OniStatus PerCDepthStream::convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) +{ + if (!m_pxcProjection.IsValid()) + return ONI_STATUS_NOT_SUPPORTED; + if ((0 == m_videoMode.resolutionX) || (0 == m_videoMode.resolutionY)) + return ONI_STATUS_ERROR; + if (NULL == colorStream) + return ONI_STATUS_BAD_PARAMETER; + + OniVideoMode videoMode; + int size = sizeof(videoMode); + if (ONI_STATUS_OK != colorStream->getProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, &size)) + return ONI_STATUS_ERROR; + + PXCPoint3DF32 posd = {(float)depthX, (float)depthY, (float)depthZ}; + PXCPointF32 posc; + m_pxcProjection->MapDepthToColorCoordinates(1, &posd, &posc); + + *pColorX = (int)(posc.x * (float)videoMode.resolutionX / (float)m_videoMode.resolutionX); + *pColorY = (int)(posc.y * (float)videoMode.resolutionY / (float)m_videoMode.resolutionY); + + return ONI_STATUS_OK; +} + +void PerCDepthStream::Mainloop() +{ + if (NULL == m_pxcStream) + { + m_running = false; + return; + } + + int frameId = 1; + pxcU64 startTimeStamp = 0; + while (m_running) + { + PXCSmartPtr image; PXCSmartSP sp; + if (PXC_STATUS_NO_ERROR > m_pxcStream->ReadStreamAsync(&image, &sp)) + continue; + if (PXC_STATUS_NO_ERROR > sp->Synchronize()) + continue; + if (!image.IsValid()) + continue; + + int left, top, right, bottom; + if (0 == getFrameRect(left, top, right, bottom)) + continue; + + PXCImage::ImageData data; + image->AcquireAccess(PXCImage::ACCESS_READ, &data); + if (PXCImage::COLOR_FORMAT_DEPTH != data.format) + { + image->ReleaseAccess(&data); + continue; + } + + OniFrame* pFrame = getServices().acquireFrame(); + pFrame->frameIndex = frameId; + pFrame->videoMode = m_videoMode; + if (m_cropping.enabled) + { + pFrame->croppingEnabled = 1; + pFrame->cropOriginX = m_cropping.originX; + pFrame->cropOriginY = m_cropping.originY; + } + else + { + pFrame->croppingEnabled = 0; + pFrame->cropOriginX = 0; + pFrame->cropOriginY = 0; + } + pFrame->sensorType = ONI_SENSOR_DEPTH; + if (0 == startTimeStamp) + { + startTimeStamp = image->QueryTimeStamp(); + pFrame->timestamp = 0; + } + else + { + pFrame->timestamp = (uint64_t)((image->QueryTimeStamp() - startTimeStamp) / 1000000); + } + + pFrame->width = right - left + 1; + pFrame->height = bottom - top + 1; + pFrame->stride = sizeof(short) * pFrame->width; + + if (m_mirror) + { + for (int row = 0; row < pFrame->height; row++) + { + pxcI16 *pSrc = (pxcI16 *)(data.planes[0] + data.pitches[0] * (row + top) + sizeof(short) * (m_videoMode.resolutionX - 1 - left)); + pxcI16 *pDst = (pxcI16 *)((pxcBYTE *)(pFrame->data) + pFrame->stride * row); + for (int col = 0; col < pFrame->width; col++, pSrc--, pDst++) + { + *pDst = *pSrc; + } + } + } + else + { + for (int row = 0; row < pFrame->height; row++) + { + pxcI16 *pSrc = (pxcI16 *)(data.planes[0] + data.pitches[0] * (row + top) + sizeof(short) * left); + pxcI16 *pDst = (pxcI16 *)((pxcBYTE *)(pFrame->data) + pFrame->stride * row); + memcpy(pDst, pSrc, pFrame->stride); + } + } + + image->ReleaseAccess(&data); + + raiseNewFrame(pFrame); + getServices().releaseFrame(pFrame); + + frameId++; + } +} + +} //namespace perc_device \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDepthStream.h b/Source/Drivers/PerCDevice/PerCDepthStream.h new file mode 100644 index 00000000..6cfafdb7 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDepthStream.h @@ -0,0 +1,27 @@ +#ifndef _PERC_DEPTH_STREAM_H_ +#define _PERC_DEPTH_STREAM_H_ + +#include "PerCBaseStream.h" +#include "pxcprojection.h" +#include "pxcmetadata.h" + +namespace perc_device +{ +class PerCDepthStream + : public PerCBaseStream +{ +public: + PerCDepthStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode); + + OniStatus SetVideoMode(OniVideoMode* pVideoMode); + OniStatus GetVideoMode(OniVideoMode* pVideoMode); + + virtual OniStatus convertDepthToColorCoordinates(StreamBase* /*colorStream*/, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY); +private: + PXCSmartPtr m_pxcProjection; +private: + void Mainloop(); +}; +}// namespace perc_device + +#endif //_PERC_DEPTH_STREAM_H_ \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDevice.cpp b/Source/Drivers/PerCDevice/PerCDevice.cpp new file mode 100644 index 00000000..41f8eeae --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDevice.cpp @@ -0,0 +1,336 @@ +#include "PerCDepthStream.h" +#include "PerCColorStream.h" +#include "PerCIRStream.h" +#include "PerCDevice.h" +#include "PerCDeviceEnumerator.h" + +#include "pxcsession.h" +#include "pxcsmartptr.h" +#include "pxccapture.h" + +namespace perc_device +{ +PerCDevice::PerCDevice(const char *uri, oni::driver::DriverServices& driverServices) + : m_driverServices(driverServices) + , m_idxDeviceInternal(deviceEnumerator().getDeviceIndex(uri)) +{ + fillStreamArray(); +} + +PerCDevice::~PerCDevice() +{ + clearSensorsList(); +} + + +OniStatus PerCDevice::getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) +{ + *numSensors = m_sensors.GetSize(); + *pSensors = m_sensors.GetData(); + return ONI_STATUS_OK; +} + +oni::driver::StreamBase* PerCDevice::createStream(OniSensorType sensorType) +{ + switch (sensorType) + { + case ONI_SENSOR_COLOR: + { + if (-1 == m_streamColor.idxStream) + break; + + PerCColorStream* pColor = XN_NEW(PerCColorStream, m_idxDeviceInternal, m_streamColor.idxStream, m_streamColor.videoModeDef); + if ((NULL != pColor) && (!pColor->isValid())) + { + XN_DELETE(pColor); + pColor = NULL; + } + return pColor; + } + case ONI_SENSOR_DEPTH: + { + if (-1 == m_streamDepth.idxStream) + break; + + PerCDepthStream* pDepth = XN_NEW(PerCDepthStream, m_idxDeviceInternal, m_streamDepth.idxStream, m_streamDepth.videoModeDef); + if ((NULL != pDepth) && (!pDepth->isValid())) + { + XN_DELETE(pDepth); + pDepth = NULL; + } + return pDepth; + } + case ONI_SENSOR_IR: + { + if (-1 == m_streamIR.idxStream) + break; + + PerCIRStream* pIR = XN_NEW(PerCIRStream, m_idxDeviceInternal, m_streamIR.idxStream, m_streamIR.videoModeDef); + if ((NULL != pIR) && (!pIR->isValid())) + { + XN_DELETE(pIR); + pIR = NULL; + } + return pIR; + } + } + + m_driverServices.errorLoggerAppend("PerCDevice: Can't create a stream of type %d", sensorType); + return NULL; +} + +void PerCDevice::destroyStream(oni::driver::StreamBase* pStream) +{ + XN_DELETE(pStream); +} + +OniStatus PerCDevice::getProperty(int propertyId, void* data, int* pDataSize) +{ + OniStatus rc = ONI_STATUS_OK; + propertyId; + data; + pDataSize; + + //switch (propertyId) + //{ + //case ONI_DEVICE_PROPERTY_DRIVER_VERSION: + // { + // if (*pDataSize == sizeof(OniVersion)) + // { + // OniVersion* version = (OniVersion*)data; + // version->major = version->minor = version->maintenance = version->build = 2; + // } + // else + // { + // m_driverServices.errorLoggerAppend("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVersion)); + // rc = ONI_STATUS_ERROR; + // } + // } + // break; + //default: + // m_driverServices.errorLoggerAppend("Unknown property: %d\n", propertyId); + // rc = ONI_STATUS_ERROR; + //} + return rc; +} + +bool PerCDevice::fillColorSensorVideoMode(OniSensorInfo &sensor, PXCCapture::Device *device, int idxStream) +{ + sensor.numSupportedVideoModes = 0; + sensor.pSupportedVideoModes = NULL; + if ((ONI_SENSOR_COLOR != sensor.sensorType) || (NULL == device)) + return false; + + xnl::Array modeSet; + + PXCSmartPtr vstream = 0; + if (PXC_STATUS_NO_ERROR > device->CreateStream(idxStream, &vstream)) + return false; + for (int idxProfile = 0; ;idxProfile++) + { + PXCCapture::VideoStream::ProfileInfo pinfo; + if (PXC_STATUS_NO_ERROR > vstream->QueryProfile(idxProfile, &pinfo)) + break; + + bool validMode = true; + OniVideoMode mode = {ONI_PIXEL_FORMAT_RGB888, 0, 0, 0};// make MSVS happy + switch (pinfo.imageInfo.format) + { + case PXCImage::COLOR_FORMAT_RGB32: + case PXCImage::COLOR_FORMAT_RGB24: + mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; + break; + case PXCImage::COLOR_FORMAT_YUY2: + case PXCImage::COLOR_FORMAT_NV12: + case PXCImage::COLOR_FORMAT_GRAY: + //TODO + validMode = false; + break; + default: + validMode = false; + break; + } + if (!validMode) + continue; + mode.resolutionX = pinfo.imageInfo.width; + mode.resolutionY = pinfo.imageInfo.height; + if (0 == pinfo.frameRateMin.denominator) + continue; + mode.fps = (int)(pinfo.frameRateMin.numerator / pinfo.frameRateMin.denominator); + + for (XnUInt32 idxMode = 0; idxMode < modeSet.GetSize(); idxMode++) + { + if ((mode.fps == modeSet[idxMode].fps) && + (mode.pixelFormat == modeSet[idxMode].pixelFormat) && + (mode.resolutionX == modeSet[idxMode].resolutionX) && + (mode.resolutionY == modeSet[idxMode].resolutionY)) + { + validMode = false; + break; + } + } + if (validMode) + modeSet.AddLast(mode); + } + + if (0 == modeSet.GetSize()) + return false; + + sensor.pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, modeSet.GetSize()); + if (NULL == sensor.pSupportedVideoModes) + return false; + sensor.numSupportedVideoModes = modeSet.GetSize(); + memcpy(sensor.pSupportedVideoModes, modeSet.GetData(), modeSet.GetSize() * sizeof(OniVideoMode)); + return true; +} + +bool PerCDevice::fillDepthSensorVideoMode(OniSensorInfo &sensor, PXCCapture::Device *device, int idxStream) +{ + sensor.numSupportedVideoModes = 0; + sensor.pSupportedVideoModes = NULL; + if ((ONI_SENSOR_DEPTH != sensor.sensorType) || (NULL == device)) + return false; + + pxcF32 fDepthUnit; + device->QueryProperty(PXCCapture::Device::PROPERTY_DEPTH_UNIT, &fDepthUnit); + OniPixelFormat pixFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + if ((99.f < fDepthUnit) && (101.f > fDepthUnit)) + pixFormat = ONI_PIXEL_FORMAT_DEPTH_100_UM; + else if ((999.f < fDepthUnit) && (1001.f > fDepthUnit)) + pixFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + else + { + m_driverServices.errorLoggerAppend("PerCDevice: the unit of depth values is invalid %f", fDepthUnit); + return false; + } + + xnl::Array modeSet; + + PXCSmartPtr vstream = 0; + if (PXC_STATUS_NO_ERROR > device->CreateStream(idxStream, &vstream)) + return false; + for (int idxProfile = 0; ;idxProfile++) + { + PXCCapture::VideoStream::ProfileInfo pinfo; + if (PXC_STATUS_NO_ERROR > vstream->QueryProfile(idxProfile, &pinfo)) + break; + + bool validMode = true; + OniVideoMode mode = {ONI_PIXEL_FORMAT_DEPTH_1_MM, 0, 0, 0};// make MSVS happy + switch (pinfo.imageInfo.format) + { + case PXCImage::COLOR_FORMAT_DEPTH: + mode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + break; + case PXCImage::COLOR_FORMAT_VERTICES: + default: + validMode = false; + break; + } + if (!validMode) + continue; + mode.resolutionX = pinfo.imageInfo.width; + mode.resolutionY = pinfo.imageInfo.height; + if (0 == pinfo.frameRateMin.denominator) + continue; + mode.fps = (int)(pinfo.frameRateMin.numerator / pinfo.frameRateMin.denominator); + + for (XnUInt32 idxMode = 0; idxMode < modeSet.GetSize(); idxMode++) + { + if ((mode.fps == modeSet[idxMode].fps) && + (mode.pixelFormat == modeSet[idxMode].pixelFormat) && + (mode.resolutionX == modeSet[idxMode].resolutionX) && + (mode.resolutionY == modeSet[idxMode].resolutionY)) + { + validMode = false; + break; + } + } + if (validMode) + modeSet.AddLast(mode); + } + + if (0 == modeSet.GetSize()) + return false; + + sensor.pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, modeSet.GetSize()); + if (NULL == sensor.pSupportedVideoModes) + return false; + sensor.numSupportedVideoModes = modeSet.GetSize(); + memcpy(sensor.pSupportedVideoModes, modeSet.GetData(), modeSet.GetSize() * sizeof(OniVideoMode)); + return true; +} + +void PerCDevice::fillStreamArray() +{ + m_sensors.Clear(); + PXCSmartPtr device = deviceEnumerator().getDevice(m_idxDeviceInternal); + if (!device.IsValid()) + { + m_driverServices.errorLoggerAppend("PerCDevice: device is invalid"); + return; + } + + m_streamColor.idxStream = m_streamDepth.idxStream = m_streamIR.idxStream = -1; + + for (int idxStream = 0; ;idxStream++) + { + PXCCapture::Device::StreamInfo sinfo; + if (PXC_STATUS_NO_ERROR > device->QueryStream(idxStream, &sinfo)) + break; + if (PXCCapture::VideoStream::CUID != sinfo.cuid) + continue; + m_sensors.SetSize(m_sensors.GetSize() + 1); + OniSensorInfo &sensor = m_sensors[m_sensors.GetSize() - 1]; + bool validSensor = false; + switch (sinfo.imageType) + { + case PXCImage::IMAGE_TYPE_COLOR: + sensor.sensorType = ONI_SENSOR_COLOR; + validSensor = fillColorSensorVideoMode(sensor, device, idxStream); + if ((-1 == m_streamColor.idxStream) && validSensor) + { + m_streamColor.idxStream = idxStream; + m_streamColor.videoModeDef = sensor.pSupportedVideoModes[0]; + } + break; + case PXCImage::IMAGE_TYPE_DEPTH: + sensor.sensorType = ONI_SENSOR_DEPTH; + validSensor = fillDepthSensorVideoMode(sensor, device, idxStream); + if (validSensor) + { + if (-1 == m_streamDepth.idxStream) + { + m_streamDepth.idxStream = idxStream; + m_streamDepth.videoModeDef = sensor.pSupportedVideoModes[0]; + } + else if (-1 == m_streamIR.idxStream) + { + sensor.sensorType = ONI_SENSOR_IR; + for (int idxMode = 0; idxMode < sensor.numSupportedVideoModes; idxMode++) + { + sensor.pSupportedVideoModes[idxMode].pixelFormat = ONI_PIXEL_FORMAT_GRAY16; + } + m_streamIR.idxStream = idxStream; + m_streamIR.videoModeDef = sensor.pSupportedVideoModes[0]; + } + } + break; + default: + break; + } + if (!validSensor) + m_sensors.SetSize(m_sensors.GetSize() - 1); + } +} + +void PerCDevice::clearSensorsList() +{ + for(XnUInt32 i = 0; i < m_sensors.GetSize(); ++i) + { + XN_DELETE_ARR(m_sensors[i].pSupportedVideoModes); + } + m_sensors.Clear(); +} + +}//namespace perc_device \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDevice.h b/Source/Drivers/PerCDevice/PerCDevice.h new file mode 100644 index 00000000..ab6c9b71 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDevice.h @@ -0,0 +1,58 @@ +#ifndef _PERC_DEVICE_H_ +#define _PERC_DEVICE_H_ + +#include "Driver/OniDriverAPI.h" +#include "XnLib.h" +#include "XnArray.h" + +#include "pxccapture.h" + + +namespace perc_device +{ +class PerCDevice + : public oni::driver::DeviceBase +{ +private: + PerCDevice(const PerCDevice&); + void operator=(const PerCDevice&); +public: + PerCDevice(const char *uri, oni::driver::DriverServices& driverServices); + ~PerCDevice(); + + bool isValid() const + { + return ((XnUInt32)-1 != m_idxDeviceInternal); + } +public: + OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors); + + oni::driver::StreamBase* createStream(OniSensorType sensorType); + void destroyStream(oni::driver::StreamBase* pStream); + + OniStatus getProperty(int propertyId, void* data, int* pDataSize); +private: + oni::driver::DriverServices& m_driverServices; + + xnl::Array m_sensors; + + XnUInt32 m_idxDeviceInternal; + + struct SensorSetting + { + int idxStream; + OniVideoMode videoModeDef; + }; + SensorSetting m_streamColor; + SensorSetting m_streamDepth; + SensorSetting m_streamIR; + + bool fillColorSensorVideoMode(OniSensorInfo &sensor, PXCCapture::Device *device, int idxStream); + bool fillDepthSensorVideoMode(OniSensorInfo &sensor, PXCCapture::Device *device, int idxStream); + + void fillStreamArray(); + void clearSensorsList(); +}; +}// namespace perc_device + +#endif //_PERC_DEVICE_H_ \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDevice.vcxproj b/Source/Drivers/PerCDevice/PerCDevice.vcxproj new file mode 100644 index 00000000..47b5d10d --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDevice.vcxproj @@ -0,0 +1,218 @@ + + + + + Debug + Win32 + + + Debug + x64 + + + Release + Win32 + + + Release + x64 + + + + {D7E18C47-5E29-4D56-9C5B-57F7765F7B61} + PerCDevice + + + + DynamicLibrary + true + MultiByte + v100 + + + DynamicLibrary + true + Unicode + v100 + + + DynamicLibrary + false + true + MultiByte + v100 + + + DynamicLibrary + false + true + Unicode + v100 + + + + + + + + + + + + + + + + + + + $(SolutionDir)Bin\$(Platform)-$(Configuration)\OpenNI2\Drivers\ + $(SolutionDir)Bin\Intermediate\$(Platform)-$(Configuration)\$(ProjectName)\ + + + $(SolutionDir)Bin\$(Platform)-$(Configuration)\OpenNI2\Drivers\ + $(SolutionDir)Bin\Intermediate\$(Platform)-$(Configuration)\$(ProjectName)\ + + + $(SolutionDir)Bin\$(Platform)-$(Configuration)\OpenNI2\Drivers\ + $(SolutionDir)Bin\Intermediate\$(Platform)-$(Configuration)\$(ProjectName)\ + + + $(SolutionDir)Bin\$(Platform)-$(Configuration)\OpenNI2\Drivers\ + $(SolutionDir)Bin\Intermediate\$(Platform)-$(Configuration)\$(ProjectName)\ + + + + Disabled + ..\..\..\Include;..\..\..\ThirdParty\PSCommon\XnLib\Include;$(PCSDK_DIR)/include + _WINDLL;%(PreprocessorDefinitions);PerCDevice2_EXPORT + ProgramDatabase + Level4 + true + true + + + true + $(SolutionDir)Bin\$(Platform)-$(Configuration)\;$(PCSDK_DIR)/lib/$(PlatformName) + XnLib.lib;libpxc_d.lib;%(AdditionalDependencies) + true + LIBCMTD + + + ..\..\..\Include + + + + + Disabled + ..\..\..\Include;..\..\..\ThirdParty\PSCommon\XnLib\Include;$(PCSDK_DIR)/include + WIN32;_DEBUG;_WINDOWS;_USRDLL;PERCDEVICE_EXPORT + ProgramDatabase + Level4 + true + true + + + + true + $(SolutionDir)Bin\$(Platform)-$(Configuration)\;$(PCSDK_DIR)/lib/$(PlatformName) + XnLib.lib;libpxc_d.lib;%(AdditionalDependencies) + true + LIBCMTD + + + ..\..\..\Include + + + + + Level4 + MaxSpeed + true + _MBCS;%(PreprocessorDefinitions);PerCDevice2_EXPORT + ..\..\..\Include;..\..\..\ThirdParty\PSCommon\XnLib\Include;$(PCSDK_DIR)/include + true + true + AnySuitable + Speed + true + true + false + StreamingSIMDExtensions2 + Fast + + + true + true + true + XnLib.lib;libpxc.lib;%(AdditionalDependencies) + $(SolutionDir)Bin\$(Platform)-$(Configuration)\;$(PCSDK_DIR)/lib/$(PlatformName) + true + LIBCMT + + + ..\..\..\Include + + + + + Level4 + MaxSpeed + true + _MBCS;%(PreprocessorDefinitions);PerCDevice2_EXPORT + ..\..\..\Include;..\..\..\ThirdParty\PSCommon\XnLib\Include;$(PCSDK_DIR)/include + true + true + AnySuitable + Speed + true + true + false + StreamingSIMDExtensions2 + Fast + + + true + true + true + XnLib.lib;libpxc.lib;%(AdditionalDependencies) + $(SolutionDir)Bin\$(Platform)-$(Configuration)\;$(PCSDK_DIR)/lib/$(PlatformName) + true + LIBCMT + + + ..\..\..\Include + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDevice.vcxproj.filters b/Source/Drivers/PerCDevice/PerCDevice.vcxproj.filters new file mode 100644 index 00000000..7fe7f8cc --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDevice.vcxproj.filters @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDeviceEnumerator.cpp b/Source/Drivers/PerCDevice/PerCDeviceEnumerator.cpp new file mode 100644 index 00000000..4e6c0134 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDeviceEnumerator.cpp @@ -0,0 +1,125 @@ +#include +#include "PerCDeviceEnumerator.h" + + +namespace perc_device +{ +PerCDeviceEnumerator::PerCDeviceEnumerator() +{ + pxcStatus sts = PXCSession_Create(&m_session); + if (PXC_STATUS_NO_ERROR > sts) + return; + + enumDevices(); +} + +PerCDeviceEnumerator::~PerCDeviceEnumerator() +{ +} + +XnUInt32 PerCDeviceEnumerator::getDevicesCount() +{ + return m_deviceInfo.GetSize(); +} +size_t PerCDeviceEnumerator::getDeviceURI(XnUInt32 idxDevice, char *uri, size_t uriSize) +{ + if (idxDevice >= m_deviceInfo.GetSize()) + return 0; + return getDeviceURI(getDevice(m_deviceInfo[idxDevice]), uri, uriSize); +} + +size_t PerCDeviceEnumerator::getDeviceURI(PXCCapture::Device *device, char *uri, size_t uriSize) +{ + if (NULL == device) + return 0; + + PXCCapture::DeviceInfo dinfo; + if (PXC_STATUS_NO_ERROR > device->QueryDevice(&dinfo)) + return 0; + + size_t convertedChars = 0; + wcstombs_s(&convertedChars, uri, uriSize, dinfo.did, _TRUNCATE); + return convertedChars; +} + +XnUInt32 PerCDeviceEnumerator::getDeviceIndex(const char *uri) +{ + XnUInt32 idx = (XnUInt32)-1; + if (XN_STATUS_OK != m_devicesUri.Get(uri, idx)) + return (XnUInt32)-1; + return idx; +} + + +PXCCapture::Device *PerCDeviceEnumerator::getDevice(XnUInt32 idxDevice) +{ + if (idxDevice >= m_deviceInfo.GetSize()) + return NULL; + return getDevice(m_deviceInfo[idxDevice]); +} + +PXCCapture::Device *PerCDeviceEnumerator::getDevice(const DeviceInfo &deviceInfo) +{ + if (NULL == m_session) + return NULL; + + PXCSmartPtr capture; + if (PXC_STATUS_NO_ERROR > m_session->CreateImpl((PXCSession::ImplDesc *)&deviceInfo.descModule, &capture)) + return NULL; + + PXCSmartPtr device; + if (PXC_STATUS_NO_ERROR > capture->CreateDevice(deviceInfo.idxDevice, &device)) + return NULL; + return device.ReleasePtr(); +} + +void PerCDeviceEnumerator::enumDevices(PXCSession::ImplDesc &descModule) +{ + if (NULL == m_session) + return; + + PXCSmartPtr capture; + if (PXC_STATUS_NO_ERROR > m_session->CreateImpl(&descModule, &capture)) + return; + + static char uri[ONI_MAX_STR]; + for (int idxDevice = 0; ; idxDevice++) + { + PXCSmartPtr device = NULL; + if (PXC_STATUS_NO_ERROR > capture->CreateDevice(idxDevice, &device)) + break; + + DeviceInfo info = {descModule, idxDevice}; + getDeviceURI(device, uri, ONI_MAX_STR); + m_devicesUri[uri] = m_deviceInfo.GetSize(); + m_deviceInfo.AddLast(info); + } +} + +void PerCDeviceEnumerator::enumDevices() +{ + PXCSession::ImplDesc descTemplate; + memset(&descTemplate, 0, sizeof(descTemplate)); + descTemplate.group = PXCSession::IMPL_GROUP_SENSOR; + descTemplate.subgroup = PXCSession::IMPL_SUBGROUP_VIDEO_CAPTURE; + for (int idxModule = 0; ; idxModule++) + { + PXCSession::ImplDesc desc; + if (PXC_STATUS_NO_ERROR > m_session->QueryImpl(&descTemplate, idxModule, &desc)) + break; + + enumDevices(desc); + } +} + +PerCDeviceEnumerator &PerCDeviceEnumerator::getPerCDeviceEnumerator() +{ + static PerCDeviceEnumerator enumerator; + return enumerator; +} + +PerCDeviceEnumerator &deviceEnumerator() +{ + return PerCDeviceEnumerator::getPerCDeviceEnumerator(); +} +}; \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDeviceEnumerator.h b/Source/Drivers/PerCDevice/PerCDeviceEnumerator.h new file mode 100644 index 00000000..1f5d751c --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDeviceEnumerator.h @@ -0,0 +1,61 @@ +#ifndef _PERC_DEVICE_ENUMERATOR_H_ +#define _PERC_DEVICE_ENUMERATOR_H_ + +#include + +#include "Driver/OniDriverAPI.h" +#include "XnLib.h" +#include "XnHash.h" +#include "XnArray.h" +#include "XnPlatform.h" + +#include "pxcsession.h" +#include "pxcsmartptr.h" +#include "pxccapture.h" + +namespace perc_device +{ + +class PerCDeviceEnumerator +{ +public: + PXCSession *getSession(){ return m_session;} + + XnUInt32 getDevicesCount(); + size_t getDeviceURI(XnUInt32 idxDevice, char *uri, size_t uriSize); + PXCCapture::Device *getDevice(XnUInt32 idxDevice); + XnUInt32 getDeviceIndex(const char *uri); +private: + PXCSmartPtr m_session; + + struct DeviceInfo + { + PXCSession::ImplDesc descModule; + XnUInt32 idxDevice; + }; + xnl::Array m_deviceInfo; + xnl::StringsHash m_devicesUri; + + PXCCapture::Device *getDevice(const DeviceInfo &deviceInfo); + + void enumDevices(PXCSession::ImplDesc &descModule); + void enumDevices(); +private: + PerCDeviceEnumerator(); + ~PerCDeviceEnumerator(); + + PerCDeviceEnumerator(const PerCDeviceEnumerator&); + PerCDeviceEnumerator& operator =(const PerCDeviceEnumerator&); +private: + static size_t getDeviceURI(PXCCapture::Device *device, char *uri, size_t uriSize); +public: + static PerCDeviceEnumerator &getPerCDeviceEnumerator(); +}; + +PerCDeviceEnumerator &deviceEnumerator(); + +}//namespace perc_device + +#endif //_PERC_DEVICE_ENUMERATOR_H_ + + diff --git a/Source/Drivers/PerCDevice/PerCDriver.cpp b/Source/Drivers/PerCDevice/PerCDriver.cpp new file mode 100644 index 00000000..0e5e0bdb --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDriver.cpp @@ -0,0 +1,103 @@ +#include "PerCDevice.h" +#include "PerCDriver.h" +#include "PerCDeviceEnumerator.h" + +namespace perc_device +{ + +static const char VENDOR_VAL[] = "Intel"; +static const char NAME_VAL[] = "PerCDevice"; + +PerCDriver::PerCDriver(OniDriverServices* pDriverServices) + : DriverBase(pDriverServices) +{ +} + +PerCDriver::~PerCDriver() +{ + clearDevices(); +} + +OniStatus PerCDriver::initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) +{ + OniStatus ret = DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + if (ONI_STATUS_OK != ret) + { + return (ret); + } + + XnUInt32 count = deviceEnumerator().getDevicesCount(); + for (XnUInt32 idx = 0; idx < count; idx++) + { + OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); + + deviceEnumerator().getDeviceURI(idx, pInfo->uri, ONI_MAX_STR); + //xnOSStrCopy(pInfo->uri, uri.Data(), ONI_MAX_STR); + xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); + xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); + + deviceConnected(pInfo); + deviceStateChanged(pInfo, 0); + } + return ONI_STATUS_OK; +} + +oni::driver::DeviceBase* PerCDriver::deviceOpen(const char* uri, const char* /*mode*/) +{ + PerCDevice *pDevice = NULL; + + if (m_devices.Get(uri, pDevice) == XN_STATUS_OK) + { + getServices().errorLoggerAppend("Device is already open."); + return NULL; + } + + pDevice = XN_NEW(PerCDevice, uri, getServices()); + if (NULL == pDevice) + return NULL; + if (!pDevice->isValid()) + { + XN_DELETE(pDevice); + return NULL; + } + + // Add the device and return it. + m_devices[uri] = pDevice; + return pDevice; +} + +void PerCDriver::deviceClose(oni::driver::DeviceBase* pDevice) +{ + for (xnl::StringsHash::Iterator iter = m_devices.Begin(); iter != m_devices.End(); ++iter) + { + if (iter->Value() == pDevice) + { + m_devices.Remove(iter); + XN_DELETE(pDevice); + return; + } + } + + // not our device?! + XN_ASSERT(FALSE); +} + +void PerCDriver::shutdown() +{ + // Close all open devices and release the memory + clearDevices(); +} + +void PerCDriver::clearDevices() +{ + for (xnl::StringsHash::Iterator it = m_devices.Begin(); it != m_devices.End(); ++it) + { + XN_DELETE(it->Value()); + } + + m_devices.Clear(); +} + +}//namespace perc_device + +ONI_EXPORT_DRIVER(perc_device::PerCDriver); \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCDriver.h b/Source/Drivers/PerCDevice/PerCDriver.h new file mode 100644 index 00000000..98c537a7 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCDriver.h @@ -0,0 +1,37 @@ +#ifndef _PERC_DRIVER_H_ +#define _PERC_DRIVER_H_ + +#include "Driver/OniDriverAPI.h" +#include "XnLib.h" +#include "XnHash.h" +#include "XnPlatform.h" + +#include "pxcsession.h" +#include "pxcsmartptr.h" +#include "pxccapture.h" + +namespace perc_device +{ + +class PerCDriver + : public oni::driver::DriverBase +{ +public: + PerCDriver(OniDriverServices* pDriverServices); + ~PerCDriver(); + + virtual OniStatus initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie); + + virtual oni::driver::DeviceBase* deviceOpen(const char* uri, const char* /*mode*/); + virtual void deviceClose(oni::driver::DeviceBase* pDevice); + + virtual void shutdown(); +protected: + XN_THREAD_HANDLE m_threadHandle; + xnl::StringsHash m_devices; + void clearDevices(); +}; + +}//namespace perc_device + +#endif //_PERC_DRIVER_H_ \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCIRStream.cpp b/Source/Drivers/PerCDevice/PerCIRStream.cpp new file mode 100644 index 00000000..4ab48360 --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCIRStream.cpp @@ -0,0 +1,122 @@ +#include "PerCIRStream.h" + +namespace perc_device +{ +PerCIRStream::PerCIRStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode) + : PerCBaseStream(idxDeviceInternal, idxStream, videoMode) +{ +} + +OniStatus PerCIRStream::SetVideoMode(OniVideoMode *pVideoMode) +{ + if (m_running) + return ONI_STATUS_NOT_SUPPORTED; + m_videoMode.pixelFormat = pVideoMode->pixelFormat; + m_videoMode.fps = pVideoMode->fps; + m_videoMode.resolutionX = pVideoMode->resolutionX; + m_videoMode.resolutionY = pVideoMode->resolutionY; + return ONI_STATUS_OK; +} +OniStatus PerCIRStream::GetVideoMode(OniVideoMode* pVideoMode) +{ + pVideoMode->pixelFormat = m_videoMode.pixelFormat; + pVideoMode->fps = m_videoMode.fps; + pVideoMode->resolutionX = m_videoMode.resolutionX; + pVideoMode->resolutionY = m_videoMode.resolutionY; + return ONI_STATUS_OK; +} + +void PerCIRStream::Mainloop() +{ + if (NULL == m_pxcStream) + { + m_running = false; + return; + } + + int frameId = 1; + pxcU64 startTimeStamp = 0; + while (m_running) + { + PXCSmartPtr image; PXCSmartSP sp; + if (PXC_STATUS_NO_ERROR > m_pxcStream->ReadStreamAsync(&image, &sp)) + continue; + if (PXC_STATUS_NO_ERROR > sp->Synchronize()) + continue; + if (!image.IsValid()) + continue; + + int left, top, right, bottom; + if (0 == getFrameRect(left, top, right, bottom)) + continue; + + PXCImage::ImageData data; + image->AcquireAccess(PXCImage::ACCESS_READ, &data); + if (PXCImage::COLOR_FORMAT_DEPTH != data.format) + { + image->ReleaseAccess(&data); + continue; + } + + OniFrame* pFrame = getServices().acquireFrame(); + pFrame->frameIndex = frameId; + pFrame->videoMode = m_videoMode; + if (m_cropping.enabled) + { + pFrame->croppingEnabled = 1; + pFrame->cropOriginX = m_cropping.originX; + pFrame->cropOriginY = m_cropping.originY; + } + else + { + pFrame->croppingEnabled = 0; + pFrame->cropOriginX = 0; + pFrame->cropOriginY = 0; + } + pFrame->sensorType = ONI_SENSOR_DEPTH; + if (0 == startTimeStamp) + { + startTimeStamp = image->QueryTimeStamp(); + pFrame->timestamp = 0; + } + else + { + pFrame->timestamp = (uint64_t)((image->QueryTimeStamp() - startTimeStamp) / 1000000); + } + + pFrame->width = right - left + 1; + pFrame->height = bottom - top + 1; + pFrame->stride = sizeof(short) * pFrame->width; + + if (m_mirror) + { + for (int row = 0; row < pFrame->height; row++) + { + pxcI16 *pSrc = (pxcI16 *)(data.planes[1] + data.pitches[1] * (row + top) + sizeof(short) * (m_videoMode.resolutionX - 1 - left)); + pxcI16 *pDst = (pxcI16 *)((pxcBYTE *)(pFrame->data) + pFrame->stride * row); + for (int col = 0; col < pFrame->width; col++, pSrc--, pDst++) + { + *pDst = *pSrc; + } + } + } + else + { + for (int row = 0; row < pFrame->height; row++) + { + pxcI16 *pSrc = (pxcI16 *)(data.planes[1] + data.pitches[1] * (row + top) + sizeof(short) * left); + pxcI16 *pDst = (pxcI16 *)((pxcBYTE *)(pFrame->data) + pFrame->stride * row); + memcpy(pDst, pSrc, pFrame->stride); + } + } + + image->ReleaseAccess(&data); + + raiseNewFrame(pFrame); + getServices().releaseFrame(pFrame); + + frameId++; + } +} + +} //namespace perc_device \ No newline at end of file diff --git a/Source/Drivers/PerCDevice/PerCIRStream.h b/Source/Drivers/PerCDevice/PerCIRStream.h new file mode 100644 index 00000000..84d264ab --- /dev/null +++ b/Source/Drivers/PerCDevice/PerCIRStream.h @@ -0,0 +1,21 @@ +#ifndef _PERC_IR_STREAM_H_ +#define _PERC_IR_STREAM_H_ + +#include "PerCBaseStream.h" + +namespace perc_device +{ +class PerCIRStream + : public PerCBaseStream +{ +public: + PerCIRStream(XnUInt32 idxDeviceInternal, XnUInt32 idxStream, const OniVideoMode &videoMode); + + OniStatus SetVideoMode(OniVideoMode* pVideoMode); + OniStatus GetVideoMode(OniVideoMode* pVideoMode); +private: + void Mainloop(); +}; +}// namespace perc_device + +#endif //_PERC_IR_STREAM_H_ \ No newline at end of file