/// <summary>
/// Draw a bone between 2 tracked joint.
/// <summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth image is streched to fit</param>
/// <param name="joint0">Index for the first joint</param>
/// <param name="joint1">Index for the second joint</param>
void UKinect::drawBone(const NUI_SKELETON_DATA& skeletonData, NUI_SKELETON_POSITION_INDEX joint0, NUI_SKELETON_POSITION_INDEX joint1) {
  NUI_SKELETON_POSITION_TRACKING_STATE state0 = skeletonData.eSkeletonPositionTrackingState[joint0];
  NUI_SKELETON_POSITION_TRACKING_STATE state1 = skeletonData.eSkeletonPositionTrackingState[joint1];

  // Any is not tracked
  if (NUI_SKELETON_POSITION_NOT_TRACKED == state0 || NUI_SKELETON_POSITION_NOT_TRACKED == state1) {
    return;
  }

  // Both are inferred
  if (NUI_SKELETON_POSITION_INFERRED == state0 && NUI_SKELETON_POSITION_INFERRED == state1) {
    return;
  }

  LONG x1, y1, x2, y2;
  LONG x1d, y1d, x2d, y2d;
  USHORT depth;

  NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint0], &x1d, &y1d, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
  NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, x1d, y1d, depth, &x1, &y1);
  NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint1], &x2d, &y2d, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
  NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, x2d, y2d, depth, &x2, &y2);

  // We assume all drawn bones are inferred unless BOTH joints are tracked
  if (NUI_SKELETON_POSITION_TRACKED == state0 && NUI_SKELETON_POSITION_TRACKED == state1) {
    line(skeletonCVMat, Point(x1, y1), Point(x2, y2), CV_RGB(0, 0, 255), 4);
  } else {
    line(skeletonCVMat, Point(x1, y1), Point(x2, y2), CV_RGB(160, 160, 180), 4);
  }
}
void SkeletalTracker::LoadPosition( NUI_SKELETON_DATA * pSkel, ArmPosition * pArmPosition)
{
	int i = NUI_SKELETON_POSITION_HAND_RIGHT;
	NuiTransformSkeletonToDepthImage( pSkel->SkeletonPositions[i], &(pArmPosition->m_rightHand.m_x), &(pArmPosition->m_rightHand.m_y), (USHORT*) &( pArmPosition->m_rightHand.m_z) );

	i = NUI_SKELETON_POSITION_HAND_LEFT;
	NuiTransformSkeletonToDepthImage( pSkel->SkeletonPositions[i], &(pArmPosition->m_leftHand.m_x), &(pArmPosition->m_leftHand.m_y), (USHORT*) &(pArmPosition->m_leftHand.m_z) );

	i = NUI_SKELETON_POSITION_HEAD;
	NuiTransformSkeletonToDepthImage( pSkel->SkeletonPositions[i], &(pArmPosition->m_body.m_x), &(pArmPosition->m_body.m_y), (USHORT*) &(pArmPosition->m_body.m_z) );

	pArmPosition->m_startTime = getMilliCount();
}
/// <summary>
/// Find closest skeleton to set tracked
/// </summary>
/// <param name="trackIDs">Array of skeleton tracking IDs</param>
void UKinect::ChooseClosestSkeletons(DWORD trackIDs[TrackIDIndexCount]) {
  ZeroMemory(trackIDs, TrackIDIndexCount * sizeof(DWORD));

  // Initial depth array with max posible value
  USHORT nearestDepth[TrackIDIndexCount] = { NUI_IMAGE_DEPTH_MAXIMUM, NUI_IMAGE_DEPTH_MAXIMUM };

  for (int i = 0; i < NUI_SKELETON_COUNT; i++) {
    if (NUI_SKELETON_NOT_TRACKED != skeletonFrame.SkeletonData[i].eTrackingState) {
      LONG   x, y;
      USHORT depth;

      // Transform skeleton coordinates to depth image
      NuiTransformSkeletonToDepthImage(skeletonFrame.SkeletonData[i].Position, &x, &y, &depth);

      // Compare depth to peviously found item
      if (depth < nearestDepth[FirstTrackID]) {
        // Move depth and track ID in first place to second place and assign with the new closer one
        nearestDepth[SecondTrackID] = nearestDepth[FirstTrackID];
        nearestDepth[FirstTrackID] = depth;

        trackIDs[SecondTrackID] = trackIDs[FirstTrackID];
        trackIDs[FirstTrackID] = skeletonFrame.SkeletonData[i].dwTrackingID;
      } else if (depth < nearestDepth[SecondTrackID]) {
        // Replace old depth and track ID in second place with the newly found closer one
        nearestDepth[SecondTrackID] = depth;
        trackIDs[SecondTrackID] = skeletonFrame.SkeletonData[i].dwTrackingID;
      }
    }
  }
}
Beispiel #4
0
int 
KinectImpl::setTrackingPos(NUI_SKELETON_DATA data, int skelId, int trackingId)
{
  float x,y;

  if(data.eSkeletonPositionTrackingState[skelId] == NUI_SKELETON_POSITION_NOT_TRACKED){
	  return -1;
  }
  if( data.SkeletonPositions[skelId].x ==  0 &&
	   data.SkeletonPositions[skelId].y == 0 &&
	    data.SkeletonPositions[skelId].z == 0) return -1;


  NuiTransformSkeletonToDepthImage(data.SkeletonPositions[skelId], &x, &y, NUI_IMAGE_RESOLUTION_320x240 );
  trackingData[trackingId*2] = (short)x;
  trackingData[trackingId*2+1] = (short)y;

 //  std::cout << " (" << trackingData[trackingId*2] <<"," << trackingData[trackingId*2+1] << ")" ;

  skeletonData[trackingId*3] = data.SkeletonPositions[skelId].x;
  skeletonData[trackingId*3+1] = data.SkeletonPositions[skelId].y;
  skeletonData[trackingId*3+2] = data.SkeletonPositions[skelId].z;

  return 1;
}
/// <summary>
/// Draw a circle to indicate a skeleton of which only position info is available
/// </summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth stream image is streched to fit</param>
void UKinect::drawPosition(const NUI_SKELETON_DATA& skeletonData) {
  LONG xd, yd, x, y;
  USHORT depth;
  NuiTransformSkeletonToDepthImage(skeletonData.Position, &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
  NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
  circle(skeletonCVMat, Point(static_cast<int>(x), static_cast<int>(y)), 10, CV_RGB(0, 50, 200), 2);
  putText(skeletonCVMat, lexical_cast<string>(skeletonData.dwTrackingID), Point(x, y), 1, 2, CV_RGB(255, 255, 255), 2);
}
	Vec2i Kinect::getSkeletonVideoPos( const ci::Vec3f &position )
	{
		float x;
		float y;
		Vector4 pos;
		pos.x = position.x;
		pos.y = position.y;
		pos.z = position.z;
		pos.w = 1.0f;
		NuiTransformSkeletonToDepthImage( pos, &x, &y, mDeviceOptions.getVideoResolution() );
		return Vec2i( (int32_t)x, (int32_t)y );
	}
ofVec3f KinectManager::handPositionToScreenPosition(Vector4 _pos)
{
	ofVec3f pos = ofVec3f();

	float xPos;
	float yPos;
	NuiTransformSkeletonToDepthImage(_pos, &xPos, &yPos, NUI_IMAGE_RESOLUTION_1280x960);

	pos.x = xPos / 1280 * ofGetWindowWidth();
	pos.y = yPos / 960 * ofGetWindowHeight();

	return pos;
}
/// <summary>
/// Converts a skeleton point to screen space
/// </summary>
/// <param name="skeletonPoint">skeleton point to tranform</param>
/// <param name="width">width (in pixels) of output buffer</param>
/// <param name="height">height (in pixels) of output buffer</param>
/// <returns>point in screen-space</returns>
D2D1_POINT_2F CSkeletonBasics::SkeletonToScreen(Vector4 skeletonPoint, int width, int height)
{
    LONG x, y;
    USHORT depth;

    // Calculate the skeleton's position on the screen
    // NuiTransformSkeletonToDepthImage returns coordinates in NUI_IMAGE_RESOLUTION_320x240 space
    NuiTransformSkeletonToDepthImage(skeletonPoint, &x, &y, &depth);

    float screenPointX = static_cast<float>(x * width) / cScreenWidth;
    float screenPointY = static_cast<float>(y * height) / cScreenHeight;

    return D2D1::Point2F(screenPointX, screenPointY);
}
Beispiel #9
0
GLfloat* SkeletonToScreen(Vector4 skeletonPoint)
{
	LONG x, y;
	USHORT depth;

	// Calculate the skeleton's position on the screen
	// NuiTransformSkeletonToDepthImage returns coordinates in NUI_IMAGE_RESOLUTION_320x240 space
	NuiTransformSkeletonToDepthImage(skeletonPoint, &x, &y, &depth);

	float screenPointX = static_cast<float>(x * width) / 320;
	float screenPointY = static_cast<float>(y * height) / 240;

	GLfloat pointIn2D[2] = { screenPointX, screenPointY };

	return pointIn2D;
}
Beispiel #10
0
QPointF MapToScreen(const Vector4 &point)
{
    //@to-do: It's not fixed
    const NUI_IMAGE_RESOLUTION resolution = NUI_IMAGE_RESOLUTION_640x480;
    long x, y;
    ushort depth;

    NuiTransformSkeletonToDepthImage(point, &x, &y, &depth, resolution); // Returns coordinates in depth space

    long backupX = x, backupY = y;
    if (FAILED(NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(resolution, resolution, NULL, x, y, depth, &x, &y))) {
        x = backupX;
        y = backupY;
    }

    return QPointF(x, y);
}
Beispiel #11
0
point moduleCapture::SkeletonPointToScreen(Vector4 skeletonPoint, int height, int width)
{
	LONG x, y;
    USHORT depth;

    // Calculate the skeleton's position on the screen
    // NuiTransformSkeletonToDepthImage returns coordinates in NUI_IMAGE_RESOLUTION_320x240 space
    NuiTransformSkeletonToDepthImage(skeletonPoint, &x, &y, &depth);

    float screenPointX = static_cast<float>(x * width) / 320;
    float screenPointY = static_cast<float>(y * height) / 240;

	point p;
	p.x = screenPointX;
	p.y = screenPointY;
    return p;
}
Beispiel #12
0
	virtual void convertWorldToProjective( const uint32_t deviceID, const uint32_t coordinateCount, const _2RealVector3f* inWorld, _2RealVector3f* outProjective )
	{
		checkDeviceRunning(deviceID, "_2RealImplOpenNI::convertWorldToProjective()" );

		//fetching and writing data
		Vector4 in;
		USHORT depth;
		for( uint32_t i=0; i<coordinateCount; ++i )
		{
			in.x = inWorld[i].x;
			in.y = inWorld[i].y;
			in.z = inWorld[i].z;
			LONG x,y;
			NuiTransformSkeletonToDepthImage( in, &x, &y, &depth );
			outProjective[i].x = (float)x;
			outProjective[i].y = (float)y;
			outProjective[i].z = depth;
		}
	}
void UKinect::drawHand(DWORD ID, int joint, int _catch, bool pressed, bool active, double press)
{
	LONG x, y, xd, yd;
	USHORT depth;
	Vector4 vPoint = {0,0,0,0};

	vector<double> tmp = skeletonJointPosition(ID, joint);
	if (tmp.size()==0) return;

	vPoint.x = tmp[0];
	vPoint.y = tmp[1];
	vPoint.z = tmp[2];

	NuiTransformSkeletonToDepthImage(vPoint, &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
	hr = NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
	if (FAILED(hr)) return;

	if (press>2) press=2; 
	press=press*125;
	
	circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),10,CV_RGB( static_cast<int>(press),static_cast<int>(press),static_cast<int>(press)),-1);

	if (1 == _catch)
	{
		if (pressed){
			circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 0, 255),-1);
		} else if (active) {
			circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 255 ,0),-1);
		} else {
			circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 255, 0, 0),-1);
		}
	}
	else
	{
		if (pressed){
			circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 0, 255),3);
		} else if (active) {
			circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 255, 0),3);
		} else {
			circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 255, 0, 0),3);
		}
	}
}
bool KinectHandler::handleJoints( NUI_SKELETON_DATA& skeletonData )
{
    POINT coordInDepth; USHORT depth = 0;
    OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);
    
    for ( int i=0; i<NUI_SKELETON_POSITION_COUNT; ++i )
    {
        osg::Vec3& point = (*_points)[i];
        NuiTransformSkeletonToDepthImage(
            skeletonData.SkeletonPositions[i], &coordInDepth.x, &coordInDepth.y, &depth, KINECT_IMAGE_RESOLUTION );
        point.x() = (double)coordInDepth.x / KINECT_IMAGE_WIDTH;  // 0.0 - 1.0
        point.y() = (double)coordInDepth.y / KINECT_IMAGE_HEIGHT;  // 0.0 - 1.0
        
        // Transform depth to [0, 1], assuming original depth from 0 to 4000 in millmeters
        point.z() = (double)(depth >> NUI_IMAGE_PLAYER_INDEX_SHIFT);
        point.z() *= 0.00025;  // real value can't be less than 800, that is, 0.2
    }
    _points->dirty();
    return true;
}
vector<double> UKinect::skeletonJointPositionOnImage(DWORD ID, int joint) {
  vector<double> position;

  if (&skeletonFrame != NULL) {
    for (int i = 0; i < NUI_SKELETON_COUNT; i++) {
      const NUI_SKELETON_DATA & skeletonData = skeletonFrame.SkeletonData[i];

      if ((skeletonData.eTrackingState == NUI_SKELETON_TRACKED) && (skeletonData.dwTrackingID == ID) && (skeletonData.eSkeletonPositionTrackingState[joint] >= NUI_SKELETON_POSITION_INFERRED)) {
        LONG x, y, xd, yd;
        USHORT depth;
        NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint], &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
        NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
        position.push_back(x);
        position.push_back(y);
        position.push_back(static_cast<double>(depth));
      }
    }
  }

  return position;
}
/// <summary>
/// Draw a joint of the skeleton
/// </summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth image is streched to fit</param>
/// <param name="joint">Index for the joint to be drawn</param>
void UKinect::drawJoint(const NUI_SKELETON_DATA& skeletonData, NUI_SKELETON_POSITION_INDEX joint) {
  NUI_SKELETON_POSITION_TRACKING_STATE state = skeletonData.eSkeletonPositionTrackingState[joint];

  // Not tracked
  if (NUI_SKELETON_POSITION_NOT_TRACKED == state) {
    return;
  }

  LONG x, y, xd, yd;
  USHORT depth;
  NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint], &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
  NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);

  if (NUI_SKELETON_POSITION_TRACKED == state) {
    circle(skeletonCVMat, Point(static_cast<int>(x), static_cast<int>(y)), 7, CV_RGB(0, 200, 0), -1);
  } else {
    circle(skeletonCVMat, Point(static_cast<int>(x), static_cast<int>(y)), 6, CV_RGB(0, 200, 0), 2);
  }

  // draw "L" and "R" informations near shoulders
  if (joint == 4) putText(skeletonCVMat, lexical_cast<string>("L"), Point(static_cast<int>(x), static_cast<int>(y) - 10), 1, 2, CV_RGB(255, 255, 255), 2);
  if (joint == 8) putText(skeletonCVMat, lexical_cast<string>("R"), Point(static_cast<int>(x) - 13, static_cast<int>(y) - 10), 1, 2, CV_RGB(255, 255, 255), 2);
}
Beispiel #17
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;
            }
        }
    }
}
Beispiel #18
0
int _tmain(int argc, _TCHAR* argv[])
{
	cv::setUseOptimized( true );

	// Kinectのインスタンス生成、初期化
	INuiSensor* pSensor;
	HRESULT hResult = S_OK;
	hResult = NuiCreateSensorByIndex( 0, &pSensor );
	if( FAILED( hResult ) ){
		std::cerr << "Error : NuiCreateSensorByIndex" << std::endl;
		return -1;
	}

	hResult = pSensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON );
	if( FAILED( hResult ) ){
		std::cerr << "Error : NuiInitialize" << std::endl;
		return -1;
	}

	// Colorストリーム
	HANDLE hColorEvent = INVALID_HANDLE_VALUE;
	HANDLE hColorHandle = INVALID_HANDLE_VALUE;
	hColorEvent = CreateEvent( nullptr, true, false, nullptr );
	hResult = pSensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, hColorEvent, &hColorHandle );
	if( FAILED( hResult ) ){
		std::cerr << "Error : NuiImageStreamOpen( COLOR )" << std::endl;
		return -1;
	}

	// Depth&Playerストリーム
	HANDLE hDepthPlayerEvent = INVALID_HANDLE_VALUE;
	HANDLE hDepthPlayerHandle = INVALID_HANDLE_VALUE;
	hDepthPlayerEvent = CreateEvent( nullptr, true, false, nullptr );
	hResult = pSensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_640x480, 0, 2, hDepthPlayerEvent, &hDepthPlayerHandle );
	if( FAILED( hResult ) ){
		std::cerr << "Error : NuiImageStreamOpen( DEPTH&PLAYER )" << std::endl;
		return -1;
	}

	// Skeletonストリーム
	HANDLE hSkeletonEvent = INVALID_HANDLE_VALUE;
	hSkeletonEvent = CreateEvent( nullptr, true, false, nullptr );
	hResult = pSensor->NuiSkeletonTrackingEnable( hSkeletonEvent, 0 );
	if( FAILED( hResult ) ){
		std::cerr << "Error : NuiSkeletonTrackingEnable" << std::endl;
		return -1;
	}

	HANDLE hEvents[3] = { hColorEvent, hDepthPlayerEvent, hSkeletonEvent };

	// カラーテーブル
	cv::Vec3b color[7];
	color[0] = cv::Vec3b(   0,   0,   0 );
	color[1] = cv::Vec3b( 255,   0,   0 );
	color[2] = cv::Vec3b(   0, 255,   0 );
	color[3] = cv::Vec3b(   0,   0, 255 );
	color[4] = cv::Vec3b( 255, 255,   0 );
	color[5] = cv::Vec3b( 255,   0, 255 );
	color[6] = cv::Vec3b(   0, 255, 255 );

	cv::namedWindow( "Color" );
	cv::namedWindow( "Depth" );
	cv::namedWindow( "Player" );
	cv::namedWindow( "Skeleton" );

	while( 1 ){
		// フレームの更新待ち
		ResetEvent( hColorEvent );
		ResetEvent( hDepthPlayerEvent );
		ResetEvent( hSkeletonEvent );
		WaitForMultipleObjects( ARRAYSIZE( hEvents ), hEvents, true, INFINITE );

		// Colorカメラからフレームを取得
		NUI_IMAGE_FRAME pColorImageFrame = { 0 };
		hResult = pSensor->NuiImageStreamGetNextFrame( hColorHandle, 0, &pColorImageFrame );
		if( FAILED( hResult ) ){
			std::cerr << "Error : NuiImageStreamGetNextFrame( COLOR )" << std::endl;
			return -1;
		}

		// Depthセンサーからフレームを取得
		NUI_IMAGE_FRAME pDepthPlayerImageFrame = { 0 };
		hResult = pSensor->NuiImageStreamGetNextFrame( hDepthPlayerHandle, 0, &pDepthPlayerImageFrame );
		if( FAILED( hResult ) ){
			std::cerr << "Error : NuiImageStreamGetNextFrame( DEPTH&PLAYER )" << std::endl;
			return -1;
		}

		// Skeletonフレームを取得
		NUI_SKELETON_FRAME pSkeletonFrame = { 0 };
		hResult = pSensor->NuiSkeletonGetNextFrame( 0, &pSkeletonFrame );
		if( FAILED( hResult ) ){
			std::cout << "Error : NuiSkeletonGetNextFrame" << std::endl;
			return -1;
		}

		// Color画像データの取得
		INuiFrameTexture* pColorFrameTexture = pColorImageFrame.pFrameTexture;
		NUI_LOCKED_RECT sColorLockedRect;
		pColorFrameTexture->LockRect( 0, &sColorLockedRect, nullptr, 0 );

		// Depthデータの取得
		INuiFrameTexture* pDepthPlayerFrameTexture = pDepthPlayerImageFrame.pFrameTexture;
		NUI_LOCKED_RECT sDepthPlayerLockedRect;
		pDepthPlayerFrameTexture->LockRect( 0, &sDepthPlayerLockedRect, nullptr, 0 );

		// 表示
		cv::Mat colorMat( 480, 640, CV_8UC4, reinterpret_cast<uchar*>( sColorLockedRect.pBits ) );

		LONG registX = 0;
		LONG registY = 0;
		ushort* pBuffer = reinterpret_cast<ushort*>( sDepthPlayerLockedRect.pBits );
		cv::Mat bufferMat = cv::Mat::zeros( 480, 640, CV_16UC1 );
		cv::Mat playerMat = cv::Mat::zeros( 480, 640, CV_8UC3 );
		for( int y = 0; y < 480; y++ ){
			for( int x = 0; x < 640; x++ ){
				pSensor->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution( NUI_IMAGE_RESOLUTION_640x480, NUI_IMAGE_RESOLUTION_640x480, nullptr, x, y, *pBuffer, &registX, &registY );
				if( ( registX >= 0 ) && ( registX < 640 ) && ( registY >= 0 ) && ( registY < 480 ) ){
					bufferMat.at<ushort>( registY, registX ) = *pBuffer & 0xFFF8;
					playerMat.at<cv::Vec3b>( registY, registX ) = color[*pBuffer & 0x7];
				}
				pBuffer++;
			}
		}
		cv::Mat depthMat( 480, 640, CV_8UC1 );
		bufferMat.convertTo( depthMat, CV_8UC3, -255.0f / NUI_IMAGE_DEPTH_MAXIMUM, 255.0f );

		cv::Mat skeletonMat = cv::Mat::zeros( 480, 640, CV_8UC3 );
		cv::Point2f point;
		for( int count = 0; count < NUI_SKELETON_COUNT; count++ ){
			NUI_SKELETON_DATA skeleton = pSkeletonFrame.SkeletonData[count];
			if( skeleton.eTrackingState == NUI_SKELETON_TRACKED ){
				for( int position = 0; position < NUI_SKELETON_POSITION_COUNT; position++ ){
					NuiTransformSkeletonToDepthImage( skeleton.SkeletonPositions[position], &point.x, &point.y, NUI_IMAGE_RESOLUTION_640x480 );
					cv::circle( skeletonMat, point, 10, static_cast<cv::Scalar>( color[count + 1] ), -1, CV_AA );
				}
			}
		}
		
		cv::imshow( "Color", colorMat );
		cv::imshow( "Depth", depthMat );
		cv::imshow( "Player", playerMat );
		cv::imshow( "Skeleton", skeletonMat );

		// フレームの解放
		pColorFrameTexture->UnlockRect( 0 );
		pDepthPlayerFrameTexture->UnlockRect( 0 );
		pSensor->NuiImageStreamReleaseFrame( hColorHandle, &pColorImageFrame );
		pSensor->NuiImageStreamReleaseFrame( hDepthPlayerHandle, &pDepthPlayerImageFrame );

		// ループの終了判定(Escキー)
		if( cv::waitKey( 30 ) == VK_ESCAPE ){
			break;
		}
	}

	// Kinectの終了処理
	pSensor->NuiShutdown();
	pSensor->NuiSkeletonTrackingDisable();
	CloseHandle( hColorEvent );
	CloseHandle( hDepthPlayerEvent );
	CloseHandle( hSkeletonEvent );
	CloseHandle( hColorHandle );
	CloseHandle( hDepthPlayerHandle );

	cv::destroyAllWindows();

	return 0;
}
//-------------------------------------------------------------------
// 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 );
	}

}
void MultiCursorAppCpp::detectHandPosition(CvBlobs blobs)
{
	FLOAT offset = 0.03f;
	INT blobID = 0;
	for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) {
		int numIntersectionPoints = 0;
		Vector4 handPosition;
		handPosition.w = 1;
		handPosition.x = 0.0f;
		handPosition.y = 0.0f;
		handPosition.z = 0.0f;
		Point3f center3f = Point3_<FLOAT>(userData[blobID].headInfo.cameraPoint.x, userData[blobID].headInfo.cameraPoint.y, userData[blobID].headInfo.cameraPoint.z);

		// ユーザの領域内を探索
		for (int y = it->second->miny; y <= it->second->maxy; y++) {
			for (int x = it->second->minx; x <= it->second->maxx; x++)
			{
				float length = sqrt(
					pow(center3f.x - point3fMatrix.ptr<float>(y, x)[0], 2)
					+ pow(center3f.y - point3fMatrix.ptr<float>(y, x)[1], 2)
					//+ pow(center3f.z - point3fMatrix.ptr<float>(y, x)[2], 2)
					);
				// Define the intersection point of the sphere which its center is head and the hand as the hand position 
				if (*heightMatrix.ptr<USHORT>(y, x) > userData[blobID].headInfo.height - HEAD_LENGTH - SHOULDER_LENGTH	// 肩より高い点か
					&& 0 < point3fMatrix.ptr<float>(y, x)[2]*1000 && point3fMatrix.ptr<float>(y, x)[2] * 1000 < KINECT_HEIGHT	// Kinectの検出できる範囲の値か
					&& it->first == labelMat.at<unsigned long>(y, x)	// 同じblob内のみ探索
					) // Don't include desk
				{
					if (SENCIG_CIRCLE_RADIUS - offset < length ) {	// 注目点が球と交差しているかどうか
						handPosition.x += point3fMatrix.ptr<float>(y, x)[0];
						handPosition.y += point3fMatrix.ptr<float>(y, x)[1];
						handPosition.z += point3fMatrix.ptr<float>(y, x)[2];

						circle(userAreaMat, Point(x, y), 2, Scalar(255, 255, 0), -1);
						numIntersectionPoints++;
					}
					else if (length > SENCIG_CIRCLE_RADIUS) {
#ifdef TRACK_GESTURE_BY_AREA
						++userData[blobID].handInfo.area;	// 手の面積を点の数で表す

#ifdef USE_KINECT_V1
						LONG handPositionX2d;
						LONG handPositionY2d;
						USHORT dis;
						NuiTransformSkeletonToDepthImage(handPosition, &handPositionX2d, &handPositionY2d, &dis, KINECT_RESOLUTION);
						circle(userAreaMat, Point(handPositionX2d, handPositionY2d), 7, Scalar(0, 200, 0), 3);
#else
						DepthSpacePoint depthPoint;
						CameraSpacePoint cameraPoint;
						cameraPoint.X = point3fMatrix.ptr<float>(y, x)[0];
						cameraPoint.Y = point3fMatrix.ptr<float>(y, x)[1];
						cameraPoint.Z = point3fMatrix.ptr<float>(y, x)[2];
						kinectBasics.GetMapper()->MapCameraPointToDepthSpace(cameraPoint, &depthPoint);

						int index = ((y * kinectBasics.widthDepth) + x) * 3;
						UCHAR* dataDepth = &userAreaMat.data[index];
						dataDepth[0] = 0;
						dataDepth[1] = 200;
						dataDepth[2] = 255;
						//circle(userAreaMat, Point(depthPoint.X, depthPoint.Y), 1, Scalar(0, 255, 255), 3);
#endif
#endif
					}
				}
			}
		}


		if (numIntersectionPoints > 0)
		{
			userData[blobID].handInfo.isTracked = true;

			handPosition.x /= numIntersectionPoints;
			handPosition.y /= numIntersectionPoints;
			handPosition.z /= numIntersectionPoints;

			userData[blobID].handInfo.cameraPoint.x = handPosition.x;
			userData[blobID].handInfo.cameraPoint.y = handPosition.y;
			userData[blobID].handInfo.cameraPoint.z = handPosition.z;

			// Show the hand positions on the depth image
#ifdef USE_KINECT_V1
			LONG handPositionX2d;
			LONG handPositionY2d;
			USHORT dis;
			NuiTransformSkeletonToDepthImage(handPosition, &handPositionX2d, &handPositionY2d, &dis, KINECT_RESOLUTION);
			circle(userAreaMat, Point(handPositionX2d, handPositionY2d), 7, Scalar(0, 200, 0), 3);
#else
			DepthSpacePoint depthPoint;
			CameraSpacePoint cameraPoint;
			cameraPoint.X = handPosition.x;
			cameraPoint.Y = handPosition.y;
			cameraPoint.Z = handPosition.z;
			kinectBasics.GetMapper()->MapCameraPointToDepthSpace(cameraPoint, &depthPoint);
			circle(userAreaMat, Point(depthPoint.X, depthPoint.Y), 7, Scalar(0, 200, 0), 3);
#endif
		}
		else
		{
			userData[blobID].handInfo.isTracked = false;

			userData[blobID].handInfo.cameraPoint.x = 0.0f;
			userData[blobID].handInfo.cameraPoint.y = 0.0f;
			userData[blobID].handInfo.cameraPoint.z = 0.0f;
		}

#ifdef TRACK_GESTURE_BY_AREA
		// 面積で手の開閉をチェック
		if (userData[blobID].handInfo.area != 0) {
			float distance = sqrt(pow(handPosition.x, 2) + pow(handPosition.y, 2) + pow(handPosition.z, 2));
			userData[blobID].handInfo.area /= distance;
		}
		// cout << "area: " << userData[blobID].handInfo.area << endl;
#endif


		blobID++;
	}

	// Replace preUserData by current userData	/ 前フレームのデータを現フレームのデータで置き換える
	preUserData.clear();
	preUserData = userData;
	for (vector<UserData>::iterator p = preUserData.begin(); p != preUserData.end(); p++)
	{
		p->isDataFound = false; // 初期化
	}
}
Beispiel #21
0
void KinectImpl::drawNuiSkeleton(int width, int height, int playerID)
{
	int scaleX = width;
	int scaleY = height;
	long x=0,y=0;
	unsigned short depth=0;
	float fx=0,fy=0;
	long cx=0,cy=0;
	float display_posf[NUI_SKELETON_POSITION_COUNT][2];

	for (int i = 0; i < NUI_SKELETON_POSITION_COUNT; i++)
	{
		switch(showedTextureIndex){
			case IMAGE_TEXTURE:
				NuiTransformSkeletonToDepthImage( skels[playerID][i], &x, &y, &depth);
				NuiImageGetColorPixelCoordinatesFromDepthPixel(NUI_IMAGE_RESOLUTION_640x480, NULL, x, y, depth, &cx, &cy);
				display_posf[i][0] = 1.0f - (cx / 640.0f) * 2.0f;
				display_posf[i][1] = 1.0f - (cy / 480.0f) * 2.0f;
				break;
			case DEPTH_TEXTURE:
				NuiTransformSkeletonToDepthImage( skels[playerID][i], &fx, &fy);
				display_posf[i][0] = 1.0f - fx / 320.0f * 2.0f;
				display_posf[i][1] = 1.0f - fy / 240.0f * 2.0f;
				break;
			default:
				break;
		}
	}

	glColor3ub(255, 255, 0);
	glLineWidth(6);
	glBegin(GL_LINE_STRIP);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HIP_CENTER][0], display_posf[NUI_SKELETON_POSITION_HIP_CENTER][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_SPINE][0], display_posf[NUI_SKELETON_POSITION_SPINE][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_SHOULDER_CENTER][0], display_posf[NUI_SKELETON_POSITION_SHOULDER_CENTER][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HEAD][0], display_posf[NUI_SKELETON_POSITION_HEAD][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_SHOULDER_CENTER][0], display_posf[NUI_SKELETON_POSITION_SHOULDER_CENTER][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_SHOULDER_LEFT][0], display_posf[NUI_SKELETON_POSITION_SHOULDER_LEFT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_ELBOW_LEFT][0], display_posf[NUI_SKELETON_POSITION_ELBOW_LEFT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_WRIST_LEFT][0], display_posf[NUI_SKELETON_POSITION_WRIST_LEFT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HAND_LEFT][0], display_posf[NUI_SKELETON_POSITION_HAND_LEFT][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_SHOULDER_CENTER][0], display_posf[NUI_SKELETON_POSITION_SHOULDER_CENTER][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_SHOULDER_RIGHT][0], display_posf[NUI_SKELETON_POSITION_SHOULDER_RIGHT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_ELBOW_RIGHT][0], display_posf[NUI_SKELETON_POSITION_ELBOW_RIGHT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_WRIST_RIGHT][0], display_posf[NUI_SKELETON_POSITION_WRIST_RIGHT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HAND_RIGHT][0], display_posf[NUI_SKELETON_POSITION_HAND_RIGHT][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HIP_CENTER][0], display_posf[NUI_SKELETON_POSITION_HIP_CENTER][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HIP_LEFT][0], display_posf[NUI_SKELETON_POSITION_HIP_LEFT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_KNEE_LEFT][0], display_posf[NUI_SKELETON_POSITION_KNEE_LEFT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_ANKLE_LEFT][0], display_posf[NUI_SKELETON_POSITION_ANKLE_LEFT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_FOOT_LEFT][0], display_posf[NUI_SKELETON_POSITION_FOOT_LEFT][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HIP_CENTER][0], display_posf[NUI_SKELETON_POSITION_HIP_CENTER][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_HIP_RIGHT][0], display_posf[NUI_SKELETON_POSITION_HIP_RIGHT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_KNEE_RIGHT][0], display_posf[NUI_SKELETON_POSITION_KNEE_RIGHT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_ANKLE_RIGHT][0], display_posf[NUI_SKELETON_POSITION_ANKLE_RIGHT][1]);
		glVertex2f( display_posf[NUI_SKELETON_POSITION_FOOT_RIGHT][0], display_posf[NUI_SKELETON_POSITION_FOOT_RIGHT][1]);
	glEnd();
	glColor3ub(0, 0, 0);
}
Beispiel #22
0
void measureBody(NUI_Controller* mNui)
{
	//Head Width Measurement
	NUI_Vector4 head,neck,lShoulder,rShoulder,lFoot,rFoot,lHip,rHip,lElbow,rElbow,lHand,rHand,torso ;
	head=mNui->m_Points[NUI_SKELETON_POSITION_HEAD];
	LONG projHeadX,projHeadY;
	USHORT depth;
	NuiTransformSkeletonToDepthImage( head, &projHeadX, &projHeadY, &depth ,mNui->m_DepthResolution);

	UCHAR* headPtr=(UCHAR*)( uImage->imageData+projHeadY*uImage->widthStep+projHeadX);
	USHORT* headDepthPtr=(USHORT*) (dImage->imageData+projHeadY*dImage->widthStep+projHeadX*2);
	UCHAR* iPtr=headPtr;
	LONG leftX=projHeadX;
	LONG rightX=projHeadX;
	LONG leftStep=0;
	LONG rightStep=0;
	while(*(--iPtr)>0 && 0<leftX)
	{
		leftX--;	//Extend the line horizontally until it reaches the borders of the head.
		leftStep++;
	}
	iPtr=headPtr;
	while(*(++iPtr)>0 && rightX<(m_Width-1))
	{
		rightX++;
		rightStep++;
	}

	NUI_Vector4 leftHead=NuiTransformDepthImageToSkeleton(leftX,projHeadY,*(headDepthPtr-leftStep),mNui->m_DepthResolution);
	NUI_Vector4 rightHead=NuiTransformDepthImageToSkeleton(rightX,projHeadY,*(headDepthPtr+leftStep),mNui->m_DepthResolution);

	bodyMeasurements[HEAD_WIDTH]=abs(rightHead.x-leftHead.y);

	//Head Height Measurement
	
	neck=mNui->m_Points[NUI_SKELETON_POSITION_SHOULDER_CENTER];
	bodyMeasurements[HEAD_HEIGHT]=abs(head.y-neck.y);

	//Body Height Measurement
	
	lFoot=mNui->m_Points[NUI_SKELETON_POSITION_FOOT_LEFT];
	rFoot=mNui->m_Points[NUI_SKELETON_POSITION_FOOT_RIGHT];
	float lowPointY=(lFoot.y+rFoot.y)/2;
	
	iPtr=headPtr-uImage->widthStep;	//Initialize the pointer 1 pixel above, since decrement will take place after comparison.
	LONG topY;
	int topStep=0;
	while(*iPtr>0 && (topY>0))
	{
		iPtr-=uImage->widthStep;
		topY--;
		topStep++;
	}
	NUI_Vector4 topPoint=NuiTransformDepthImageToSkeleton(projHeadX,topY,*(headDepthPtr - topStep*dImage->widthStep/2),mNui->m_DepthResolution);
	bodyMeasurements[BODY_HEIGHT]=abs(topPoint.y-lowPointY);

	//Hip Height Measurement

	lHip=mNui->m_Points[NUI_SKELETON_POSITION_HIP_LEFT];
	rHip=mNui->m_Points[NUI_SKELETON_POSITION_HIP_RIGHT];
	bodyMeasurements[HIP_HEIGHT]=(abs(lHip.y-lFoot.y)+abs(rHip.y-lHip.y))/2;

	//Elbow-Fingertip Measurement
	lElbow=mNui->m_Points[NUI_SKELETON_POSITION_ELBOW_LEFT];
	rElbow=mNui->m_Points[NUI_SKELETON_POSITION_ELBOW_RIGHT];
	lHand=mNui->m_Points[NUI_SKELETON_POSITION_HAND_LEFT];
	rHand=mNui->m_Points[NUI_SKELETON_POSITION_HAND_RIGHT];


	LONG lHandUpX,rHandUpX,lHandDownX,rHandDownX,lHandUpY,lHandDownY,rHandUpY,rHandDownY;
	NuiTransformSkeletonToDepthImage( lHand, &lHandUpX, &lHandUpY, &depth ,mNui->m_DepthResolution);
	NuiTransformSkeletonToDepthImage( rHand, &rHandUpX, &rHandUpY, &depth ,mNui->m_DepthResolution);
	NuiTransformSkeletonToDepthImage( lElbow, &lHandDownX, &lHandDownY, &depth ,mNui->m_DepthResolution);
	NuiTransformSkeletonToDepthImage( rElbow, &rHandDownX, &rHandDownY, &depth ,mNui->m_DepthResolution);

	UCHAR* lHandPtr=(UCHAR*)( uImage->imageData+(int)(lHandUpY-1)*uImage->widthStep+(int)lHandUpX);//Initialize the pointer 1 pixel above, since decrement will take place after comparison.
	while(*lHandPtr>0  && (lHandUpY>0))
	{
		lHandPtr-=uImage->widthStep;
		lHandUpY--;	
	}	//Extend the line vertically until it reaches the borders of the arm.


	UCHAR* lElbowPtr=(UCHAR*) (uImage->imageData+(int)(lHandDownY+1)*uImage->widthStep+(int)lHandDownX);//Initialize the pointer 1 pixel above, since decrement will take place after comparison.
	while(*lElbowPtr>0  && (lHandDownY<(m_Width-1)))
	{
		lElbowPtr+=uImage->widthStep;
		lHandDownY++;	
	}	//Extend the line vertically until it reaches the borders of the arm.

	UCHAR* rHandPtr=(UCHAR*) (uImage->imageData+(int)(rHandUpY-1)*uImage->widthStep+(int)rHandUpX);//Initialize the pointer 1 pixel above, since decrement will take place after comparison.
	while(*rHandPtr>0  && (rHandUpY>0))
	{
		rHandPtr-=uImage->widthStep;
		rHandUpY--;	
	}	//Extend the line vertically until it reaches the borders of the arm.

	UCHAR* rElbowPtr=(UCHAR*)(uImage->imageData+(int)(rHandDownY+1)*uImage->widthStep+(int)rHandDownX);//Initialize the pointer 1 pixel above, since decrement will take place after comparison.
	while(*rElbowPtr>0  && (rHandDownY<(m_Height-1)))
	{
		rElbowPtr+=uImage->widthStep;
		rHandDownY++;	
	}	//Extend the line vertically until it reaches the borders of the arm.

	USHORT* depthPtr=(USHORT*) (dImage->imageData);
	NUI_Vector4 leftArmDown=NuiTransformDepthImageToSkeleton(lHandDownX,lHandDownY,*(depthPtr+lHandDownY*dImage->widthStep/2+lHandDownX),mNui->m_DepthResolution);
	NUI_Vector4 leftArmUp=NuiTransformDepthImageToSkeleton(lHandUpX,lHandUpY,*(depthPtr+lHandUpY*dImage->widthStep/2+lHandUpX),mNui->m_DepthResolution);
	NUI_Vector4 rightArmDown=NuiTransformDepthImageToSkeleton(rHandDownX,rHandDownY,*(depthPtr+rHandDownY*dImage->widthStep/2+rHandDownX),mNui->m_DepthResolution);
	NUI_Vector4 rightArmUp=NuiTransformDepthImageToSkeleton(rHandUpX,rHandUpY,*(depthPtr+rHandUpY*dImage->widthStep/2+rHandUpX),mNui->m_DepthResolution);
	bodyMeasurements[ELBOW_FINGERTIP]=(abs(rightArmUp.y-rightArmDown.y)+abs(leftArmUp.y-leftArmDown.y))/2;
	//Wrist to Fingertip Measurement
	bodyMeasurements[WRIST_FINGERTIP]=abs(leftArmUp.y-mNui->m_Points[NUI_SKELETON_POSITION_WRIST_LEFT].y)+abs(rightArmUp.y-mNui->m_Points[NUI_SKELETON_POSITION_WRIST_RIGHT].y)/2;

	//SHoulder Width Measurement
	lShoulder=mNui->m_Points[NUI_SKELETON_POSITION_SHOULDER_LEFT];
	rShoulder=mNui->m_Points[NUI_SKELETON_POSITION_SHOULDER_RIGHT];
	bodyMeasurements[SHOULDER_WIDTH]=abs(rShoulder.x-lShoulder.x);

	//Hip Width Measurement
	LONG lHipX,lHipY,rHipX,rHipY;
	NuiTransformSkeletonToDepthImage(lHip,&lHipX,&lHipY,&depth,mNui->m_DepthResolution);
	NuiTransformSkeletonToDepthImage(rHip,&rHipX,&rHipY,&depth,mNui->m_DepthResolution);

	UCHAR* lEndPtr=(UCHAR*)(uImage->imageData+(int)lHipY*uImage->widthStep+(int)lHipX);
	leftX=lHipX;
	leftStep=0;
	while(*(--lEndPtr)>0 && leftX>0)
	{
		leftX--;	//Extend the line horizontally until it reaches the borders of the head.
		leftStep++;
	}
	NUI_Vector4 leftHipEnd=NuiTransformDepthImageToSkeleton(leftX,lHipY,*((USHORT*)(dImage->imageData+(int)lHipY*dImage->widthStep+(int)leftX*2)),mNui->m_DepthResolution);

	UCHAR* rEndPtr=(UCHAR*) (uImage->imageData+(int)rHipY*uImage->widthStep+(int)rHipX);
	rightX=rHipX;
	rightStep=0;
	while(*(++rEndPtr)>0 && rightX<(m_Width-1))
	{
		rightX++;	//Extend the line horizontally until it reaches the borders of the head.
		rightStep++;
	}
	NUI_Vector4 rightHipEnd=NuiTransformDepthImageToSkeleton(rightX,rHipY,*((USHORT*)(dImage->imageData+(int)rHipY*dImage->widthStep+(int)rightX*2)),mNui->m_DepthResolution);
	bodyMeasurements[HIP_WIDTH]=abs(rightHipEnd.x-leftHipEnd.x);

	//Torso Height Measurement

	torso=mNui->m_Points[NUI_SKELETON_POSITION_SPINE];
	bodyMeasurements[TORSO_HEIGHT]=abs(torso.y-lowPointY);


}
Beispiel #23
0
void drawNuiSkeleton(int playerID)
{
	int scaleX = DEFAULT_WIDTH;
	int scaleY = DEFAULT_HEIGHT;
	long x=0,y=0;
	unsigned short depth=0;
	float fx=0,fy=0;
	long cx=0,cy=0;
	int display_pos[NUI_SKELETON_POSITION_COUNT][2];

	for (int i = 0; i < NUI_SKELETON_POSITION_COUNT; i++)
	{
		// Overlay on depth image
		//NuiTransformSkeletonToDepthImage( skels[playerID][i], &fx, &fy);
		//display_pos[i][0] = (int) ( fx / 320.0 * DEFAULT_WIDTH);
		//display_pos[i][1] = (int) ( fy / 240.0 * DEFAULT_HEIGHT);

		// Overlay on color image
		NuiTransformSkeletonToDepthImage( skels[playerID][i], &x, &y, &depth);
		NuiImageGetColorPixelCoordinatesFromDepthPixel(NUI_IMAGE_RESOLUTION_640x480, NULL, x, y, depth, &cx, &cy);
		display_pos[i][0] = (int) cx;
		display_pos[i][1] = (int) cy;
	}

	glColor3ub(255, 255, 0);
	glLineWidth(6);
	glBegin(GL_LINE_STRIP);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HIP_CENTER][0], scaleY - display_pos[NUI_SKELETON_POSITION_HIP_CENTER][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_SPINE][0], scaleY - display_pos[NUI_SKELETON_POSITION_SPINE][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_SHOULDER_CENTER][0], scaleY - display_pos[NUI_SKELETON_POSITION_SHOULDER_CENTER][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HEAD][0], scaleY - display_pos[NUI_SKELETON_POSITION_HEAD][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_SHOULDER_CENTER][0], scaleY - display_pos[NUI_SKELETON_POSITION_SHOULDER_CENTER][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_SHOULDER_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_SHOULDER_LEFT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_ELBOW_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_ELBOW_LEFT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_WRIST_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_WRIST_LEFT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HAND_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_HAND_LEFT][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_SHOULDER_CENTER][0], scaleY - display_pos[NUI_SKELETON_POSITION_SHOULDER_CENTER][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_SHOULDER_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_SHOULDER_RIGHT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_ELBOW_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_ELBOW_RIGHT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_WRIST_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_WRIST_RIGHT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HAND_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_HAND_RIGHT][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HIP_CENTER][0], scaleY - display_pos[NUI_SKELETON_POSITION_HIP_CENTER][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HIP_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_HIP_LEFT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_KNEE_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_KNEE_LEFT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_ANKLE_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_ANKLE_LEFT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_FOOT_LEFT][0], scaleY - display_pos[NUI_SKELETON_POSITION_FOOT_LEFT][1]);
	glEnd();
	glBegin(GL_LINE_STRIP);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HIP_CENTER][0], scaleY - display_pos[NUI_SKELETON_POSITION_HIP_CENTER][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_HIP_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_HIP_RIGHT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_KNEE_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_KNEE_RIGHT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_ANKLE_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_ANKLE_RIGHT][1]);
		glVertex2i( scaleX - display_pos[NUI_SKELETON_POSITION_FOOT_RIGHT][0], scaleY - display_pos[NUI_SKELETON_POSITION_FOOT_RIGHT][1]);
	glEnd();
	glColor3ub(0, 0, 0);
}
Beispiel #24
0
void getSphereSizes(NUI_Controller* mNui)
{
	int x_init=0;
	int y_init=0;
	int step=0;
	try 
	{
		for (int i=0;i<16;i++)
		{
			NUI_SKELETON_POSITION_INDEX sJoint=nuiIDs[i];
			for (int j=0;j<i;j++)				//If joint is processed before, do not repeat it
			{
				if (nuiIDs[j]==sJoint)
					sphereRadii[i]=sphereRadii[j];
			}

			NUI_Vector4 realPosition=mNui->m_Points[sJoint];
			Vector2 endOfJoint;
			LONG x,y;
			USHORT depth;				
			NuiTransformSkeletonToDepthImage( realPosition, &x, &y, &depth ,mNui->m_DepthResolution);
			x_init=x;
			y_init=y;
			step=0;
			int radius=0;
			//cvShowImage(windowName.c_str(),uImage);
			//cvSetMouseCallback(windowName.c_str(), mouseEvent, 0);
			//cvWaitKey();
		
			endOfJoint.x=x;
			endOfJoint.y=x;

			UCHAR* iPtr=(UCHAR*)(uImage->imageData+y_init*uImage->widthStep+x_init);
			USHORT* dPtr=(USHORT*)(dImage->imageData+y_init*dImage->widthStep+x_init*2);//Multipy x_init by 2, since dImage is 16 bits- 2bytes
			while( step<m_Width)					//Slowly enlarge the joint sphere until it reaches the end of a bone in one direction.
			{			
				if (x_init-step>-1)
				{
					UCHAR tValue=*(iPtr-step);
					if( tValue!=0 )
					{
						endOfJoint.x-=(step-1);
						NUI_Vector4 trueEnd=NuiTransformDepthImageToSkeleton(endOfJoint.x,endOfJoint.y,*(dPtr-(step-1)),mNui->m_DepthResolution);
						radius=abs(realPosition.x-trueEnd.x);
						break;
					}
				}
				if (x_init+step<m_Width)
				{
					UCHAR tValue=*(iPtr+step);
					if( tValue!=0)
					{

						endOfJoint.x+=(step-1);
						NUI_Vector4 trueEnd=NuiTransformDepthImageToSkeleton(endOfJoint.x,endOfJoint.y,*(dPtr+(step-1)),mNui->m_DepthResolution);
						radius=abs(realPosition.x-trueEnd.x);
						break;
					}
				}
				if (step<m_Height)
				{
					if (y_init-step>-1)
					{
						UCHAR tValue=*(iPtr-step*uImage->widthStep);
						if(  tValue!=0 )
						{
							endOfJoint.y-=(step-1);
							NUI_Vector4 trueEnd=NuiTransformDepthImageToSkeleton(endOfJoint.x,endOfJoint.y,*(dPtr-(step-1)*dImage->widthStep/2),mNui->m_DepthResolution);
							radius=abs(realPosition.y-trueEnd.y);
							break;
						}
					}
					if (y_init+step<m_Height)
					{
						UCHAR tValue=*(iPtr+step*uImage->widthStep);
						if( tValue!=0)
						{
							endOfJoint.y+=(step-1);
							NUI_Vector4 trueEnd=NuiTransformDepthImageToSkeleton(endOfJoint.x,endOfJoint.y,*(dPtr+(step-1)*dImage->widthStep/2),mNui->m_DepthResolution);
							radius=abs(realPosition.y-trueEnd.y);
							break;
						}
					}
				}
				step++;
			}
			sphereRadii[i]=radius;
		}
	}
	catch( Ogre::Exception& e ) {
			MessageBox( NULL, e.getFullDescription().c_str(), "An exception has occured!", MB_OK | MB_ICONERROR | MB_TASKMODAL);

	}
	catch(cv::Exception e) {
		MessageBox( NULL, e.err.c_str(), "An exception has occured!", MB_OK | MB_ICONERROR | MB_TASKMODAL);

	}
}