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;
	}
Beispiel #3
0
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;
}