/**
* @brief QKinect::tracking : Boucle principale de capture
*/
void QKinect::tracking()
{
// Tant que la capture est demandé
while(isRunning)
{
// Récupération de la frame suivante
HRESULT res = NuiSkeletonGetNextFrame(1, &mSkeletonFrame);
NuiTransformSmooth(&mSkeletonFrame, NULL);
for(int i = 0; i < NUI_SKELETON_COUNT; i++)
{
if(mSkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED)
{
// Vérouillage du mutex pour le vecteur des points de chaque squelettes
mDrawZone->lock();
// Enregistrement des coordonnée de chaque points pour la zone de dessin
mDrawZone->SetSkeleton(i, mSkeletonFrame.SkeletonData[i]);
// Dévérouillage du mutex pour le vecteur des points de chaque squelettes
mDrawZone->unlock();
}
else if(mSkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_NOT_TRACKED)
{
// Squelette n'est plus traqué, on le retire de la zone de dessin
mDrawZone->RemoveSkeleton(i);
}
}
// Sleep de 30ms pour éviter de pourrir la ressource CPU :)
msleep(30);
}
}
void CSkeletalViewerApp::Nui_GotSkeletonAlert( )
{
NUI_SKELETON_FRAME SkeletonFrame;
HRESULT hr = NuiSkeletonGetNextFrame( 0, &SkeletonFrame );
bool bFoundSkeleton = true;
for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
{
if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
{
bFoundSkeleton = false;
}
}
// no skeletons!
//
if( bFoundSkeleton )
{
return;
}
// smooth out the skeleton data
NuiTransformSmooth(&SkeletonFrame,NULL);
// we found a skeleton, re-start the timer
m_bScreenBlanked = false;
m_LastSkeletonFoundTime = -1;
// draw each skeleton color according to the slot within they are found.
//
bool bBlank = true;
for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
{
if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
{
Nui_DrawSkeleton( bBlank, &SkeletonFrame.SkeletonData[i], GetDlgItem( m_hWnd, IDC_SKELETALVIEW ), i );
bBlank = false;
}
}
Nui_DoDoubleBuffer(GetDlgItem(m_hWnd,IDC_SKELETALVIEW), m_SkeletonDC);
}
//-------------------------------------------------------------------
// Nui_GotSkeletonAlert
//
// Handle new skeleton data
//-------------------------------------------------------------------
void SkeletalTracker::Nui_GotSkeletonAlert( )
{
NUI_SKELETON_FRAME SkeletonFrame = {0};
bool bFoundSkeleton = false;
if ( SUCCEEDED(NuiSkeletonGetNextFrame( 0, &SkeletonFrame )) )
// if ( SUCCEEDED(m_pNuiSensor->NuiSkeletonGetNextFrame( 0, &SkeletonFrame )) )
{
for ( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
{
// if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED ||
// (SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_ONLY ))
{
bFoundSkeleton = true;
}
}
}
// no skeletons!
if( !bFoundSkeleton )
{
return;
}
// smooth out the skeleton data
NUI_TRANSFORM_SMOOTH_PARAMETERS smoothParams;
smoothParams.fSmoothing = 0.6f;
smoothParams.fCorrection = 0.3f;
smoothParams.fPrediction = 0.6f;
smoothParams.fJitterRadius = 0.6f;
smoothParams.fMaxDeviationRadius = 0.6f;
HRESULT hr = NuiTransformSmooth(&SkeletonFrame,&smoothParams);
// HRESULT hr = NuiTransformSmooth(&SkeletonFrame,&smoothParams);
if ( FAILED(hr) )
{
return;
}
// we found a skeleton, re-start the skeletal timer
bool bSkeletonIdsChanged = false;
USHORT currentMaxDepth = NUI_IMAGE_DEPTH_MAXIMUM;
int selectedSkeleton = -1;
for ( int i = 0 ; i < NUI_SKELETON_COUNT; i++ )
{
if ( m_SkeletonIds[i] != SkeletonFrame.SkeletonData[i].dwTrackingID )
{
m_SkeletonIds[i] = SkeletonFrame.SkeletonData[i].dwTrackingID;
bSkeletonIdsChanged = true;
}
// Show skeleton only if it is tracked, and the center-shoulder joint is at least inferred.
if ( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED)
{
LONG x, y;
USHORT depth;
// Transform skeleton coordinates to depth image
NuiTransformSkeletonToDepthImage(SkeletonFrame.SkeletonData[i].Position, &x, &y, &depth);
if (depth < currentMaxDepth)
{
selectedSkeleton = i;
currentMaxDepth = depth;
}
}
else if ( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_ONLY )
{
// OutputDebugString( L"Skeleton position only\r\n" );
}
}
if (selectedSkeleton > -1)
{
Nui_DrawSkeleton( &SkeletonFrame.SkeletonData[selectedSkeleton], NULL, selectedSkeleton );
}
}
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;
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
HRESULT hr;
// Get pointer to Kinect handles
unsigned __int64 *MXadress;
if(nrhs==0) {
mexErrMsgTxt("Give Pointer to Kinect as input");
}
MXadress = (unsigned __int64*)mxGetData(prhs[0]);
int depthwidth=(int)MXadress[7];
int depthheight=(int)MXadress[8];
// Initialize Output Skeleton Array
int Jdimsc[2];
Jdimsc[0]=200; Jdimsc[1]=6;
plhs[0] = mxCreateNumericArray(2, Jdimsc, mxDOUBLE_CLASS, mxREAL);
double *Pos;
Pos = mxGetPr(plhs[0]);
NUI_SKELETON_FRAME SkeletonFrame;
// Wait for a Skeleton_Frame to arrive
hr = NuiSkeletonGetNextFrame( 200, &SkeletonFrame );
if( FAILED( hr ) )
{
printf("Failed to get Frame\r\n");
}
else
{
// Check if there is a Skeleton found
bool NoSkeletonFound = true;
for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
{
if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
{
NoSkeletonFound = false;
}
}
if( NoSkeletonFound ) { return; }
// Smooth the skeleton data
NuiTransformSmooth(&SkeletonFrame,NULL);
// Copy Skeleton points to output array
int r=0;
for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
{
if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
{
NUI_SKELETON_DATA * pSkel = &SkeletonFrame.SkeletonData[i];
int j;
float fx=0,fy=0;
for (j = 0; j < NUI_SKELETON_POSITION_COUNT; j++)
{
Pos[j+r]=i+1;
Pos[j+r+Jdimsc[0]]=pSkel->SkeletonPositions[j].x;
Pos[j+r+Jdimsc[0]*2]=pSkel->SkeletonPositions[j].y;
Pos[j+r+Jdimsc[0]*3]=pSkel->SkeletonPositions[j].z;
NuiTransformSkeletonToDepthImage( pSkel->SkeletonPositions[j], &fx, &fy );
Pos[j+r+Jdimsc[0]*4] = (double) ( fx * depthwidth + 0.5f );
Pos[j+r+Jdimsc[0]*5] = (double) ( fy * depthheight + 0.5f );
}
r+=NUI_SKELETON_POSITION_COUNT;
}
}
}
}
