void Kinect::run() { while ( mCapture ) { if ( mSensor != 0 ) { // Get elapsed time to calculate frame rate double time = getElapsedSeconds(); ////////////////////////////////////////////////////////////////////////////////////////////// if ( mEnabledDepth && mDepthStreamHandle != 0 && !mNewDepthFrame ) { // Acquire depth image _NUI_IMAGE_FRAME imageFrame; HRESULT hr = mSensor->NuiImageStreamGetNextFrame( mDepthStreamHandle, WAIT_TIME, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } else { // Read texture to surface INuiFrameTexture * texture = imageFrame.pFrameTexture; _NUI_LOCKED_RECT lockedRect; hr = texture->LockRect( 0, &lockedRect, 0, 0 ); if ( FAILED( hr ) ) { error( hr ); } if ( lockedRect.Pitch == 0 ) { trace( "Invalid buffer length received" ); } else { pixelToDepthSurface( mDepthSurface, (uint16_t*)lockedRect.pBits ); } // Clean up hr = mSensor->NuiImageStreamReleaseFrame( mDepthStreamHandle, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } // Update frame rate mFrameRateDepth = (float)( 1.0 / ( time - mReadTimeDepth ) ); mReadTimeDepth = time; // Set flag mNewDepthFrame = true; // Update user count mUserCount = 0; for ( uint32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) { if ( mActiveUsers[ i ] ) { mUserCount++; } } } } ////////////////////////////////////////////////////////////////////////////////////////////// if ( mEnabledSkeletons && mIsSkeletonDevice && !mNewSkeletons ) { // Acquire skeleton _NUI_SKELETON_FRAME skeletonFrame; HRESULT hr = mSensor->NuiSkeletonGetNextFrame( WAIT_TIME, &skeletonFrame ); if ( FAILED( hr ) ) { error( hr ); } else { // Iterate through skeletons bool foundSkeleton = false; for ( int32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) { // Clear skeleton data mSkeletons[ i ].clear(); // Mark skeleton found if ( ( skeletonFrame.SkeletonData + i )->eTrackingState == NUI_SKELETON_TRACKED ) { // Smooth out the skeleton data when found if ( !foundSkeleton ) { hr = mSensor->NuiTransformSmooth( &skeletonFrame, 0 ); if ( FAILED( hr ) ) { error( hr ); } foundSkeleton = true; } // Get skeleton data _NUI_SKELETON_DATA skeletonData = *( skeletonFrame.SkeletonData + i ); // Set joint data for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; j++ ) { Vector4 point = *( skeletonData.SkeletonPositions + j ); ( mSkeletons.begin() + i )->insert( std::make_pair<JointName, Vec3f>( (JointName)j, Vec3f( point.x, point.y, point.z ) ) ); } } } // Update frame rate mFrameRateSkeletons = (float)( 1.0 / ( time - mReadTimeSkeletons ) ); mReadTimeSkeletons = time; // Set flag mNewSkeletons = true; } } ////////////////////////////////////////////////////////////////////////////////////////////// if ( mEnabledVideo && mVideoStreamHandle != 0 && !mNewVideoFrame ) { // Acquire video image _NUI_IMAGE_FRAME imageFrame; HRESULT hr = mSensor->NuiImageStreamGetNextFrame( mVideoStreamHandle, WAIT_TIME, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } else { // Read texture INuiFrameTexture * texture = imageFrame.pFrameTexture; _NUI_LOCKED_RECT lockedRect; hr = texture->LockRect( 0, &lockedRect, 0, 0 ); if ( FAILED( hr ) ) { error( hr ); } if ( lockedRect.Pitch != 0 ) { pixelToVideoSurface( mVideoSurface, (uint8_t *)lockedRect.pBits ); } else { trace( "Invalid buffer length received." ); } // Clean up hr = mSensor->NuiImageStreamReleaseFrame( mVideoStreamHandle, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } // Update frame rate mFrameRateVideo = (float)( 1.0 / ( time - mReadTimeVideo ) ); mReadTimeVideo = time; // Set flag mNewVideoFrame = true; } } } // Pause thread Sleep( 17 ); } // Return to join thread return; }
void Kinect::run() { while ( mCapture ) { if ( mSensor != 0 ) { // Get elapsed time to calculate frame rate double time = getElapsedSeconds(); ////////////////////////////////////////////////////////////////////////////////////////////// if ( mDeviceOptions.isDepthEnabled() && mDepthStreamHandle != 0 && !mNewDepthSurface ) { _NUI_IMAGE_FRAME imageFrame; long hr = mSensor->NuiImageStreamGetNextFrame( mDepthStreamHandle, WAIT_TIME, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } else { INuiFrameTexture * texture = imageFrame.pFrameTexture; _NUI_LOCKED_RECT lockedRect; hr = texture->LockRect( 0, &lockedRect, 0, 0 ); if ( FAILED( hr ) ) { error( hr ); } if ( lockedRect.Pitch == 0 ) { trace( "Invalid buffer length received" ); } else { pixelToDepthSurface( (uint16_t*)lockedRect.pBits ); } hr = mSensor->NuiImageStreamReleaseFrame( mDepthStreamHandle, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } mFrameRateDepth = (float)( 1.0 / ( time - mReadTimeDepth ) ); mReadTimeDepth = time; mUserCount = 0; for ( uint32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) { if ( mActiveUsers[ i ] ) { mUserCount++; } } mNewDepthSurface = true; } } ////////////////////////////////////////////////////////////////////////////////////////////// if ( mDeviceOptions.isSkeletonTrackingEnabled() && mIsSkeletonDevice && !mNewSkeletons ) { _NUI_SKELETON_FRAME skeletonFrame; long hr = mSensor->NuiSkeletonGetNextFrame( WAIT_TIME, &skeletonFrame ); if ( FAILED( hr ) ) { error( hr ); } else { bool foundSkeleton = false; for ( int32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) { mSkeletons.at( i ).clear(); NUI_SKELETON_TRACKING_STATE trackingState = skeletonFrame.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( &skeletonFrame, &transform ); if ( FAILED( hr ) ) { error( hr ); } foundSkeleton = true; } // Flip X when flipping the image. if ( mFlipped ) { ( skeletonFrame.SkeletonData + i )->Position.x *= -1.0f; for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; j++ ) { ( skeletonFrame.SkeletonData + i )->SkeletonPositions[ j ].x *= -1.0f; } } _NUI_SKELETON_BONE_ORIENTATION bones[ NUI_SKELETON_POSITION_COUNT ]; hr = NuiSkeletonCalculateBoneOrientations( skeletonFrame.SkeletonData + i, bones ); if ( FAILED( hr ) ) { error( hr ); } for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; j++ ) { Bone bone( *( ( skeletonFrame.SkeletonData + i )->SkeletonPositions + j ), *( bones + j ) ); ( mSkeletons.begin() + i )->insert( std::make_pair<JointName, Bone>( (JointName)j, bone ) ); } } } mFrameRateSkeletons = (float)( 1.0 / ( time - mReadTimeSkeletons ) ); mReadTimeSkeletons = time; mNewSkeletons = true; } } ////////////////////////////////////////////////////////////////////////////////////////////// if ( mDeviceOptions.isVideoEnabled() && mVideoStreamHandle != 0 && !mNewVideoSurface ) { _NUI_IMAGE_FRAME imageFrame; long hr = mSensor->NuiImageStreamGetNextFrame( mVideoStreamHandle, WAIT_TIME, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } else { INuiFrameTexture * texture = imageFrame.pFrameTexture; _NUI_LOCKED_RECT lockedRect; hr = texture->LockRect( 0, &lockedRect, 0, 0 ); if ( FAILED( hr ) ) { error( hr ); } if ( lockedRect.Pitch != 0 ) { pixelToVideoSurface( (uint8_t *)lockedRect.pBits ); } else { trace( "Invalid buffer length received." ); } hr = mSensor->NuiImageStreamReleaseFrame( mVideoStreamHandle, &imageFrame ); if ( FAILED( hr ) ) { error( hr ); } mFrameRateVideo = (float)( 1.0 / ( time - mReadTimeVideo ) ); mReadTimeVideo = time; mNewVideoSurface = true; } } } Sleep( 8 ); } // Return to join thread return; }
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; }