void storeNuiDepth(void)
{
NUI_IMAGE_FRAME depthFrame;
if(WAIT_OBJECT_0 != WaitForSingleObject(hNextDepthFrameEvent, 0)) return;
HRESULT hr = pNuiSensor->NuiImageStreamGetNextFrame(
pDepthStreamHandle,
0,
&depthFrame );
if( FAILED( hr ) ){
return;
}
if(depthFrame.eImageType != NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX)
STDERR("Depth type is not match with the depth and players\r\n");
INuiFrameTexture *pTexture = depthFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 ){
unsigned short *pBuffer = (unsigned short *)LockedRect.pBits;
memcpy(depth, LockedRect.pBits, pTexture->BufferLen());
#if defined(USE_FACETRACKER)
setDepthImage(LockedRect.pBits, LockedRect.size);
#endif
NUI_SURFACE_DESC pDesc;
pTexture->GetLevelDesc(0, &pDesc);
//printf("w: %d, h: %d, byte/pixel: %d\r\n", pDesc.Width, pDesc.Height, LockedRect.Pitch/pDesc.Width);
unsigned short *p = (unsigned short *)pBuffer;
for(int i=0;i<pTexture->BufferLen()/2;i++){
//*p = (unsigned short)((*p & 0xff00)>>8) | ((*p & 0x00ff)<<8); // for test
//*p = (unsigned short)((*p & 0xfff8)>>3);
*p = (unsigned short)(NuiDepthPixelToDepth(*pBuffer));
p++;
}
glBindTexture(GL_TEXTURE_2D, bg_texture[DEPTH_TEXTURE]);
glPixelStorei(GL_UNPACK_ALIGNMENT, 2);
glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE,
pDesc.Width, pDesc.Height,
0, GL_LUMINANCE, GL_UNSIGNED_SHORT, pBuffer);
pTexture->UnlockRect(0);
}
else{
STDERR("Buffer length of received texture is bogus\r\n");
}
pNuiSensor->NuiImageStreamReleaseFrame( pDepthStreamHandle, &depthFrame );
}
void storeNuiImage(void)
{
NUI_IMAGE_FRAME imageFrame;
if(WAIT_OBJECT_0 != WaitForSingleObject(hNextColorFrameEvent, 0)) return;
HRESULT hr = pNuiSensor->NuiImageStreamGetNextFrame(
pVideoStreamHandle,
0,
&imageFrame );
if( FAILED( hr ) ){
return;
}
if(imageFrame.eImageType != NUI_IMAGE_TYPE_COLOR)
STDERR("Image type is not match with the color\r\n");
INuiFrameTexture *pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 ){
byte * pBuffer = (byte *)LockedRect.pBits;
#if defined(USE_FACETRACKER)
setColorImage(LockedRect.pBits, LockedRect.size);
#endif
NUI_SURFACE_DESC pDesc;
pTexture->GetLevelDesc(0, &pDesc);
//printf("w: %d, h: %d, byte/pixel: %d\r\n", pDesc.Width, pDesc.Height, LockedRect.Pitch/pDesc.Width);
typedef struct t_RGBA{
byte r;
byte g;
byte b;
byte a;
};
t_RGBA *p = (t_RGBA *)pBuffer;
for(int i=0;i<pTexture->BufferLen()/4;i++){
byte b = p->b;
p->b = p->r;
p->r = b;
p->a = (byte)255;
p++;
}
glBindTexture(GL_TEXTURE_2D, bg_texture[IMAGE_TEXTURE]);
glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
pDesc.Width, pDesc.Height,
0, GL_RGBA, GL_UNSIGNED_BYTE, pBuffer);
pTexture->UnlockRect(0);
}else{
STDERR("Buffer length of received texture is bogus\r\n");
}
pNuiSensor->NuiImageStreamReleaseFrame( pVideoStreamHandle, &imageFrame );
}
void KinectSensor::GotDepthAlert( )
{
const NUI_IMAGE_FRAME* pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &pImageFrame);
if (FAILED(hr))
{
return;
}
INuiFrameTexture* pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch)
{ // Copy depth frame to face tracking
memcpy(m_DepthBuffer->GetBuffer(), PBYTE(LockedRect.pBits), min(m_DepthBuffer->GetBufferSize(), UINT(pTexture->BufferLen())));
}
else
{
OutputDebugString( L"Buffer length of received depth texture is bogus\r\n" );
}
hr = NuiImageStreamReleaseFrame(m_pDepthStreamHandle, pImageFrame);
}
void FubiKinectSDKSensor::gotDepthAlert( )
{
const NUI_IMAGE_FRAME* pImageFrame = NULL;
if (FAILED(NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &pImageFrame)))
return;
INuiFrameTexture* pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
if (SUCCEEDED(pTexture->LockRect(0, &LockedRect, NULL, 0)) && LockedRect.Pitch)
{ // Copy depth frame to face tracking
memcpy(m_depthBuffer->GetBuffer(), PBYTE(LockedRect.pBits), min(m_depthBuffer->GetBufferSize(), UINT(pTexture->BufferLen())));
// Release frame in the meantime
NuiImageStreamReleaseFrame(m_pDepthStreamHandle, pImageFrame);
// Convert to non-player-id-depth
m_depthBuffer->CopyTo(m_convertedDepthBuffer, 0, 0, 0);
// And extract player id
extractPlayerID(m_depthBuffer, m_playerPixelBuffer);
m_imageDataNew = true;
}
}
HRESULT DepthSensor::CopyExtendedDepth(NUI_IMAGE_FRAME &imageFrame)
{
HRESULT hr = S_OK;
if (nullptr == m_pDepthImagePixelBuffer)
{
throw std::runtime_error("Error depth image pixel buffer is nullptr.");
return E_FAIL;
}
INuiFrameTexture *extendedDepthTex = nullptr;
// Extract the extended depth in NUI_DEPTH_IMAGE_PIXEL format from the frame
BOOL nearModeOperational = FALSE;
hr = mNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(mDepthStreamHandle, &imageFrame, &nearModeOperational, &extendedDepthTex);
if (FAILED(hr))
{
throw std::runtime_error("Error getting extended depth texture.");
return hr;
}
NUI_LOCKED_RECT extendedDepthLockedRect;
// Lock the frame data to access the un-clamped NUI_DEPTH_IMAGE_PIXELs
hr = extendedDepthTex->LockRect(0, &extendedDepthLockedRect, nullptr, 0);
if (FAILED(hr) || extendedDepthLockedRect.Pitch == 0)
{
throw std::runtime_error("Error getting extended depth texture pixels.");
return hr;
}
/////////////////////////////////////////////////////////////////memcpy
// Copy the depth pixels so we can return the image frame
errno_t err = memcpy_s(m_pDepthImagePixelBuffer, cDepthImagePixels * sizeof(NUI_DEPTH_IMAGE_PIXEL), extendedDepthLockedRect.pBits, extendedDepthTex->BufferLen());
extendedDepthTex->UnlockRect(0);
if (0 != err)
{
throw std::runtime_error("Error copying extended depth texture pixels.");
return hr;
}
return hr;
}
void FubiKinectSDKSensor::gotVideoAlert( )
{
const NUI_IMAGE_FRAME* pImageFrame = NULL;
if (FAILED(NuiImageStreamGetNextFrame(m_pVideoStreamHandle, 0, &pImageFrame)))
return;
INuiFrameTexture* pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
if (SUCCEEDED(pTexture->LockRect(0, &LockedRect, NULL, 0)) && LockedRect.Pitch)
{ // Copy video frame to face tracking
memcpy(m_videoBuffer->GetBuffer(), PBYTE(LockedRect.pBits), min(m_videoBuffer->GetBufferSize(), UINT(pTexture->BufferLen())));
// Release frame
NuiImageStreamReleaseFrame(m_pVideoStreamHandle, pImageFrame);
// Convert copied data
m_videoBuffer->CopyTo(m_convertedVideoBuffer, 0, 0, 0);
m_imageDataNew = true;
}
}
void FubiKinectSDKSensor::gotVideoAlert( )
{
NUI_IMAGE_FRAME pImageFrame;
if (FAILED(m_pNuiSensor->NuiImageStreamGetNextFrame(m_pVideoStreamHandle, 0, &pImageFrame)))
return;
INuiFrameTexture* pTexture = pImageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
if (SUCCEEDED(pTexture->LockRect(0, &LockedRect, NULL, 0)))
{
if (LockedRect.Pitch)
{
if (m_options.m_rgbOptions.isValid())
{
// Copy video frame to face tracking
memcpy(m_videoBuffer->GetBuffer(), PBYTE(LockedRect.pBits), min(m_videoBuffer->GetBufferSize(), UINT(pTexture->BufferLen())));
// Convert copied data
m_videoBuffer->CopyTo(m_convertedVideoBuffer, 0, 0, 0);
m_imageDataNew = true;
}
else if (m_options.m_irOptions.isValid())
{
//memcpy(m_irBuffer, PBYTE(LockedRect.pBits), min(UINT(m_options.m_irOptions.m_width*m_options.m_irOptions.m_height*sizeof(unsigned short)), UINT(pTexture->BufferLen())));
unsigned int len = m_options.m_irOptions.m_width*m_options.m_irOptions.m_height;
unsigned short* pSource = (unsigned short*)LockedRect.pBits;
unsigned short* pDest = m_irBuffer;
for (unsigned int i=0; i < len; ++i)
{
*pDest++ = (*pSource++) >> 6;
}
m_imageDataNew = true;
}
}
// Release texture
pTexture->UnlockRect(0);
// Release frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pVideoStreamHandle, &pImageFrame);
}
/// <summary>
/// Retrieve depth data from stream frame
/// </summary>
void NuiDepthStream::ProcessDepth()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
if (m_Recording)
{
//////Initializaing a video writer and allocate an image for recording /////////
if ((m_TimerCount++)%FramesPerFile==0)
{
WCHAR szFilename[MAX_PATH] = { 0 };
if (SUCCEEDED(GetFileName(szFilename,_countof(szFilename), m_instanceName, DepthSensor)))
{
char char_szFilename[MAX_PATH] = {0};
size_t convertedChars;
wcstombs_s(&convertedChars,char_szFilename,sizeof(char_szFilename),szFilename,sizeof(char_szFilename));
m_pwriter=cvCreateVideoWriter(char_szFilename,
CV_FOURCC('L', 'A', 'G', 'S'),
//-1, //user specified
FramesPerSecond,m_ImageRes);
//2,m_ImageRes);
}
m_TimerCount%=FramesPerFile;
}
}
// Attempt to get the depth frame
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_hStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return;
}
if (m_paused)
{
// Stream paused. Skip frame process and release the frame.
goto ReleaseFrame;
}
BOOL nearMode;
INuiFrameTexture* pTexture;
///FT image texture
INuiFrameTexture* pFTTexture;
pFTTexture=imageFrame.pFrameTexture;
// Get the depth image pixel texture
hr = m_pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(m_hStreamHandle, &imageFrame, &nearMode, &pTexture);
if (FAILED(hr))
{
goto ReleaseFrame;
}
NUI_LOCKED_RECT lockedRect;
///FT locked rect
NUI_LOCKED_RECT FTlockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
pTexture->LockRect(0, &lockedRect, NULL, 0);
// Lock the FT frame data
pFTTexture->LockRect(0, &FTlockedRect, NULL, 0);
// Make sure we've received valid data
if (lockedRect.Pitch != 0)
{
// Conver depth data to color image and copy to image buffer
m_imageBuffer.CopyDepth(lockedRect.pBits, lockedRect.size, nearMode, m_depthTreatment);
// Convert 8 bits depth frame to 12 bits
NUI_DEPTH_IMAGE_PIXEL* depthBuffer = (NUI_DEPTH_IMAGE_PIXEL*)lockedRect.pBits;
cv::Mat depthMat(m_ImageRes.height, m_ImageRes.width, CV_8UC1);
INT cn = 1;
for(int i=0;i<depthMat.rows;i++){
for(int j=0;j<depthMat.cols;j++){
USHORT realdepth = ((depthBuffer->depth)&0x0fff); //Taking 12LSBs for depth
BYTE intensity = realdepth == 0 || realdepth > 4095 ? 0 : 255 - (BYTE)(((float)realdepth / 4095.0f) * 255.0f);//Scaling to 255 scale grayscale
depthMat.data[i*depthMat.cols*cn + j*cn + 0] = intensity;
depthBuffer++;
}
}
// Copy FT depth data to IFTImage buffer
memcpy(m_pFTdepthBuffer->GetBuffer(), PBYTE(FTlockedRect.pBits), std::min(m_pFTdepthBuffer->GetBufferSize(), UINT(pFTTexture->BufferLen())));
if (m_Recording)
{
//*m_pcolorImage = depthMat;
//cvWriteFrame(m_pwriter,m_pcolorImage);
const NUI_SKELETON_FRAME* pSkeletonFrame = m_pPrimaryViewer->getSkeleton();
m_pDepthInbedAPPs->processFrame(depthMat, lockedRect.pBits, m_ImageRes, pSkeletonFrame);
//if (m_TimerCount%FramesPerFile==0)
//{
// cvReleaseVideoWriter(&m_pwriter);
//}
}
// Draw out the data with Direct2D
if (m_pStreamViewer)
{
m_pStreamViewer->SetImage(&m_imageBuffer);
}
}
// Done with the texture. Unlock and release it
pFTTexture->UnlockRect(0);
pTexture->UnlockRect(0);
pTexture->Release();
ReleaseFrame:
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_hStreamHandle, &imageFrame);
}
bool KinectCapture::GetColorImage(cv::Mat& image)
{
NUI_IMAGE_FRAME imageFrame;
NUI_LOCKED_RECT lockedRect;
HRESULT hr;
// Get a color frame from Kinect
image = cv::Mat::zeros(m_ColorHeight, m_ColorWidth,CV_8UC4);
hr = m_NuiSensor->NuiImageStreamGetNextFrame(m_ColorStreamHandle, 100, &imageFrame);
if (FAILED(hr))
{
if (hr == E_FAIL)
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed with unspecified"<< std::endl;
}
if (hr == E_INVALIDARG)
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed with dwFlag parameter is NULL" << std::endl;
}
if (hr == E_NUI_DEVICE_NOT_READY)
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed with kinect not initialized" << std::endl;
}
if (hr == E_OUTOFMEMORY)
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed with out of memory" << std::endl;
}
if (hr == E_POINTER)
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed with invalid handle" << std::endl;
}
if (hr == E_NUI_FRAME_NO_DATA)
{
return false;
}
return false;
}
INuiFrameTexture* colorTexture = imageFrame.pFrameTexture;
// Lock the frame data to access depth data
hr = colorTexture->LockRect(0, &lockedRect, NULL, 0);
if (FAILED(hr) || lockedRect.Pitch == 0)
{
std::cout << "Error getting color texture pixels." << std::endl;
return false;
}
// Copy the depth pixels so we can return the image frame
errno_t err = memcpy_s(m_colorImagePixelBuffer, 4 * m_ColorImagePixels * sizeof(BYTE), lockedRect.pBits, colorTexture->BufferLen());
colorTexture->UnlockRect(0);
if (0 != err)
{
std::cout << "Error copying color texture pixels." << std::endl;
return false;
}
// Release the Kinect camera frame
m_NuiSensor->NuiImageStreamReleaseFrame(m_ColorStreamHandle, &imageFrame);
if (FAILED(hr))
{
std::cout << "Kinect Color stream NuiImageStreamReleaseFrame call failed." << std::endl;
return false;
}
cv::Mat colorImg(m_ColorHeight, m_ColorWidth, CV_8UC4, m_colorImagePixelBuffer);
image = colorImg;
return true;
}
bool KinectCapture::GetDepthImage(cv::Mat & image)
{
NUI_IMAGE_FRAME imageFrame;
NUI_LOCKED_RECT lockedRect;
HRESULT hr;
image = cv::Mat::zeros(m_ColorHeight, m_ColorWidth, CV_16U);
// Get a depth frame from Kinect
hr = m_NuiSensor->NuiImageStreamGetNextFrame(m_DepthStreamHandle, 30, &imageFrame);
if (FAILED(hr))
{
if (hr == E_NUI_FRAME_NO_DATA)
{
return false;
}
else
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed." << std::endl;
}
return false;
}
INuiFrameTexture* depthTexture = imageFrame.pFrameTexture;
// Lock the frame data to access depth data
hr = depthTexture->LockRect(0, &lockedRect, NULL, 0);
if (FAILED(hr) || lockedRect.Pitch == 0)
{
std::cout << "Error getting depth texture pixels." << std::endl;
return false;
}
// Copy the depth pixels so we can return the image frame
errno_t err = memcpy_s(m_depthImagePixelBuffer, m_DepthImagePixels * sizeof(USHORT), lockedRect.pBits, depthTexture->BufferLen());
depthTexture->UnlockRect(0);
if (0 != err)
{
std::cout << "Error copying depth texture pixels." << std::endl;
return false;
}
// Release the Kinect camera frame
m_NuiSensor->NuiImageStreamReleaseFrame(m_DepthStreamHandle, &imageFrame);
if (FAILED(hr))
{
std::cout << "Kinect Depth stream NuiImageStreamReleaseFrame call failed." << std::endl;
return false;
}
cv::Mat depthImg(m_DepthHeight,m_DepthWidth,CV_16U, m_depthImagePixelBuffer);
image = depthImg;
return true;
}
