C++ UserGenerator::GetSkeletonCap方法代码示例

int main(int argc, char **argv) {
    ros::init(argc, argv, "openni_tracker");
    ros::NodeHandle nh;

    string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
    XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
    CHECK_RC(nRetVal, "InitFromXml");

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

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

	if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
		printf("Supplied user generator doesn't support skeleton\n");
		return 1;
	}

    XnCallbackHandle hUserCallbacks;
	g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);

	XnCallbackHandle hCalibrationCallbacks;
	g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

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

		XnCallbackHandle hPoseCallbacks;
		g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);

		g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
	}

	g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

	nRetVal = g_Context.StartGeneratingAll();
	CHECK_RC(nRetVal, "StartGenerating");

	ros::Rate r(30);

        
	ros::NodeHandle pnh("~");
	string frame_id("openni_depth_frame");
	pnh.getParam("camera_frame_id", frame_id);
	
	image_transport::ImageTransport it(nh);
	image_transport::Publisher pub = it.advertise("people_segmentation_image", 1);
                
	while (ros::ok()) {
		g_Context.WaitAndUpdateAll();
		publishTransforms(frame_id, pub);
		r.sleep();
	}

	g_Context.Shutdown();
	return 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;
			
		}
	}
}

void glut_display() {
	xn::DepthMetaData pDepthMapMD;
	xn::ImageMetaData pImageMapMD;
	XnUserID pUser[2];
	XnUInt16 nUsers=2;
#ifdef DEBUGOUT
	ofstream datei;
#endif

	glEnable(GL_TEXTURE_2D);

	pUser[0] = 0;
	pUser[1] = 0;

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	/*glFrustum(	-ROOM_X/2+(1500.0/1750.0)*static_cast<int>(headtrans.x),
				ROOM_X/2+(1500.0/1750.0)*static_cast<int>(headtrans.x),
				-ROOM_Y/2-(1500.0/1750.0)*static_cast<int>(headtrans.y),
				ROOM_Y/2-(1500.0/1750.0)*static_cast<int>(headtrans.y),
				1525,
				2525);*/

	float nearplane = 1525;
	float screenaspect = ROOM_X/ROOM_Y;
	glFrustum(	nearplane*(-0.5 * screenaspect + headtrans.x)/headtrans.z,
				nearplane*( 0.5 * screenaspect + headtrans.x)/headtrans.z,
				nearplane*(-0.5 + headtrans.y)/headtrans.z,
				nearplane*( 0.5 + headtrans.y)/headtrans.z,
				nearplane,
				2525);


	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	nRetVal = context.WaitAndUpdateAll();
	checkError("Fehler beim Aktualisieren der Daten", nRetVal);

	/* Anzahl der User auslesen und in Objekten speichern */
	user.GetUsers(pUser, nUsers);
	if(pUser[0]!=0 && pUserOld!=1) {
		cout << "User 1 erkannt" << endl;
		pUserOld=1;
	}

	xn::SkeletonCapability pSkeleton = user.GetSkeletonCap();
	XnCallbackHandle hnd;
	pSkeleton.RegisterCalibrationCallbacks(skel_cal_start, skel_cal_end, 0,hnd);
	
	if(calibration){

	}

	pSkeleton.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
	if(pSkeleton.IsCalibrated(pUser[0])) {
		XnSkeletonJointTransformation head;
		pSkeleton.StartTracking(pUser[0]);
		pSkeleton.GetSkeletonJoint(pUser[0], XN_SKEL_HEAD, head);

		if(head.position.fConfidence && head.orientation.fConfidence) {
/*			headtrans.rotmat[0] = head.orientation.orientation.elements[0];
			headtrans.rotmat[1] = head.orientation.orientation.elements[1];
			headtrans.rotmat[2] = head.orientation.orientation.elements[2];
			headtrans.rotmat[3] = 0;
			
			headtrans.rotmat[4] = head.orientation.orientation.elements[3];
			headtrans.rotmat[5] = head.orientation.orientation.elements[4];
			headtrans.rotmat[6] = head.orientation.orientation.elements[5];
			headtrans.rotmat[7] = 0;
			
			headtrans.rotmat[8] = -head.orientation.orientation.elements[6];
			headtrans.rotmat[9] = -head.orientation.orientation.elements[7];
			headtrans.rotmat[10] =-head.orientation.orientation.elements[8];
			headtrans.rotmat[11] = 0;

			headtrans.rotmat[12] = 0;
			headtrans.rotmat[13] = 0;
			headtrans.rotmat[14] = 0;
			headtrans.rotmat[15] = 1;*/

			headtrans.x = head.position.position.X;
			headtrans.y = head.position.position.Y;
			headtrans.z = head.position.position.Z;
			
			 
#ifdef CONSOLEOUT
			clearScreen();
			cout	<< "Confidence Position: " << head.position.fConfidence
					<< " X: " << head.position.position.X
					<< " Y: " << head.position.position.Y
					<< " Z: " << head.position.position.Z << endl

					<< "Confidence Rotation: " << head.orientation.fConfidence << endl
					<< "\t" << headtrans.rotmat[0]
					<< "\t" << headtrans.rotmat[4]
					<< "\t" << headtrans.rotmat[8] << endl
					<< "\t" << headtrans.rotmat[1]
//.........这里部分代码省略.........

void writeSkeleton() {
	string s;

	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;

	s += "{\"skeletons\":[";

	int skeletons = 0;
	int activeUsers = 0;
  int totalJoints;
  string userSkeleton;
  string activeSkeletons[nUsers];

  userGenerator.GetUsers(aUsers, nUsers);
  for (int i = 0; i < nUsers; ++i) {
    totalJoints = 0;
    userSkeleton = "";

    char tmp[1024];
    sprintf(tmp, "{\"userid\":%d,\"joints\":[", aUsers[i]);
    userSkeleton += tmp;

    if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
			if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) {
        writeJoint(&userSkeleton, "head", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) {
				writeJoint(&userSkeleton, "neck", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) {
				writeJoint(&userSkeleton, "l_collar", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) {
				writeJoint(&userSkeleton, "l_shoulder", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) {
				writeJoint(&userSkeleton, "l_elbow", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) {
				writeJoint(&userSkeleton, "l_wrist", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) {
				writeJoint(&userSkeleton, "l_hand", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) {
				writeJoint(&userSkeleton, "l_fingertop", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) {
				writeJoint(&userSkeleton, "r_collar", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) {
				writeJoint(&userSkeleton, "r_shoulder", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) {
				writeJoint(&userSkeleton, "r_elbow", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) {
				writeJoint(&userSkeleton, "r_wrist", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) {
				writeJoint(&userSkeleton, "r_hand", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) {
				writeJoint(&userSkeleton, "r_fingertip", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) {
				writeJoint(&userSkeleton, "torso", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) {
				writeJoint(&userSkeleton, "waist", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) {
				writeJoint(&userSkeleton, "l_hip", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) {
				writeJoint(&userSkeleton, "l_knee", jointCoords);
        totalJoints++;
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) {
				writeJoint(&userSkeleton, "l_ankle", jointCoords);
        totalJoints++;
			}
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
	XnStatus rc = XN_STATUS_OK;

	rc = g_Context.InitFromXmlFile(SAMPLE_XML_PATH);
	CHECK_RC(rc, "InitFromXml");

	rc = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
	CHECK_RC(rc, "Find depth generator");
	rc = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
	CHECK_RC(rc, "Find Image generator");
	rc = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
	CHECK_RC(rc, "Find user generator");

	// Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi
	g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator);
	//g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint();

	if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON) ||
		!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
	{
		printf("User generator doesn't support either skeleton or pose detection.\n");
		return XN_STATUS_ERROR;
	}

	g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

	rc = g_Context.StartGeneratingAll();
	CHECK_RC(rc, "StartGenerating");

	XnCallbackHandle hUserCBs, hCalibrationCBs, hPoseCBs;
	g_UserGenerator.RegisterUserCallbacks(NewUser, LostUser, NULL, hUserCBs);
	g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(CalibrationStarted, CalibrationEnded, NULL, hCalibrationCBs);
	g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(PoseDetected, NULL, NULL, hPoseCBs);


	#ifdef USE_GLUT
	
	// Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi
	g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator);
	//g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint();

	glInit(&argc, argv);
	glutMainLoop();

	#else

	if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context))
	{
		printf("Error initing opengles\n");
		CleanupExit();
	}

	glDisable(GL_DEPTH_TEST);
//	glEnable(GL_TEXTURE_2D);
	glEnableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);

	while ((!_kbhit()) && (!g_bQuit))
	{
		glutDisplay();
		eglSwapBuffers(display, surface);
	}

	opengles_shutdown(display, surface, context);

	CleanupExit();

	#endif
}

// Callback: New user was detected
void XN_CALLBACK_TYPE new_user(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
	printf("{\"found_user\":{\"userid\":%d}, \"elapsed\":%.3f}\n", nId, clockAsFloat(last));
	userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}

void XN_CALLBACK_TYPE user_reenter(xn::UserGenerator& generator, XnUserID nId, void *pCookie) {
	printf("{\"user_reenter\": {\"userid\": %d}}\n", nId);
	userGenerator.GetSkeletonCap().StartTracking(nId);
}

	void matrixCalc(void *outputs)
	{
		TML::Matrix out1(outputs, 0);
		TML::Matrix out2(outputs, 1);
		TML::Matrix out3(outputs, 2);
		TML::Matrix out4(outputs, 3);
		
		xn::DepthMetaData depthMD;
		xn::SceneMetaData sceneMD;
		xn::ImageMetaData imageMD;
		
		depth.GetMetaData(depthMD);
		user.GetUserPixels(0, sceneMD);
		image.GetMetaData(imageMD);
		
		context.WaitNoneUpdateAll();
		
		t_jit_matrix_info tmi;
		memset(&tmi, 0, sizeof(tmi));
		tmi.dimcount = 2;
		tmi.planecount = 1;
		tmi.dimstride[0] = 4;
		tmi.dimstride[1] = depthMD.XRes()*4;
		int width = tmi.dim[0] = depthMD.XRes();
		int height = tmi.dim[1] = depthMD.YRes();
		tmi.type = _jit_sym_float32;
		
		out1.resizeTo(&tmi);
		
		tmi.planecount = 1;
		tmi.dimstride[0] = 1;
		tmi.dimstride[1] = depthMD.XRes();
		tmi.type = _jit_sym_char;
		out2.resizeTo(&tmi);
		
		tmi.planecount = 4;
		tmi.dimstride[0] = 4;
		tmi.dimstride[1] = depthMD.XRes()*4;
		tmi.type = _jit_sym_char;
		out3.resizeTo(&tmi);
		
		const XnDepthPixel* pDepth = depthMD.Data();
		float *depthData = (float*)out1.data();
		
		//Copy depth data
		int x,y;
		for (y=0; y<height; y++)
		{
			for (x=0; x<width; x++)
			{
				depthData[0] = (float)pDepth[0]/powf(2, 15);
				
				depthData++;
				pDepth++;
			}
		}
		
		//Get the users
		unsigned char *userData = (unsigned char*)out2.data();
		const XnLabel* pLabels = sceneMD.Data();
		for (y=0; y<height; y++)
		{
			for (x=0; x<width; x++)
			{
				userData[0] = pLabels[0];
				
				userData++;
				pLabels++;
			}
		}
		
		//Get the colors
		const XnRGB24Pixel* pPixels = imageMD.RGB24Data();
		unsigned char *pixData = (unsigned char*)out3.data();
		for (y=0; y<height; y++)
		{
			for (x=0; x<width; x++)
			{
				pixData[0] = 0;
				pixData[1] = pPixels[0].nRed;
				pixData[2] = pPixels[0].nGreen;
				pixData[3] = pPixels[0].nBlue;
				
				pixData+=4;
				pPixels++;
			}
		}
		
		//For all the users -- output the joint info...
		XnUserID aUsers[15];
		XnUInt16 nUsers = 15;
		user.GetUsers(aUsers, nUsers);
		
		int rUsers = 0;
		
		xn::SkeletonCapability sc = user.GetSkeletonCap();
		
		int i;
		for (i=0; i<nUsers; i++)
		{
//.........这里部分代码省略.........

void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
	post("New User %d\n", nId);
	generator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}

void NiAnimation::updateSkeletonAnimation(xn::UserGenerator &user_generator)
{
    XnUInt16 num_users = MAX_USERS;
    user_generator.GetUsers(all_users, num_users);

    // tracking only default (first) person
    if (!user_generator.GetSkeletonCap().IsTracking(DEFAULT_USER)) return;

    // POSITION

    // animik: x,y,z € <-196, 196>

    XnSkeletonJointPosition torso;
    user_generator.GetSkeletonCap().GetSkeletonJointPosition(all_users[0], XN_SKEL_TORSO, torso);
    if (torso.fConfidence == 1) setPosition(torso.position.X*MM0, torso.position.Y*MM0+40, 150-torso.position.Z*MM0);


    // ROTATION

    double x, y, z, tx, ty, tz;

    // hip -> abdomen -> chest -> ...
    if (calculateRotation(user_generator, XN_SKEL_LEFT_HIP, tx, ty, tz, MM1) &&
        calculateRotation(user_generator, XN_SKEL_RIGHT_HIP, x,  y,  z, MM1))
        setRotation(getNodeByName("hip"), (x+tx)/2, (y+ty)/2, (z+tz)/2);

    if (calculateRotation(user_generator, XN_SKEL_TORSO, x, y, z, MM1))
        setRotation(getNodeByName("abdomen"), x, y, z);

    if (calculateRotation(user_generator, XN_SKEL_NECK, tx, ty, tz, MM1)) // !!! + torso ^
        setRotation(getNodeByName("chest"), (x+tx)/2, (y+ty)/2, (z+tz)/2);

    // ... -> neck -> head
    if (calculateRotation(user_generator, XN_SKEL_NECK, x, y, z, MM1))
        setRotation(getNodeByName("neck"), x, y, z);

    if (calculateRotation(user_generator, XN_SKEL_HEAD, x, y, z, MM1))
        setRotation(getNodeByName("head"), x, y, z);

    if (!global_mirror)
    {
        // ... -> lCollar -> lShldr -> lForeArm -> lHand
        if (calculateRotation(user_generator, XN_SKEL_LEFT_COLLAR, x, y, z, MM2))
            setRotation(getNodeByName("rCollar"), x, y, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_SHOULDER, x, y, z, MM2))
            setRotation(getNodeByName("rShldr"), x, y*MM2, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_ELBOW, x, y, z, MM2))
            setRotation(getNodeByName("rForeArm"), x, y, z);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_HAND, x, y, z, MM2))
            setRotation(getNodeByName("rHand"), x, y, z/MM2);

        // ... -> rCollar -> rShldr -> rForeArm -> rHand
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_COLLAR, x, y, z, MM2))
            setRotation(getNodeByName("lCollar"), x, y, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_SHOULDER, x, y, z, MM2))
            setRotation(getNodeByName("lShldr"), x, y*MM2, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_ELBOW, x, y, z, MM2))
            setRotation(getNodeByName("lForeArm"), x, y, z);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_HAND, x, y, z, MM2))
            setRotation(getNodeByName("lHand"), x, y, z/MM2);

        // hip -> lThigh -> lShin -> lFoot
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_HIP, x, y, z, MM3) )
            setRotation(getNodeByName("lThigh"), x/MM3, y, z);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_KNEE, x, y, z, MM3))
            setRotation(getNodeByName("lShin"), x, y, z);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_FOOT, x, y, z, MM3))
            setRotation(getNodeByName("lFoot"), x/MM3, y, z);

        // hip -> rThigh -> rShin -> rFoot
        if (calculateRotation(user_generator, XN_SKEL_LEFT_HIP, x, y, z, MM3))
            setRotation(getNodeByName("rThigh"), x/MM3, y, z);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_KNEE, x, y, z, MM3))
            setRotation(getNodeByName("rShin"), x, y, z);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_FOOT, x, y, z, MM3))
            setRotation(getNodeByName("rFoot"), x/MM3, y, z);
    }
    else
    {
        // ... -> lCollar -> lShldr -> lForeArm -> lHand
        if (calculateRotation(user_generator, XN_SKEL_LEFT_COLLAR, x, y, z, MM2))
            setRotation(getNodeByName("lCollar"), x, y, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_SHOULDER, x, y, z, MM2))
            setRotation(getNodeByName("lShldr"), x, y*MM2, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_ELBOW, x, y, z, MM2))
            setRotation(getNodeByName("lForeArm"), x, y, z);
        if (calculateRotation(user_generator, XN_SKEL_LEFT_HAND, x, y, z, MM2))
            setRotation(getNodeByName("lHand"), x, y, z/MM2);

        // ... -> rCollar -> rShldr -> rForeArm -> rHand
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_COLLAR, x, y, z, MM2))
            setRotation(getNodeByName("rCollar"), x, y, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_SHOULDER, x, y, z, MM2))
            setRotation(getNodeByName("rShldr"), x, y*MM2, z/MM2);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_ELBOW, x, y, z, MM2))
            setRotation(getNodeByName("rForeArm"), x, y, z);
        if (calculateRotation(user_generator, XN_SKEL_RIGHT_HAND, x, y, z, MM2))
            setRotation(getNodeByName("rHand"), x, y, z/MM2);

        // hip -> lThigh -> lShin -> lFoot
//.........这里部分代码省略.........

void start_kinect() {
    XnStatus nRetVal = XN_STATUS_OK;
    xn::EnumerationErrors errors;
    UsersCount = 0;

    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_depth);      
    CHECK_RC(nRetVal,"No depth");
    
	  nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);      
    CHECK_RC(nRetVal,"No image");

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

    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");

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

    g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

    nRetVal = g_Context.StartGeneratingAll();
    CHECK_RC(nRetVal, "StartGenerating");

    XnUserID aUsers[MAX_NUM_USERS];
    XnUInt16 nUsers;

    XnSkeletonJointTransformation anyjoint;

    printf("Starting to run\n");
    if(g_bNeedPose)
    {
        printf("Assume calibration pose\n");
    }
    XnUInt32 epochTime = 0;
    while (!xnOSWasKeyboardHit())
    {
        g_Context.WaitOneUpdateAll(g_UserGenerator);
        // print the torso information for the first user already tracking
        nUsers=MAX_NUM_USERS;
        g_UserGenerator.GetUsers(aUsers, nUsers);
        int numTracked=0;
        int userToPrint=-1;

        WriteLock w_lock(myLock);
    	  pDepthMap = g_depth.GetDepthMap();
        pPixelMap = g_image.GetRGB24ImageMap();
            
    	  g_depth.GetMetaData(g_depthMD);    
        g_image.GetMetaData(g_imageMD);
        pPixelPoint = g_imageMD.RGB24Data();

//.........这里部分代码省略.........

void sendOSC() {

	if (handMode) {
		sendHandOSC();
		return;
	}
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	userGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i) {
		if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
			lo_bundle bundle = lo_bundle_new(LO_TT_IMMEDIATE);

			if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) {
				oscFunc(&bundle, "head", 0);
			}
			if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) {
				oscFunc(&bundle, "neck", 1);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) {
				oscFunc(&bundle, "l_collar", 2);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) {
				oscFunc(&bundle, "l_shoulder", 3);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) {
				oscFunc(&bundle, "l_elbow", 4);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) {
				oscFunc(&bundle, "l_wrist", 5);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) {
				oscFunc(&bundle, "l_hand", 6);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) {
				oscFunc(&bundle, "l_fingertip", 7);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) {
				oscFunc(&bundle, "r_collar", 8);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) {
				oscFunc(&bundle, "r_shoulder", 9);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) {
				oscFunc(&bundle, "r_elbow", 10);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) {
				oscFunc(&bundle, "r_wrist", 11);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) {
				oscFunc(&bundle, "r_hand", 12);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) {
				oscFunc(&bundle, "r_fingertip", 13);
			}
			if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) {
				oscFunc(&bundle, "torso", 14);
			}
			if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) {
				oscFunc(&bundle, "waist", 15);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) {
				oscFunc(&bundle, "l_hip", 16);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) {
				oscFunc(&bundle, "l_knee", 17);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) {
				oscFunc(&bundle, "l_ankle", 18);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0) {
				oscFunc(&bundle, "l_foot", 19);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0) {
				oscFunc(&bundle, "r_hip", 20);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0) {
				oscFunc(&bundle, "r_knee", 21);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0) {
				oscFunc(&bundle, "r_ankle", 22);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0) {
				oscFunc(&bundle, "r_foot", 23);
			}
			if (lo_send_bundle(addr, bundle) != 0) { 
				printf("error: unable to send bundle\n");
				lo_bundle_pp(bundle);
			}
			lo_bundle_free_messages(bundle);
		}
		else {
			//Send user's center of mass
			sendUserPosMsg(aUsers[i]);
		}
	}
}

void DrawJoint(XnUserID player, XnSkeletonJoint eJoint)
{
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
	{
		return;
	}

	XnSkeletonJointPosition joint;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	if (joint.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt;
	pt = joint.position;

	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);

	drawCircle(pt.X, pt.Y, 2);

	//////////////////////////////////code/////////////////////////////////////////


	if(eJoint==XN_SKEL_LEFT_HAND && tmp==0 && player==1){//左手
		//printf("ske,%d,0,%f,%f,%f,",player,pt.X,pt.Y,pt.Z);
		//fflush(stdout);
		za_tmpX[zaC]=pt.X;
		za_tmpY[zaC]=pt.Y;
		za_tmpZ[zaC]=pt.Z;
		H_L_X=pt.X;
		H_L_Y=pt.Y;
		H_L_Z=pt.Z;
		tmp=1;
		zaC++;

	}else if(eJoint==XN_SKEL_RIGHT_HAND && tmp==1 && player==1){//右手
		//printf("ske,%d,1,%f,%f,%f,",player,pt.X,pt.Y,pt.Z);
		za_tmpX[zaC]=pt.X;
		za_tmpY[zaC]=pt.Y;
		za_tmpZ[zaC]=pt.Z;
		H_R_X=pt.X;
		H_R_Y=pt.Y;
		H_R_Z=pt.Z;
		tmp=0;
		zaC++;
		LRh=1;
		//printf("zaC=%d\n",zaC);
	}
	


	//左zaC_MIN<-0,2,4,6,8,10,12,14=zaC_MAX-2
	//右zaC_MIN+1<-1,3...15=zaC_MAX-1
	//za_tmpなんとか
	//ズーム判定
	//else if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手)
	if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手)
		/*
		if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ
			X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ
			X=X-(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}

		if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ
			Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ
			Y=Y-(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}
		*/

		if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ
			X=X+(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ
			X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}

		if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ
			Y=Y+(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ
			Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}



		/*
		if(X>=10000) X=10000;
		if(X<=(-10000)) X=(-10000);

		if(Y>=10000) X=10000;
		if(Y<=(-10000)) Y=(-10000);
		*/
//.........这里部分代码省略.........

int main(int argc, char **argv) {
    ros::init(argc, argv, "openni_tracker1");
    ros::NodeHandle nh;
/*
    string configFilename = ros::package::getPath("openni_tracker1") + "/openni_tracker1.xml";
    XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
    CHECK_RC(nRetVal, "InitFromXml"); */


XnStatus nRetVal = g_Context.Init();
    CHECK_RC(nRetVal, "Init");
    nRetVal = g_Context.OpenFileRecording("/home/latifanjum/Downloads/OpenNI-master/Platform/Linux/Bin/x86-Release/Dataset/Wave2/wave25.oni", g_Player);
    g_Player.SetRepeat( false );
    if (nRetVal != XN_STATUS_OK)
    {
    printf("Can't open recording %s\n", xnGetStatusString(nRetVal));
    return 1;
    }


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

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

	if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
		ROS_INFO("Supplied user generator doesn't support skeleton");
		return 1;
	}

    XnCallbackHandle hUserCallbacks;
	g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);

	XnCallbackHandle hCalibrationCallbacks;
	g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

	if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) {
		g_bNeedPose = TRUE;
		if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
			ROS_INFO("Pose required, but not supported");
			return 1;
		}

		XnCallbackHandle hPoseCallbacks;
		g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);

		g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
	}

	g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

	nRetVal = g_Context.StartGeneratingAll();
	CHECK_RC(nRetVal, "StartGenerating");

	ros::Rate r(30);

        
        ros::NodeHandle pnh("~");
        string frame_id("openni_depth_frame");
        pnh.getParam("camera_frame_id", frame_id);
                
	while (ros::ok()) {
		g_Context.WaitAndUpdateAll();
		publishTransforms(frame_id);
		r.sleep();
	}

	g_Context.Shutdown();
	return 0;
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "PersonTracker");
	ros::NodeHandle nh;
	ros::Rate loop_rate(10);
    ROS_INFO("Initalizing Arm Connection....");
    ros::Publisher leftArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n0/position", 1000);
    ros::Publisher rightArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n1/position", 1000);
	yourmom = nh.advertise<prlite_kinematics::some_status_thing>("kinect_hack_status", 1000);
    ros::Duration(2.0).sleep();
    prlite_kinematics::SphereCoordinate coord;
	prlite_kinematics::some_status_thing status;
    coord.radius = 35.0;
    coord.phi = 0.0;
    coord.theta = 0.0;
	status.lulz = 0;	//initialized/reset
	yourmom.publish(status);
    leftArmPublisher.publish(coord);
    rightArmPublisher.publish(coord);
	ROS_INFO("Initalizing Kinect + NITE....");
	XnStatus nRetVal = XN_STATUS_OK;
	xn::EnumerationErrors errors;
	nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, &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, "Find depth generator");
	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");
	}

	XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
	if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
	{
		printf("Supplied user generator doesn't support skeleton\n");
		return 1;
	}
	g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
	g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

	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;
		}
		g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
		g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
	}

	g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

	nRetVal = g_Context.StartGeneratingAll();
	CHECK_RC(nRetVal, "StartGenerating");
    ROS_INFO("Ready To Go!\n");

	while (ros::ok()) {
		// Update OpenNI
		g_Context.WaitAndUpdateAll();
		xn::SceneMetaData sceneMD;
		xn::DepthMetaData depthMD;
		g_DepthGenerator.GetMetaData(depthMD);
		g_UserGenerator.GetUserPixels(0, sceneMD);
		// Print positions
		XnUserID aUsers[15];
		XnUInt16 nUsers = 15;
		g_UserGenerator.GetUsers(aUsers, nUsers);
		for (int i = 0; i < nUsers; ++i) {
			if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
				// Read joint positions for person (No WRISTs)
				XnSkeletonJointPosition torsoPosition, lShoulderPosition, rShoulderPosition, neckPosition, headPosition, lElbowPosition, rElbowPosition;
                XnSkeletonJointPosition rWristPosition, lWristPosition, rHipPosition, lHipPosition, lKneePosition, rKneePosition;
                XnSkeletonJointPosition lFootPosition, rFootPosition;
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_TORSO, torsoPosition);
                g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_NECK, neckPosition);
                g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, headPosition);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderPosition);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderPosition);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowPosition);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowPosition);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, lWristPosition);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, rWristPosition);
                g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HIP, lHipPosition);
                g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HIP, rHipPosition);
                g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_KNEE, lKneePosition);
                g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneePosition);
//.........这里部分代码省略.........