int GenerateMinecraftCharacter(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, const XnRGB24Pixel* image)
{
    int ret = 0;
    int xRes = dmd.XRes();
    int yRes = dmd.YRes();
    
    cv::Mat inputImage = cv::Mat(yRes, xRes, CV_8UC3);
    cv::Mat skin = cv::Mat::zeros(cv::Size(64,32), CV_8UC3);
    XnToCV(image,&inputImage);
    cv::cvtColor(inputImage,inputImage,CV_RGB2BGR);
    
    
    XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	int i = 0;
	for (i = 0; i < nUsers; ++i) {
	    if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) break;
	}
	
	// No users being tracked
	if (i == nUsers) return -1;
	
	ret = GenerateSkin(aUsers[i], &inputImage, &skin);
	printf("GenerateSkin returned %d on user %d\n",ret,(int)aUsers[i]);
	cv::imwrite("skin.png",skin);
	SegmentUser(aUsers[i], &inputImage, smd);
	DrawDebugPoints(aUsers[i], &inputImage);
	cv::imwrite("blah.png",inputImage);
	sync();
	system("convert skin.png -transparent black skin.png && composite -geometry +32+0 hardhat.png skin.png skin.png");
	sync();
	
	return ret;
}
示例#2
0
void send(const xn::DepthMetaData& dmd)
{
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	userGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		memset(&SendBuf, '\0', BufLen); // this may be expensive
		if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) 
		{
			strcpy(&SendBuf[0], "sensor-update");
			for(int j = 0; j < 24 ; j++)
			{
				if(jointPos(aUsers[i], jointList[j]) == 0)	
				{
					sprintf(&SendBuf[strlen(SendBuf)], " \"%s_x\" %f", jointNames[j], jointCoords[1]); // I know, I know :(
					sprintf(&SendBuf[strlen(SendBuf)], " \"%s_y\" %f", jointNames[j], jointCoords[2]);
				}
			}
			// Before we send each bundle, we need to send some size code
			unsigned char sizeBytes[4];
			//strcpy(SendBuf, P.c_str());
			int len = strlen(SendBuf);
			
			sizeBytes[0] =(unsigned char)( len >> 24 );
			sizeBytes[1] =(unsigned char)( (len << 8) >> 24 );
			sizeBytes[2] =(unsigned char)( (len << 16) >> 24 );
			sizeBytes[3] =(unsigned char)( (len << 24) >> 24 );
			if (debug) 
				printf("%s\n\n", SendBuf);
			sendto(SendSocket, (char*)sizeBytes, 4, 0, (struct sockaddr *) &RecvAddr, sizeof(RecvAddr));
			sendto(SendSocket, SendBuf, strlen(SendBuf), 0, (struct sockaddr *) &RecvAddr, sizeof(RecvAddr));
			
		}
	}
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
  if (bSuccess)
  {
    ROS_INFO("Calibration complete, start tracking user %d", nId);
    g_UserGenerator.GetSkeletonCap().StartTracking(nId);

    if (default_user == 0) // set default user if still unset
    {
      default_user = nId;
      ROS_INFO_STREAM("OpenNI tracker: Default user has been initialised with user " << default_user << ".");
    }
    std_msgs::UInt16MultiArray tracked_users;
    XnUserID users[15];
    XnUInt16 users_count = 15;
    g_UserGenerator.GetUsers(users, users_count);
    for (unsigned int user = 0; user < users_count; ++user)
    {
      tracked_users.data.push_back(users[user]);
    }
    available_tracked_users_pub.publish(tracked_users);
  }
  else
  {
    ROS_INFO("Calibration failed for user %d", nId);
    if (g_bNeedPose)
    {
      g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
    }
    else
    {
      g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
    }
  }
}
void publishTransforms(const std::string& frame_id) {
    XnUserID users[15];
    XnUInt16 users_count = 15;
    g_UserGenerator.GetUsers(users, users_count);

    for (int i = 0; i < users_count; ++i) {
        XnUserID user = users[i];
        if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
            continue;


        publishTransform(user, XN_SKEL_HEAD,           frame_id, "head2");
        publishTransform(user, XN_SKEL_NECK,           frame_id, "neck2");
        publishTransform(user, XN_SKEL_TORSO,          frame_id, "torso2");

        publishTransform(user, XN_SKEL_LEFT_SHOULDER,  frame_id, "left_shoulder2");
        publishTransform(user, XN_SKEL_LEFT_ELBOW,     frame_id, "left_elbow2");
        publishTransform(user, XN_SKEL_LEFT_HAND,      frame_id, "left_hand2");

        publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder2");
        publishTransform(user, XN_SKEL_RIGHT_ELBOW,    frame_id, "right_elbow2");
        publishTransform(user, XN_SKEL_RIGHT_HAND,     frame_id, "right_hand2");

        publishTransform(user, XN_SKEL_LEFT_HIP,       frame_id, "left_hip2");
        publishTransform(user, XN_SKEL_LEFT_KNEE,      frame_id, "left_knee2");
        publishTransform(user, XN_SKEL_LEFT_FOOT,      frame_id, "left_foot2");

        publishTransform(user, XN_SKEL_RIGHT_HIP,      frame_id, "right_hip2");
        publishTransform(user, XN_SKEL_RIGHT_KNEE,     frame_id, "right_knee2");
        publishTransform(user, XN_SKEL_RIGHT_FOOT,     frame_id, "right_foot2");
    }
}
示例#5
0
// Callback: New user was detected
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
    XnUInt32 epochTime = 0;
    xnOSGetEpochTime(&epochTime);
    XnUserID aUsers[MAX_NUM_USERS];
    XnUInt16 nUsers;
	g_UserGenerator.GetUsers(aUsers, nUsers);
    printf("%d New User %d\n", epochTime, nId);
    // New user found
    //validate the number of users
    XnUInt16 usersTracked=0;
    for(XnUInt16 i=0; i<nUsers; i++)
    {
    	if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==TRUE)
    		usersTracked++;
    }
    if(usersTracked >= numOfUser)
    {
    	printf("%d users are currently being tracked, no more users allowed\n",numOfUser);
    	return;
    }
    if (g_bNeedPose)
    {
        g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
    }
    else
    {
        g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
    }
}
示例#6
0
文件: kica.cpp 项目: phelrine/kica
void gl_onKeyboard(unsigned char key, int x, int y)
{
  XnUInt16 nUser = 16;
  XnUserID users[16];
  
  switch(key){
  case 27:
    gContext.Shutdown();
    exit(1);
    break;
  case 'c':
    gUserGenerator.GetUsers(users, nUser);
    if(nUser == 0){
      fprintf(stderr, "cannot find user\n");
    }else if(capture->start(*std::min_element(users, &users[nUser]))){
      fprintf(stderr, "start recording\n");
    }
    break;
  case 's':
    if(capture->stop()){
      fprintf(stderr, "stop recording\n");
    }
    break;
  }
}
示例#7
0
// Load calibration from file
bool LoadCalibration( xn::UserGenerator& userGenerator )
{
    printf("LoadCalibration\n");

    XnUserID aUserIDs[20] = {0};
    XnUInt16 nUsers = 20;
    userGenerator.GetUsers(aUserIDs, nUsers);
    for (int i = 0; i < nUsers; ++i) {
        // ユーザーがキャリブレーションされているときは何もしない
        if (userGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i]))
            continue;

        if (userGenerator.GetSkeletonCap().IsCalibrating(aUserIDs[i]))
            continue;

        // ファイルから、キャリブレーション情報を取得し、骨格の追跡を開始する
        printf("Load user's calibration from file: user %d\n", aUserIDs[i]);
        XnStatus rc = userGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
        if (rc == XN_STATUS_OK) {
            printf("Make sure state is coherent: user %d\n", aUserIDs[i]);
            userGenerator.GetPoseDetectionCap().StopPoseDetection(aUserIDs[i]);
            userGenerator.GetSkeletonCap().StartTracking(aUserIDs[i]);
            return true;
        }

        break;
    }

    return false;
}
示例#8
0
void publishTransforms(const std::string& frame_id, ros::Publisher& publisher)
{
	XnUserID users[NUSERS];
	XnUInt16 users_count = NUSERS;
	g_UserGenerator.GetUsers(users, users_count);

	for (int i = 0; i < users_count; ++i)
	{
		XnUserID user = users[i];
		
		XnPoint3D point;
		g_UserGenerator.GetCoM(users[i], point);
		

		int idx = user - 1;
		if (isnan(point.X)==1||isnan(point.Y)==1||isnan(point.Z)==1||point.Z==0) // TODO 
			kinectUsers[idx].active == FALSE;
		if (kinectUsers[idx].active == TRUE)
			estimateVelocity(point, kinectUsers[idx]);
			//if (point.X-oldX>=20)
			//publisher.publish(kinectUsers[idx]);
			//oldX=point.X;
		//}
		//g_DepthGenerator.ConvertRealWorldToProjective(1, &point, &point);
		// original sceleton transform publishing.
		/*if (g_UserGenerator.GetSkeletonCap().IsTracking(user))
		{
			publishTransform(user, XN_SKEL_HEAD,           frame_id, "head");
			publishTransform(user, XN_SKEL_NECK,           frame_id, "neck");
			publishTransform(user, XN_SKEL_TORSO,          frame_id, "torso");

			publishTransform(user, XN_SKEL_LEFT_SHOULDER,  frame_id, "left_shoulder");
			publishTransform(user, XN_SKEL_LEFT_ELBOW,     frame_id, "left_elbow");
			publishTransform(user, XN_SKEL_LEFT_HAND,      frame_id, "left_hand");

			publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
			publishTransform(user, XN_SKEL_RIGHT_ELBOW,    frame_id, "right_elbow");
			publishTransform(user, XN_SKEL_RIGHT_HAND,     frame_id, "right_hand");

			publishTransform(user, XN_SKEL_LEFT_HIP,       frame_id, "left_hip");
			publishTransform(user, XN_SKEL_LEFT_KNEE,      frame_id, "left_knee");
			publishTransform(user, XN_SKEL_LEFT_FOOT,      frame_id, "left_foot");

			publishTransform(user, XN_SKEL_RIGHT_HIP,      frame_id, "right_hip");
			publishTransform(user, XN_SKEL_RIGHT_KNEE,     frame_id, "right_knee");
			publishTransform(user, XN_SKEL_RIGHT_FOOT,     frame_id, "right_foot");
		}*/
	}
	for (int i = 0; i < NUSERS; ++i)
	{
		if (kinectUsers[i].id != 0)
			publisher.publish(kinectUsers[i]);
			//if (kinectUsers[i].active == 0)
				//kinectUsers[i].id = 0;
	}
}
void publishTransforms(const std::string& frame_id) {
    XnUserID users[15];
    XnUInt16 users_count = 15;
    g_UserGenerator.GetUsers(users, users_count);

    for (int i = 0; i < users_count; ++i) {
        XnUserID user = users[i];

        publishTransform(user, frame_id, "user");
    }
}
示例#10
0
// this function will print the confidence values of all the joints
void jointConf()
{
	XnUserID nId[15];
	XnUInt16 oId = 15;
	g_UserGenerator.GetUsers(nId, oId);
	
	int userSum = 0;
	float userConfidence = 0;
	
	// ITERATE THROUGH ALL TRACKED USERS AND OUTPUT INDIVIDUAL JOINT CONFIDENCE VALUES
	for (int i = 0; i < oId; ++i){
		if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(nId[i])){
			XnSkeletonJointPosition jointData;
			printf("\n");
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_HEAD, jointData);
			std::cout << "User " << nId[i] << " - Head " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_NECK, jointData);
			std::cout << "User " << nId[i] << " - Neck " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_TORSO, jointData);
			std::cout << "User " << nId[i] << " - Torso " << jointData.fConfidence << std::endl;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_SHOULDER, jointData);
			std::cout << "User " << nId[i] << " - L Shoulder " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_ELBOW, jointData);
			std::cout << "User " << nId[i] << " - L Elbow " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HAND, jointData);
			std::cout << "User " << nId[i] << " - L Hand " << jointData.fConfidence << std::endl;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_SHOULDER, jointData);
			std::cout << "User " << nId[i] << " - R Shoulder " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_ELBOW, jointData);
			std::cout << "User " << nId[i] << " - R Elbow " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HAND, jointData);
			std::cout << "User " << nId[i] << " - R Hand " << jointData.fConfidence << std::endl;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HIP, jointData);
			std::cout << "User " << nId[i] << " - L Hip " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_KNEE, jointData);
			std::cout << "User " << nId[i] << " - L Knee " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_FOOT, jointData);
			std::cout << "User " << nId[i] << " - L Foot " << jointData.fConfidence << std::endl;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HIP, jointData);
			std::cout << "User " << nId[i] << " - R Hip " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_KNEE, jointData);
			std::cout << "User " << nId[i] << " - R Knee " << jointData.fConfidence << std::endl;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_FOOT, jointData);
			std::cout << "User " << nId[i] << " - R Foot " << jointData.fConfidence << std::endl;
			
		}
	}
}
示例#11
0
XnUserID FirstTrackingUser(xn::UserGenerator userGenerator) {
  XnUserID aUsers[10];
  XnUInt16 nUsers = 10;
  userGenerator.GetUsers(aUsers, nUsers);
  // The first user that's tracking will be in control
  for (int i = 0; i < nUsers; ++i) {
    if(userGenerator.GetSkeletonCap().IsTracking(aUsers[i])){
      return aUsers[i];
    }
  }
  return NULL;
}
示例#12
0
XnUserID hiKinect::getTrackedUserID(){
  XnUInt16 nUsers = g_UserGenerator.GetNumberOfUsers();
  XnUserID aUsers[nUsers];
  g_UserGenerator.GetUsers(aUsers, nUsers);
  xn::SkeletonCapability cap = g_UserGenerator.GetSkeletonCap();
  for (int i = 0; i < nUsers; ++i) {
    if (cap.IsTracking(aUsers[i])) {
      return aUsers[i];
    }
  }
  return 0;
}
示例#13
0
// this function will calculate the average joint confidence value for every user
void jointConfidence()
{
	XnUserID nId[15];
	XnUInt16 oId = 15;
	g_UserGenerator.GetUsers(nId, oId);
	
	int userSum = 0;
	float userConfidence = 0;
	
	for (int i = 0; i < oId; ++i){
		if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(nId[i])){
			XnSkeletonJointPosition jointData;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_HEAD, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_NECK, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_TORSO, jointData);
			userSum += jointData.fConfidence;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_SHOULDER, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_ELBOW, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HAND, jointData);
			userSum += jointData.fConfidence;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_SHOULDER, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_ELBOW, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HAND, jointData);
			userSum += jointData.fConfidence;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HIP, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_KNEE, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_FOOT, jointData);
			userSum += jointData.fConfidence;
			
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HIP, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_KNEE, jointData);
			userSum += jointData.fConfidence;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_FOOT, jointData);
			userSum += jointData.fConfidence;
			
			userConfidence = userSum / 15.0f;
			csvConfidence(nId[i], userConfidence);
		}
	}
}
示例#14
0
文件: scene.cpp 项目: sgerin/mocap
// Save calibration to file
void SaveCalibration(){
  XnUserID aUserIDs[20] = {0};
  XnUInt16 nUsers = 20;
  g_UserGenerator.GetUsers(aUserIDs, nUsers);
  for (int i = 0; i < nUsers; ++i){
      // Find a user who is already calibrated
    if (g_UserGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i])){
	  // Save user's calibration to file
	  g_UserGenerator.GetSkeletonCap().SaveCalibrationDataToFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
	  break;
	}
    }
}
示例#15
0
文件: scene.cpp 项目: sgerin/mocap
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
 	
  char strLabel[50] = "";
  XnUserID aUsers[15];
  XnUInt16 nUsers = 15;

  glBegin(GL_POINT);

  g_UserGenerator.GetUsers(aUsers, nUsers);
  for (int i = 0; i < nUsers; ++i)
    {
      if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])){
	
	DrawJoint(aUsers[i], XN_SKEL_HEAD);
	DrawJoint(aUsers[i], XN_SKEL_NECK);
	DrawJoint(aUsers[i], XN_SKEL_TORSO);
	DrawJoint(aUsers[i], XN_SKEL_WAIST);

	DrawJoint(aUsers[i], XN_SKEL_LEFT_COLLAR);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_ELBOW);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_WRIST);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_HAND);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_FINGERTIP);

	DrawJoint(aUsers[i], XN_SKEL_RIGHT_COLLAR);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_WRIST);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_HAND);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_FINGERTIP);

	DrawJoint(aUsers[i], XN_SKEL_LEFT_HIP);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_KNEE);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_ANKLE);
	DrawJoint(aUsers[i], XN_SKEL_LEFT_FOOT);

	DrawJoint(aUsers[i], XN_SKEL_RIGHT_HIP);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_KNEE);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_ANKLE);
	DrawJoint(aUsers[i], XN_SKEL_RIGHT_FOOT);



      }
    }

  glEnd();

}
void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
  ROS_INFO_STREAM("Lost user " << nId << ".");

  std_msgs::UInt16MultiArray tracked_users;
  XnUserID users[15];
  XnUInt16 users_count = 15;
  g_UserGenerator.GetUsers(users, users_count);
  for (unsigned int user = 0; user < users_count; ++user)
  {
    tracked_users.data.push_back(users[user]);
  }
  available_tracked_users_pub.publish(tracked_users);
}
示例#17
0
void XN_CALLBACK_TYPE
User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
  printf("New User: %d\n", nId);
  FBLOG_INFO("User_NewUser", nId);
  XnUInt16 nUsers = g_UserGenerator.GetNumberOfUsers();
  XnUserID aUsers[nUsers];
  g_UserGenerator.GetUsers(aUsers, nUsers);
  for (int i = 0; i < nUsers; ++i) {
    if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
      return;
    }
  }
  g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(POSE_TO_USE, nId);
  (*((hiKinect *)pCookie)->callbackPtr)->InvokeAsync("", FB::variant_list_of("NEW_USER"));
}
示例#18
0
文件: main.cpp 项目: arnthorm/Fable
void processUsers(uint32_t frameId)
{
  // Process users
  XnUserID aUsers[20];
  XnUInt16 nUsers = 20;
  g_UserGenerator.GetUsers(aUsers, nUsers);
  std::vector<XnUserID> users;
  for (int i = 0; i < nUsers; ++i)
  {
    if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
    {
      users.push_back(aUsers[i]);
    }
  }
  processAndPublish(users, frameId);
}
示例#19
0
// Save calibration to file
void SaveCalibration( xn::UserGenerator& userGenerator )
{
    printf("SaveCalibration\n");

    XnUserID aUserIDs[20] = {0};
    XnUInt16 nUsers = 20;
    userGenerator.GetUsers( aUserIDs, nUsers );
    for ( int i = 0; i < nUsers; ++i ) {
        // ユーザーがキャリブレーションされていたら、その情報を保存する
        if ( userGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i]) ) {
            printf("Save user's calibration to file: user %d\n", aUserIDs[i]);
            userGenerator.GetSkeletonCap().SaveCalibrationDataToFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
            break;
        }
    }
}
示例#20
0
文件: main.cpp 项目: imclab/Bingsu
void FindPlayer()
{
	if (g_nPlayer != 0)
	{
		return;
	}
	XnUserID aUsers[20];
	XnUInt16 nUsers = 20;
	g_UserGenerator.GetUsers(aUsers, nUsers);

	for (int i = 0; i < nUsers; ++i)
	{
		if (AssignPlayer(aUsers[i]))
			return;
	}
}
// track two users 
// take down users' joint positions
// calculate joint angles and etc.
void CheckPosture(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, FILE *fp)
{
	XnUserID aUsers[2];
	XnUInt16 nUsers = 2;
	g_UserGenerator.GetUsers(aUsers, nUsers);

	for(int i = 0; i < nUsers; ++i)
	{
		// if the user is being tracked
		// take down his/her joint positions
		if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{
			// check user posture
			RecordAngle(aUsers[i], fp);
		}
	}
}
示例#22
0
文件: scene.cpp 项目: sgerin/mocap
// Load calibration from file
void LoadCalibration(){
  XnUserID aUserIDs[20] = {0};
  XnUInt16 nUsers = 20;
  g_UserGenerator.GetUsers(aUserIDs, nUsers);
  for (int i = 0; i < nUsers; ++i){
      // Find a user who isn't calibrated or currently in pose
      if (g_UserGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i])) continue;
      if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUserIDs[i])) continue;

      // Load user's calibration from file
      XnStatus rc = g_UserGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
      if (rc == XN_STATUS_OK){
	  // Make sure state is coherent
	  g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(aUserIDs[i]);
	  g_UserGenerator.GetSkeletonCap().StartTracking(aUserIDs[i]);
	}
      break;
    }
}
void publishTransforms(const std::string& frame_id)
{
  ros::Time now = ros::Time::now();

  XnUInt16 users_count = MAX_USERS;
  XnUserID users[MAX_USERS];

  g_UserGenerator.GetUsers(users, users_count);

  //per ogni utente pubblica la trasformazione
  for(int i = 0; i < users_count; ++i)
  {
    XnUserID user = users[i];

    if(checkCenterOfMass(user))
    {
      publishUserTransforms(user, frame_id);
    }
  }
}
示例#24
0
文件: main.cpp 项目: imclab/Bingsu
void XN_CALLBACK_TYPE CalibrationEnded(xn::SkeletonCapability& skeleton, XnUserID user, XnBool bSuccess, void* cxt)
{
	printf("Calibration done [%d] %ssuccessfully\n", user, bSuccess?"":"un");
	if (bSuccess)
	{
		if (!g_bCalibrated)
		{
			g_UserGenerator.GetSkeletonCap().SaveCalibrationData(user, 0);
			g_nPlayer = user;
			g_UserGenerator.GetSkeletonCap().StartTracking(user);
			g_bCalibrated = TRUE;
		}

		XnUserID aUsers[10];
		XnUInt16 nUsers = 10;
		g_UserGenerator.GetUsers(aUsers, nUsers);
		for (int i = 0; i < nUsers; ++i)
			g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(aUsers[i]);
	}
}
void publishTransforms(const std::string& frame_id, image_transport::Publisher& pub)
{
	XnUserID users[15];
	XnUInt16 users_count = 15;
	g_UserGenerator.GetUsers(users, users_count);

	for (int i = 0; i < users_count; ++i)
	{
		XnUserID user = users[i];
		if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
			continue;

		publishTransform(user, XN_SKEL_HEAD, frame_id, "head");
		publishTransform(user, XN_SKEL_NECK, frame_id, "neck");
		publishTransform(user, XN_SKEL_TORSO, frame_id, "torso");

		publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder");
		publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow");
		publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand");

		publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
		publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow");
		publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand");

		publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip");
		publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee");
		publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot");

		publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip");
		publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee");
		publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot");
	}

	// publish segmented image
	xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	g_DepthGenerator.GetMetaData(depthMD);
	g_UserGenerator.GetUserPixels(0, sceneMD);
	PublishPeopleImage(depthMD, sceneMD, pub);
}
示例#26
0
reader_result kinect_reader2::get_next() {
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	XnSkeletonJointPosition joint;

	while(true) {
		// Read next available data
		nRetVal = g_Context.WaitOneUpdateAll(g_DepthGenerator);
		if(nRetVal != XN_STATUS_OK) {
			printf("Failed updating data: %s\n", xnGetStatusString(nRetVal));
			continue;
		}

		if(g_UserGenerator.GetNumberOfUsers() > 0) {

			g_UserGenerator.GetUsers(aUsers, nUsers);
			for(int i=0; i< nUsers; i++)
			{
				if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
				{
					reader_result new_tuple;

					// player
					new_tuple.push_back(boost::any((int)aUsers[i]));

					// 1 = XN_SKEL_HEAD, ..., 24 = XN_SKEL_RIGHT_FOOD
					for(short j = 1; j <= 24; j++) {
						g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], (XnSkeletonJoint)j, joint);
						new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.position.X)));
						new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.position.Y)));
						new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.position.Z)));
						new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.fConfidence)));
					}

					return new_tuple;
				}
			}
		}
	}
}
void getSkels(std::vector<mapping_msgs::PolygonalMap> &pmaps, body_msgs::Skeletons &skels){
   XnUserID aUsers[15];
   XnUInt16 nUsers = 15;
   g_UserGenerator.GetUsers(aUsers, nUsers);
   for (int i = 0; i < nUsers; ++i)
   {
      if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
            {
               body_msgs::Skeleton skel;
               getSkeleton(aUsers[i],skel);
               skels.skeletons.push_back(skel);



               mapping_msgs::PolygonalMap pmap;
               getPolygon(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK, pmap);
//               printPt(aUsers[i], XN_SKEL_RIGHT_HAND);
               getPolygon(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, pmap);
               getPolygon(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, pmap);
               getPolygon(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_RIGHT_SHOULDER, pmap);
//               ptdist(pmap.polygons.back());
               getPolygon(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, pmap);
               getPolygon(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, pmap);
               getPolygon(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, pmap);
               getPolygon(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, pmap);
               getPolygon(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO, pmap);
               getPolygon(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO, pmap);
               getPolygon(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, pmap);
               getPolygon(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, pmap);
               getPolygon(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, pmap);
               getPolygon(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, pmap);
               getPolygon(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, pmap);
               getPolygon(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, pmap);
               getPolygon(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP, pmap);
//               getSkel(aUsers[i],pmap);
               pmaps.push_back(pmap);
            }
   }

}
示例#28
0
int main(int argc, char **argv)
{
    XnStatus nRetVal = XN_STATUS_OK;
    xn::EnumerationErrors errors;
    TotalFrames = 0;
    static struct rusage ru;
    long double t;
    double w;
    struct timeval timStart;
    struct timeval timEnd;
    struct timeval Wall;
    bool Sample = true;
    XnFPSData xnFPS;
    XnUserID aUsers[MAX_NUM_USERS];
	XnUInt16 nUsers;
	XnSkeletonJointTransformation torsoJoint;

    if (argc > 1)
	{

    	//parse the cmd line
    	if (strcasecmp(argv[1],"--help") == 0)
    	{
    		PrintHelpHeader(argv[0]);
    		return 1;
    	}
    	numOfUser = atoi(argv[1]);
    	if(numOfUser == 0)
    	{
    		PrintHelpHeader(argv[0]);
    		return 1;
    	}
    	else if(numOfUser > 2)
    	{
    		printf("Maximal Users allowed is 2\n");
    		return 1;
    	}

	}
    else
    {
    	numOfUser = 4;
    }

    const char *fn = NULL;
    if    (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
    else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
    else {
        printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
        return XN_STATUS_ERROR;
    }
    printf("Reading config from: '%s'\n", fn);

    nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors);
    if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
    {
        XnChar strError[1024];
        errors.ToString(strError, 1024);
        printf("%s\n", strError);
        return (nRetVal);
    }
    else if (nRetVal != XN_STATUS_OK)
    {
        printf("Open failed: %s\n", xnGetStatusString(nRetVal));
        return (nRetVal);
    }

    nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
    CHECK_RC(nRetVal,"No depth");

#if (XN_PLATFORM != XN_PLATFORM_MACOSX)	
    //we want out benchmark application will be running only on one CPU core
    cpu_set_t mask;
	CPU_ZERO(&mask);
	CPU_SET(1,&mask);
	sched_setaffinity(0,sizeof(mask),&mask);
#endif	
	//initialize the FPS calculator
	nRetVal = xnFPSInit(&xnFPS, 90);
	CHECK_RC(nRetVal, "FPS Init");
	//ensure the User generator exists
    nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
    if (nRetVal != XN_STATUS_OK)
    {
        nRetVal = g_UserGenerator.Create(g_Context);
        CHECK_RC(nRetVal, "Find user generator");
    }
    //register to generators callbacks
    XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
    if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
    {
        printf("Supplied user generator doesn't support skeleton\n");
        return 1;
    }
    nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
    CHECK_RC(nRetVal, "Register to user callbacks");
    nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
    CHECK_RC(nRetVal, "Register to calibration start");
    nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
    CHECK_RC(nRetVal, "Register to calibration complete");
    //ensure calibration
    if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
    {
        g_bNeedPose = TRUE;
        if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
        {
            printf("Pose required, but not supported\n");
            return 1;
        }
        nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
        CHECK_RC(nRetVal, "Register to Pose Detected");
        g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
    }
    //set skeleton profile (all joints)
    g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
    //start to generate
    nRetVal = g_Context.StartGeneratingAll();
    CHECK_RC(nRetVal, "StartGenerating");



    printf("%c[%d;%d;%dmPrimeSense Skeleton Benchmark Application\n ", 0x1B, BRIGHT,BLUE,BG_BLACK);
    printf("%c[%d;%d;%dmSet Maximal users to %d ", 0x1B, BRIGHT,BLUE,BG_BLACK,numOfUser);
	printf("%c[%dm\n", 0x1B, 0);
    printf("Starting to run\n");
    if(g_bNeedPose)
    {
        printf("Assume calibration pose\n");
    }

    XnUInt32 epochTime = 0;
    //each 30 frames (1 second) we start the CPU resources usages
    while (!xnOSWasKeyboardHit())
    {
    	if (Sample)
    	{	//get the beginning sample of CPU resources
    		getrusage(RUSAGE_SELF, &ru);
    		timStart=ru.ru_utime;
    		t=(double)timStart.tv_sec * 1000000.0 + (double)timStart.tv_usec \
    		   + (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec;
    		//get the wall clock time
    		gettimeofday(&Wall,NULL);

    		w=(double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec;
    		Sample = false;
    	}
    	g_Context.WaitOneUpdateAll(g_UserGenerator);
    	xnFPSMarkFrame(&xnFPS);
        // print the torso information for the first user already tracking every 1 second to prevent CPU of printf
        if(TotalFrames % 30 == 0)
        {
			nUsers=MAX_NUM_USERS;
			g_UserGenerator.GetUsers(aUsers, nUsers);
			int numTracked=0;
			int userToPrint=-1;
			for(XnUInt16 i=0; i<nUsers; i++)
			{
				if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
					continue;

				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint);
				printf("User %d Located At distance of %6.2f mm from the sensor\n",aUsers[i],torsoJoint.position.position.Z);

			 }
			//get the finish sample of the CPU resources
			getrusage(RUSAGE_SELF, &ru);
			timEnd=ru.ru_utime;
			t = (double)timEnd.tv_sec * 1000000.0 + (double)timEnd.tv_usec \
				+	(double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec	- t;
			//get the wall clock
			gettimeofday(&Wall,NULL);

			w = (double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec - w;

			XnDouble fps=xnFPSCalc(&xnFPS);
			//print stuff.
			printf("%c[%d;%d;%dmCPU Utilization=%3.2f%%\t", 0x1B, BRIGHT,RED,BG_BLACK,(double)100*t/w);
			printf("%c[%d;%d;%dmFPS=%3.2f ", 0x1B, BRIGHT,RED,BG_BLACK,(double)fps);
			printf("%c[%dm\n", 0x1B, 0);
			Sample= true;

		}
        TotalFrames++;
        
    }
    g_scriptNode.Release();
    g_DepthGenerator.Release();
    g_UserGenerator.Release();
    g_Context.Release();

}
示例#29
0
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
	static bool bInitialized = false;
	static GLuint depthTexID;
	static unsigned char* pDepthTexBuf;
	static int texWidth, texHeight;

	float topLeftX;
	float topLeftY;
	float bottomRightY;
	float bottomRightX;
	float texXpos;
	float texYpos;

	if(!bInitialized)
	{
		texWidth =  getClosestPowerOfTwo(dmd.XRes());
		texHeight = getClosestPowerOfTwo(dmd.YRes());

//		printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
		depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;

//		printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
		bInitialized = true;

		topLeftX = dmd.XRes();
		topLeftY = 0;
		bottomRightY = dmd.YRes();
		bottomRightX = 0;
		texXpos =(float)dmd.XRes()/texWidth;
		texYpos  =(float)dmd.YRes()/texHeight;

		memset(texcoords, 0, 8*sizeof(float));
		texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
	}

	unsigned int nValue = 0;
	unsigned int nHistValue = 0;
	unsigned int nIndex = 0;
	unsigned int nX = 0;
	unsigned int nY = 0;
	unsigned int nNumberOfPoints = 0;
	XnUInt16 g_nXRes = dmd.XRes();
	XnUInt16 g_nYRes = dmd.YRes();

	unsigned char* pDestImage = pDepthTexBuf;

	const XnDepthPixel* pDepth = dmd.Data();
	const XnLabel* pLabels = smd.Data();

	// Calculate the accumulative histogram
	memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
	for (nY=0; nY<g_nYRes; nY++)
	{
		for (nX=0; nX<g_nXRes; nX++)
		{
			nValue = *pDepth;

			if (nValue != 0)
			{
				g_pDepthHist[nValue]++;
				nNumberOfPoints++;
			}

			pDepth++;
		}
	}

	for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
	{
		g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
	}
	if (nNumberOfPoints)
	{
		for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
		{
			g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
		}
	}

	pDepth = dmd.Data();
	if (g_bDrawPixels)
	{
		XnUInt32 nIndex = 0;
		// Prepare the texture map
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++, nIndex++)
			{

				pDestImage[0] = 0;
				pDestImage[1] = 0;
				pDestImage[2] = 0;
				if (g_bDrawBackground || *pLabels != 0)
				{
					nValue = *pDepth;
					XnLabel label = *pLabels;
					XnUInt32 nColorID = label % nColors;
					if (label == 0)
					{
						nColorID = nColors;
					}

					if (nValue != 0)
					{
						nHistValue = g_pDepthHist[nValue];

						pDestImage[0] = nHistValue * Colors[nColorID][0];
						pDestImage[1] = nHistValue * Colors[nColorID][1];
						pDestImage[2] = nHistValue * Colors[nColorID][2];
					}
				}

				pDepth++;
				pLabels++;
				pDestImage+=3;
			}

			pDestImage += (texWidth - g_nXRes) *3;
		}
	}
	else
	{
		xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
	}

	glBindTexture(GL_TEXTURE_2D, depthTexID);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);

	// Display the OpenGL texture map
	glColor4f(0.75,0.75,0.75,1);

	glEnable(GL_TEXTURE_2D);
	DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
	glDisable(GL_TEXTURE_2D);

	char strLabel[50] = "";
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{

		if (g_bPrintID)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(aUsers[i], com);
			g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

			xnOSMemSet(strLabel, 0, sizeof(strLabel));
			if (!g_bPrintState)
			{
				// Tracking
				sprintf(strLabel, "%d", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
			{
				// Tracking
				sprintf(strLabel, "%d - Tracking", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
			{
				// Calibrating
				sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
			}
			else
			{
				// Nothing
				sprintf(strLabel, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
			}


			glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);

			glRasterPos2i(com.X, com.Y);
			glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
		}

		//Draw skeleton
		if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{

			glBegin(GL_LINES);

			glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
			DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

			DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);

			DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);

			DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);

			DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);

			glEnd();

		}
	}
}
示例#30
0
int main(int argc, char **argv)
{
    XnStatus nRetVal = XN_STATUS_OK;
    xn::EnumerationErrors errors;
    
    if( USE_RECORED_DATA ){
        g_Context.Init();
        g_Context.OpenFileRecording(RECORD_FILE_PATH);
        xn::Player player;
        
        // Player nodeの取得
        nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_PLAYER, player);
        CHECK_RC(nRetVal, "Find player");
        
        LOG_D("PlaybackSpeed: %d", player.GetPlaybackSpeed());
        
        xn:NodeInfoList nodeList;
        player.EnumerateNodes(nodeList);
        for( xn::NodeInfoList::Iterator it = nodeList.Begin();
            it != nodeList.End(); ++it){
            
            if( (*it).GetDescription().Type == XN_NODE_TYPE_IMAGE ){
                nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
                CHECK_RC(nRetVal, "Find image node");
                LOG_D("%s", "ImageGenerator created.");
            }
            else if( (*it).GetDescription().Type == XN_NODE_TYPE_DEPTH ){
                nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
                CHECK_RC(nRetVal, "Find depth node");
                LOG_D("%s", "DepthGenerator created.");            
            }
            else{
                LOG_D("%s %s %s", ::xnProductionNodeTypeToString((*it).GetDescription().Type ),
                      (*it).GetInstanceName(),
                      (*it).GetDescription().strName);
            }
        }
    }
    else{
        LOG_I("Reading config from: '%s'", CONFIG_XML_PATH);
        
        nRetVal = g_Context.InitFromXmlFile(CONFIG_XML_PATH, g_scriptNode, &errors);
        if (nRetVal == XN_STATUS_NO_NODE_PRESENT){
            XnChar strError[1024];
            errors.ToString(strError, 1024);
            LOG_E("%s\n", strError);
            return (nRetVal);
        }
        else if (nRetVal != XN_STATUS_OK){
            LOG_E("Open failed: %s", xnGetStatusString(nRetVal));
            return (nRetVal);
        }
        
        nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
        CHECK_RC(nRetVal,"No depth");
        
        // ImageGeneratorの作成
        nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
        CHECK_RC(nRetVal, "Find image generator");
        
    }
    // UserGeneratorの取得
    nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
    if(nRetVal!=XN_STATUS_OK){
        nRetVal = g_UserGenerator.Create(g_Context); 
        CHECK_RC(nRetVal, "Create user generator");
    }

    
    XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
    if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)){
        LOG_E("%s", "Supplied user generator doesn't support skeleton");
        return 1;
    }
    nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
    CHECK_RC(nRetVal, "Register to user callbacks");

    g_SkeletonCap = g_UserGenerator.GetSkeletonCap();
    nRetVal = g_SkeletonCap.RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
    CHECK_RC(nRetVal, "Register to calibration start");

    nRetVal = g_SkeletonCap.RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
    CHECK_RC(nRetVal, "Register to calibration complete");
    
    if (g_SkeletonCap.NeedPoseForCalibration()){
        g_bNeedPose = TRUE;
        if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)){
            LOG_E("%s", "Pose required, but not supported");
            return 1;
        }
        nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
        CHECK_RC(nRetVal, "Register to Pose Detected");
        g_SkeletonCap.GetCalibrationPose(g_strPose);
    }
    
    g_SkeletonCap.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
    
    nRetVal = g_Context.StartGeneratingAll();
    CHECK_RC(nRetVal, "StartGenerating");
    
    // 表示用の画像データの作成
    XnMapOutputMode mapMode;
    g_ImageGenerator.GetMapOutputMode(mapMode);
    g_rgbImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);

    LOG_I("%s", "Starting to run");
    if(g_bNeedPose){
        LOG_I("%s", "Assume calibration pose");
    }

    xn::Recorder recorder;
    if( DO_RECORED && !USE_RECORED_DATA ){
        // レコーダーの作成
        LOG_I("%s", "Setup Recorder");
        nRetVal = recorder.Create(g_Context);
        CHECK_RC(nRetVal, "Create recorder");
        
        // 保存設定
        nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, RECORD_FILE_PATH);
        CHECK_RC(nRetVal, "Set recorder destination file");
        
        // 深度、ビデオカメラ入力を保存対象として記録開始
        nRetVal = recorder.AddNodeToRecording(g_DepthGenerator, XN_CODEC_NULL);
        CHECK_RC(nRetVal, "Add depth node to recording");
        nRetVal = recorder.AddNodeToRecording(g_ImageGenerator, XN_CODEC_NULL);
        CHECK_RC(nRetVal, "Add image node to recording");
        
        LOG_I("%s", "Recorder setup done.");
    }

    while (!xnOSWasKeyboardHit())
    {
        g_Context.WaitOneUpdateAll(g_UserGenerator);
        if( DO_RECORED  && !USE_RECORED_DATA ){
            nRetVal = recorder.Record();
            CHECK_RC(nRetVal, "Record");
        }

        // ビデオカメラ画像の生データを取得
        xn::ImageMetaData imageMetaData;
        g_ImageGenerator.GetMetaData(imageMetaData);
        // メモリコピー
        xnOSMemCopy(g_rgbImage->imageData, imageMetaData.RGB24Data(), g_rgbImage->imageSize);
        // BGRからRGBに変換して表示
        cvCvtColor(g_rgbImage, g_rgbImage, CV_RGB2BGR);

        // UserGeneratorからユーザー識別ピクセルを取得
        xn::SceneMetaData sceneMetaData;
        g_UserGenerator.GetUserPixels(0, sceneMetaData);
        
        XnUserID allUsers[MAX_NUM_USERS];
        XnUInt16 nUsers = MAX_NUM_USERS;
        g_UserGenerator.GetUsers(allUsers, nUsers);
        for (int i = 0; i < nUsers; i++) {
            
            // キャリブレーションに成功しているかどうか
            if (g_SkeletonCap.IsTracking(allUsers[i])) {
                // スケルトンを描画
                DrawSkelton(allUsers[i], i);
            }
        }
        
        // 表示
        cvShowImage("User View", g_rgbImage);

        // ESCもしくはqが押されたら終了させる
        if (cvWaitKey(10) == 27) {
            break;
        }
    }

    if( !USE_RECORED_DATA ){
        g_scriptNode.Release();
    }
    g_DepthGenerator.Release();
    g_UserGenerator.Release();
    g_Context.Release();

	if (g_rgbImage != NULL) {
		cvReleaseImage(&g_rgbImage);	
	}
	g_Context.Shutdown();

    
}