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;
}