void KinectGrabber::Kinect_GotDepthAlert( ) {
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(
m_pDepthStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
printf("Unable to get the frame after recieving alert for depth frame \n");
return;
}
NuiImageBuffer * pTexture = pImageFrame->pFrameTexture;
KINECT_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
// draw the bits to the bitmap
RGBQUAD * rgbrun = m_rgbDepth;
USHORT * depthrun = m_depthBuffer;
USHORT * playerrun = m_playerBuffer;
USHORT * pBufferRun = (USHORT*) pBuffer;
for( int y = 0 ; y < DEPTH_HEIGHT ; y++ )
{
for( int x = 0 ; x < DEPTH_WIDTH ; x++ )
{
// set the color (just for visualization)
RGBQUAD quad = Kinect_DepthToRGB( *pBufferRun );
*rgbrun = quad;
rgbrun++;
//USHORT RealDepth = (*pBufferRun & 0xfff8) >> 3;
USHORT RealDepth = (*pBufferRun & 0x0fff);
//*depthrun = RealDepth;
//depthrun++;
//flip the depth image to match the video image
m_depthBuffer[((y+1)*DEPTH_WIDTH) - x] = RealDepth;
USHORT Player = *pBufferRun & 7;
*playerrun = Player;
playerrun++;
//inc buffer pointer
pBufferRun++;
}
}
}
else
{
printf( "Buffer length of received texture is bogus\r\n" );
}
NuiImageStreamReleaseFrame( m_pDepthStreamHandle, pImageFrame );
}
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);
}
int Read_Kinect::drawColor(HANDLE h)
{
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(h, 0, &pImageFrame);
if (FAILED(hr))
{
cout << "Get Image Frame Failed" << endl;
return -1;
}
INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch != 0)
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
_color = Mat(COLOR_HIGHT,COLOR_WIDTH,CV_8UC4,pBuffer);
for (int i=0; i<COLOR_HIGHT; i++)
for(int j=0; j<COLOR_WIDTH; j++)
{
_bottom_left_img.at<Vec3b>(i,j).val[0] = _color.at<Vec4b>(i,j).val[0];
_bottom_left_img.at<Vec3b>(i,j).val[1] = _color.at<Vec4b>(i,j).val[1];
_bottom_left_img.at<Vec3b>(i,j).val[2] = _color.at<Vec4b>(i,j).val[2];
}
// imshow("color image", _color);
}
NuiImageStreamReleaseFrame(h, pImageFrame);
return 0;
}
void KinectGrabber::Kinect_GotVideoAlert( )
{
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(
m_pVideoStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
printf("Unable to get the frame after recieving alert for video frame \n");
return;
}
NuiImageBuffer * pTexture = pImageFrame->pFrameTexture;
KINECT_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
memcpy(m_rgbBuffer, LockedRect.pBits, sizeof(BYTE) * VIDEO_HEIGHT * VIDEO_WIDTH * 4);
//m_rgbBuffer = (BYTE*) LockedRect.pBits;
Kinect_FormatRGBForOutput();
Kinect_makeRGBFromRGBA();
//2560 bytes per line = 640 * 4 (4 bytes per pixel)
} else {
printf("buffer length of recieved texture is bogus\n");
}
NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );
}
void KinectCam::Nui_GetCamFrame(BYTE *frameBuffer, int frameSize)
{
const NUI_IMAGE_FRAME *pImageFrame = NULL;
WaitForSingleObject(m_hNextVideoFrameEvent, INFINITE);
HRESULT hr = NuiImageStreamGetNextFrame(
m_pVideoStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
return;
}
INuiFrameTexture *pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
memcpy(frameBuffer, pBuffer, frameSize);
}
NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );
}
void CSkeletalViewerApp::Nui_GotVideoAlert( )
{
if (GetTickCount64() > m_videoDelay && m_videoDelay != 0) {
m_videoDelay = 0;
} else {
return;
}
//m_FramesTotal++;
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(
m_pVideoStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
return;
}
NuiImageBuffer * pTexture = pImageFrame->pFrameTexture;
KINECT_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
UINT * pBufferRun = (UINT*) pBuffer;
UINT * pVideoRun = m_videoCache;
USHORT * pPlayerRun = m_playerMap;
for( int y = 0 ; y < 480 ; y++ )
{
for( int x = 0 ; x < 640 ; x++ )
{
*pVideoRun = *pBufferRun;
pVideoRun++;
pBufferRun++;
}
}
}
else
{
OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
}
NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );
}
HRESULT RTCKinect::WriteColorImage(void)
{
static const long TIMEOUT_IN_MILLI = 100;
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(m_pVideoStreamHandle, TIMEOUT_IN_MILLI, &pImageFrame );
if( FAILED( hr ) ) {
std::cout << "NuiImageStreamGetNextFrame failed." << std::endl;
return hr;
}
NuiImageBuffer * pTexture = pImageFrame->pFrameTexture;
KINECT_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
for(int h = 0;h < m_camera_height;h++) {
for(int w = 0;w < m_camera_width;w++) {
BYTE* pixel = pBuffer + (h * m_camera_width * 4) + w * 4;
BYTE b = pixel[0];
BYTE g = pixel[1];
BYTE r = pixel[2];
int offset = h*m_camera_width*3+w*3;
m_image.pixels[offset + 0] = b;
m_image.pixels[offset + 1] = g;
m_image.pixels[offset + 2] = r;
}
m_imageOut.write();
}
}
else {
std::cout << "Buffer length of received texture is bogus\r\n" << std::endl;
}
NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );
return S_OK;
}
void Device::run()
{
HANDLE events[ 4 ];
events[ 0 ] = mColorEvent;
events[ 1 ] = mDepthEvent;
events[ 2 ] = mSkeletonEvent;
while ( mCapture ) {
if ( mSensor != 0 ) {
double time = getElapsedSeconds();
WaitForMultipleObjects( sizeof( events ) / sizeof( events[ 0 ]), events, 0, WAIT_TIME );
const NUI_IMAGE_FRAME* frameColor = 0;
const NUI_IMAGE_FRAME* frameDepth = 0;
NUI_SKELETON_FRAME frameSkeleton = { 0 };
bool readColor = !mNewColorSurface;
bool readDepth = !mNewDepthSurface;
bool readSkeleton = !mNewSkeletons;
/*if ( mDeviceOptions.isFrameSyncEnabled() && mDeviceOptions.isColorEnabled() && mDeviceOptions.isDepthEnabled() ) {
if ( readColor != readDepth ) {
readColor = false;
readDepth = false;
}
readSkeleton = readDepth;
}*/
readColor = readColor && mDeviceOptions.isColorEnabled();
readDepth = readDepth && mDeviceOptions.isDepthEnabled();
readSkeleton = readSkeleton && mDeviceOptions.isSkeletonTrackingEnabled();
//////////////////////////////////////////////////////////////////////////////////////////////
if ( readDepth && WAIT_OBJECT_0 == WaitForSingleObject( mDepthEvent, 0 ) ) {
if ( SUCCEEDED( NuiImageStreamGetNextFrame( mDepthStreamHandle, 0, &frameDepth ) ) &&
frameDepth != 0 && frameDepth->pFrameTexture != 0 ) {
mDepthTimeStamp = frameDepth->liTimeStamp.QuadPart;
INuiFrameTexture* texture = frameDepth->pFrameTexture;
_NUI_LOCKED_RECT lockedRect;
long hr = texture->LockRect( 0, &lockedRect, 0, 0 );
if ( FAILED( hr ) ) {
error( hr );
}
if ( lockedRect.Pitch == 0 ) {
console() << "Invalid buffer length received" << endl;
} else {
pixelToDepthSurface( (uint16_t*)lockedRect.pBits );
}
hr = NuiImageStreamReleaseFrame( mDepthStreamHandle, frameDepth );
if ( FAILED( hr ) ) {
error( hr );
}
mUserCount = 0;
for ( uint32_t i = 0; i < NUI_SKELETON_COUNT; ++i ) {
if ( mActiveUsers[ i ] ) {
++mUserCount;
}
}
mNewDepthSurface = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
if ( readColor && WAIT_OBJECT_0 == WaitForSingleObject( mColorEvent, 0 ) ) {
if ( SUCCEEDED( NuiImageStreamGetNextFrame( mColorStreamHandle, 0, &frameColor ) ) &&
frameColor != 0 && frameColor->pFrameTexture != 0 ) {
INuiFrameTexture* texture = frameColor->pFrameTexture;
_NUI_LOCKED_RECT lockedRect;
long hr = texture->LockRect( 0, &lockedRect, 0, 0 );
if ( FAILED( hr ) ) {
error( hr );
}
if ( lockedRect.Pitch != 0 ) {
pixelToColorSurface( (uint8_t*)lockedRect.pBits );
/*if ( mDeviceOptions.isFrameSyncEnabled() ) {
mColorFrames.push_back( ColorFrame( mColorSurface, frameColor->liTimeStamp.QuadPart ) );
if ( mColorFrames.size() > 10 ) {
mColorFrames.erase( mColorFrames.begin() );
}
}*/
} else {
console() << "Invalid buffer length received." << endl;
}
hr = NuiImageStreamReleaseFrame( mColorStreamHandle, frameColor );
if ( FAILED( hr ) ) {
error( hr );
}
mNewColorSurface = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
if ( readSkeleton && WAIT_OBJECT_0 == WaitForSingleObject( mSkeletonEvent, 0 ) ) {
long hr = NuiSkeletonGetNextFrame( 0, &frameSkeleton );
if ( SUCCEEDED( hr ) ) {
bool foundSkeleton = false;
for ( int32_t i = 0; i < NUI_SKELETON_COUNT; ++i ) {
mSkeletons.at( i ).clear();
NUI_SKELETON_TRACKING_STATE trackingState = frameSkeleton.SkeletonData[ i ].eTrackingState;
if ( trackingState == NUI_SKELETON_TRACKED || trackingState == NUI_SKELETON_POSITION_ONLY ) {
if ( !foundSkeleton ) {
_NUI_TRANSFORM_SMOOTH_PARAMETERS transform = kTransformParams[ mTransform ];
hr = mSensor->NuiTransformSmooth( &frameSkeleton, &transform );
if ( FAILED( hr ) ) {
error( hr );
}
foundSkeleton = true;
}
if ( mFlipped ) {
( frameSkeleton.SkeletonData + i )->Position.x *= -1.0f;
for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; ++j ) {
( frameSkeleton.SkeletonData + i )->SkeletonPositions[ j ].x *= -1.0f;
}
}
_NUI_SKELETON_BONE_ORIENTATION bones[ NUI_SKELETON_POSITION_COUNT ];
hr = NuiSkeletonCalculateBoneOrientations( frameSkeleton.SkeletonData + i, bones );
if ( FAILED( hr ) ) {
error( hr );
}
for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; ++j ) {
Bone bone( *( ( frameSkeleton.SkeletonData + i )->SkeletonPositions + j ), *( bones + j ) );
( mSkeletons.begin() + i )->insert( std::pair<JointName, Bone>( (JointName)j, bone ) );
}
}
}
mNewSkeletons = true;
}
mFrameRate = (float)( 1.0 / ( time - mReadTime ) );
mReadTime = time;
}
}
}
return;
}
void CSkeletalViewerApp::Nui_GotDepthAlert( )
{
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(
m_pDepthStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
return;
}
NuiImageBuffer * pTexture = pImageFrame->pFrameTexture;
KINECT_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
USHORT * pPlayerRun = m_playerMap;
for( int y = 0 ; y < 480 ; y++ )
{
for( int x = 0 ; x < 640 ; x++ )
{
*pPlayerRun = 0;
*pPlayerRun++;
}
}
// draw the bits to the bitmap
RGBQUAD * rgbrun = m_rgbWk;
USHORT * pBufferRun = (USHORT*) pBuffer;
USHORT player, depth;
long colorX = 0, colorY = 0;
for( int y = 0 ; y < 240 ; y++ )
{
for( int x = 0 ; x < 320 ; x++ )
{
depth = *pBufferRun & 0xfff8;
if (FrameCount == 0)
{
initialdepth[x + y*320] = depth;
}
//TODO if first frame, save depth values as background
player = *pBufferRun & 7;
NuiImageGetColorPixelCoordinatesFromDepthPixel(
NUI_IMAGE_RESOLUTION_640x480,
0, x, y, depth, &colorX, &colorY);
USHORT writeover = 0;
//HACK player id based:
writeover = player;
//HACK background subtraction
if (depth < initialdepth[x + y*320] - 10)
{
//HACK writeover = 10;
}
m_playerMap[colorY * 640 + colorX] = writeover;
m_playerMap[colorY * 640 + colorX + 1] = writeover;
m_playerMap[(colorY + 1)* 640 + colorX] = writeover;
m_playerMap[(colorY + 1)* 640 + colorX + 1] = writeover;
RGBQUAD quad = Nui_ShortToQuad_Depth( *pBufferRun );
pBufferRun++;
*rgbrun = quad;
rgbrun++;
}
}
UINT bgColor = 0x00ffeedd;
UINT * pVideoRun = m_videoCache;
pPlayerRun = m_playerMap;
for( int y = 0 ; y < 480 ; y++ )
{
for( int x = 0 ; x < 640; x++ )
{
if (*pPlayerRun == 0)
{
*pVideoRun = bgColor;
}
pVideoRun++;
pPlayerRun++;
}
}
m_DrawVideo.DrawFrame( (BYTE*) m_videoCache );
m_DrawDepth.DrawFrame( (BYTE*) m_rgbWk );
ULONGLONG diff = (ULONGLONG) ((1.0 / m_FramesTotal - 1.0 / 20) * 1000);
//diff = 0;
m_videoDelay = GetTickCount64() + diff;
}
else
{
OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
}
FrameCount++;
NuiImageStreamReleaseFrame( m_pDepthStreamHandle, pImageFrame );
}
DWORD WINAPI Kinect::nuiProcessThread(LPVOID pParam) {
Kinect *pthis = (Kinect *)pParam;
HRESULT hr;
KINECT_LOCKED_RECT LockedRect;
while(1) {
int nEventIdx=WaitForMultipleObjects(sizeof(pthis->_events)/sizeof(pthis->_events[0]),pthis->_events,FALSE,100);
if(nEventIdx==0)
break;
switch(nEventIdx){
case 1:
hr = NuiImageStreamGetNextFrame(
pthis->_depthHandle,
0,
&pthis->_pDepthFrame);
pthis->_pDepthFrame->pFrameTexture->LockRect(0, &LockedRect, NULL, 0);
EnterCriticalSection(&pthis->_csVideo);
if(LockedRect.Pitch !=0) {
BYTE* pBuffer = (BYTE*) LockedRect.pBits;
memcpy(pthis->_depthImg->imageData,pBuffer, (pthis->_depthImg->widthStep)*(pthis->_depthImg->height));
}
LeaveCriticalSection(&pthis->_csVideo);
NuiImageStreamReleaseFrame(pthis->_depthHandle,pthis->_pDepthFrame);
break;
case 2:
hr = NuiImageStreamGetNextFrame(
pthis->_videoHandle,
0,
&pthis->_pVideoFrame);
pthis->_pVideoFrame->pFrameTexture->LockRect(0, &LockedRect, NULL, 0);
EnterCriticalSection(&pthis->_csDepth);
if( LockedRect.Pitch != 0 ) {
BYTE* pBuffer = (BYTE*) LockedRect.pBits;
for( int y = 0 ; y < 480 ; y++ ) {
for( int x = 0 ; x < 640 ; x++) {
pthis->_videoImg->imageData[y*pthis->_videoImg->widthStep+x*pthis->_videoImg->nChannels+0] = *pBuffer++;
pthis->_videoImg->imageData[y*pthis->_videoImg->widthStep+x*pthis->_videoImg->nChannels+1] = *pBuffer++;
pthis->_videoImg->imageData[y*pthis->_videoImg->widthStep+x*pthis->_videoImg->nChannels+2] = *pBuffer++;
pBuffer++;
}
}
}
LeaveCriticalSection(&pthis->_csDepth);
NuiImageStreamReleaseFrame(pthis->_videoHandle,pthis->_pVideoFrame);
break;
case 3:
hr = NuiSkeletonGetNextFrame( 0, &pthis->_skeletonFrame );
NuiTransformSmooth(&pthis->_skeletonFrame,NULL);
EnterCriticalSection(&pthis->_csSkeleton);
//스켈레톤 데이터 변환
LeaveCriticalSection(&pthis->_csSkeleton);
break;
}
}
return 0;
}
