Exemple #1
0
void sendUserPosMsg(XnUserID id) {
	osc::OutboundPacketStream p(osc_buffer, OUTPUT_BUFFER_SIZE);
	XnPoint3D com;
	sprintf(tmp, "/user/%d", id);
	p << osc::BeginBundleImmediate;
	p << osc::BeginMessage(tmp);
	userGenerator.GetCoM(id, com);

	if (!raw)
	{
	  p << (float)(off_x + (mult_x * (1280 - com.X) / 2560));
	  p << (float)(off_y + (mult_y * (1280 - com.Y) / 2560));
	  p << (float)(off_z + (mult_z * com.Z * 7.8125 / 10000));
	}
	else
	{
	  p << com.X;
	  p << com.Y;
	  p << com.Z;
	}

	p << osc::EndMessage;
	p << osc::EndBundle;
	transmitSocket->Send(p.Data(), p.Size());
}
Exemple #2
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 writeUserPosition(string *s, XnUserID id) {
	XnPoint3D com;
	userGenerator.GetCoM(id, com);

	if (fabsf( com.X - 0.0f ) > 0.1f)
	{
		char tmp[1024];

		sprintf(tmp, "{\"userid\":%u,\"X\":%.3f,\"Y\":%.3f,\"Z\":%.3f}\n", id, com.X, com.Y, com.Z);
		*s += tmp;
	}
}
void publishTransform(XnUserID const& user, string const& frame_id, string const& child_frame_id) {
    static tf::TransformBroadcaster br;
    
    XnPoint3D icom;
    XnPoint3D com;
    g_UserGenerator.GetCoM(user,com);
    //g_DepthGenerator.ConvertProjectiveToRealWorld(1,&icom,&com);
    
    /*XnSkeletonJointPosition joint_position;
    g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
    double x = -joint_position.position.X / 1000.0;
    double y = joint_position.position.Y / 1000.0;
    double z = joint_position.position.Z / 1000.0;

    XnSkeletonJointOrientation joint_orientation;
    g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);

    XnFloat* m = joint_orientation.orientation.elements;
    KDL::Rotation rotation(m[0], m[1], m[2],
    					   m[3], m[4], m[5],
    					   m[6], m[7], m[8]);
    double qx, qy, qz, qw;
    rotation.GetQuaternion(qx, qy, qz, qw);*/
    KDL::Rotation rotation(0,0,0,
    					   0,0,0,
    					   0,0,0);
    double qx, qy, qz, qw;
    rotation.GetQuaternion(qx, qy, qz, qw);

    char child_frame_no[128];
    snprintf(child_frame_no, sizeof(child_frame_no), "%s_%d", child_frame_id.c_str(), user);

    tf::Transform transform;
    double x = -com.X/1000;
    double y = com.Y/1000;
    double z = com.Z/1000;
    transform.setOrigin(tf::Vector3(x, y, z));
    ROS_INFO("X: %d\t%d\n Y: %d\t%d\nZ: %d\t%d\n",x,com.X,y,com.Y,z,com.Z);
    //transform.setRotation(tf::Quaternion(0, 0, 0, 0));
    transform.setRotation(tf::Quaternion(qx, -qy, -qz, qw));

    // #4994
    tf::Transform change_frame;
    change_frame.setOrigin(tf::Vector3(0, 0, 0));
    tf::Quaternion frame_rotation;
    frame_rotation.setEulerZYX(1.5708, 0, 1.5708);
    change_frame.setRotation(frame_rotation);

    transform = change_frame * transform;

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_no));
}
Exemple #5
0
void gl_labelCapturingUser(XnUserID id)
{
  XnVector3D pos;
  gUserGenerator.GetCoM(id, pos);
  gDepthGenerator.ConvertRealWorldToProjective(1, &pos, &pos);
  glColor4f(0, 0, 0, 0);
  xn::DepthMetaData depthMD;
  gDepthGenerator.GetMetaData(depthMD);
  glRasterPos2i(pos.X/depthMD.XRes()*WIN_SIZE_X, 
		pos.Y/depthMD.YRes()*WIN_SIZE_Y);
  gl_printString(GLUT_BITMAP_HELVETICA_18, "Capturing");
  glColor4f(1.0, 1.0, 1.0, 1.0);
}
bool checkCenterOfMass(XnUserID const& user)
{
  XnPoint3D center_of_mass;
  XnStatus status = g_UserGenerator.GetCoM(user, center_of_mass);

  if(status != XN_STATUS_OK || (center_of_mass.X == 0 && center_of_mass.Y == 0 && center_of_mass.Z == 0))
  {
    return false;
  }
  else
  {
    return true;
  }
}
Exemple #7
0
// this function is called each frame
void glutDisplay (void)
{

	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Setup the OpenGL viewpoint
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();

	xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	g_DepthGenerator.GetMetaData(depthMD);
	#ifdef USE_GLUT
	glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
	#else
	glOrthof(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
	#endif

	glDisable(GL_TEXTURE_2D);

	if (!g_bPause)
	{
		// Read next available data
		g_Context.WaitOneUpdateAll(g_DepthGenerator);
	}

		// Process the data
		//DRAW
		g_DepthGenerator.GetMetaData(depthMD);
		g_UserGenerator.GetUserPixels(0, sceneMD);
		DrawDepthMap(depthMD, sceneMD, g_nPlayer);

		if (g_nPlayer != 0)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(g_nPlayer, com);
			if (com.Z == 0)
			{
				g_nPlayer = 0;
				FindPlayer();
			}
		}

	#ifdef USE_GLUT
	glutSwapBuffers();
	#endif
}
Exemple #8
0
XnBool AssignPlayer(XnUserID user)
{
	if (g_nPlayer != 0)
		return FALSE;

	XnPoint3D com;
	g_UserGenerator.GetCoM(user, com);
	if (com.Z == 0)
		return FALSE;

	printf("Matching for existing calibration\n");
	g_UserGenerator.GetSkeletonCap().LoadCalibrationData(user, 0);
	g_UserGenerator.GetSkeletonCap().StartTracking(user);
	g_nPlayer = user;
	return TRUE;

}
Exemple #9
0
void sendUserPosMsg(XnUserID id) {
	XnPoint3D com;
	sprintf(oscPositionAddressBuff[0], "/user/%d", id);
	lo_bundle bundle = lo_bundle_new(LO_TT_IMMEDIATE);
	lo_message msg = lo_message_new();

	userGenerator.GetCoM(id, com);

	if (!raw)
	{
		lo_message_add_float(msg, (float)(off_x + (mult_x * (1280 - com.X) / 2560)));
		lo_message_add_float(msg, (float)(off_y + (mult_y * (1280 - com.Y) / 2560)));
		lo_message_add_float(msg, (float)(off_z + (mult_z * com.Z * 7.8125 / 10000)));
	}
	else
	{
		lo_message_add_float(msg,com.X);
		lo_message_add_float(msg,com.Y);
		lo_message_add_float(msg,com.Z);
	}

	lo_bundle_add_message(bundle, oscPositionAddressBuff[0], msg);
	lo_send_bundle(addr, bundle);
}
Exemple #10
0
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player)
{
	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();
	{
		XnUInt32 nIndex = 0;
		// Prepare the texture map
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++, nIndex++)
			{
				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];
				}
				else
				{
					pDestImage[0] = 0;
					pDestImage[1] = 0;
					pDestImage[2] = 0;
				}

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

			pDestImage += (texWidth - g_nXRes) *3;
		}
	}

	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[20] = "";
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		XnPoint3D com;
		g_UserGenerator.GetCoM(aUsers[i], com);
		g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

		if (aUsers[i] == player)
			sprintf(strLabel, "%d (Player)", aUsers[i]);
		else
			sprintf(strLabel, "%d", aUsers[i]);

		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 of user
	if (player != 0)
	{
		glBegin(GL_LINES);
		glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
    
    // gesture
    static int gesture = 0;
    static XnPoint3D previousLeftHandPt;
    XnPoint3D newLeftHandPt;
    newLeftHandPt = getJointPoint(player, XN_SKEL_LEFT_HAND);
    
    if(previousLeftHandPt.X > 0 && previousLeftHandPt.X < 640)
      if(previousLeftHandPt.X - newLeftHandPt.X > 60)
          gesture = 1;
      else if(previousLeftHandPt.X - newLeftHandPt.X < -60)
          gesture = 2;
      else if(previousLeftHandPt.Y - newLeftHandPt.Y > 60)
          gesture = 3;
      else if(previousLeftHandPt.Y - newLeftHandPt.Y < -60)
          gesture = 4;
      else
          gesture = 0;
    if(gesture != 0)
      printf("gesture: %d\n", gesture);

    previousLeftHandPt = newLeftHandPt;
		
    // head
    XnPoint3D pt = getJointPoint(player, XN_SKEL_HEAD);
    
    // save
    saveLocation(pt, newLeftHandPt, gesture);
    

    DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK);

		DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
		DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
		DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

		DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
		DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
		DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

		DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
		DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);

		DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
		DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
		DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);

		DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
		DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
		DrawLimb(player, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
		glEnd();
	}
}
Exemple #11
0
// this function is called each frame
void glutDisplay (void)
{

	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Setup the OpenGL viewpoint
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();

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

	xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	xn::ImageMetaData imageMD;
	g_DepthGenerator.GetMetaData(depthMD);
	g_ImageGenerator.GetMetaData(imageMD);
	#ifdef USE_GLUT
	glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
	#else
	glOrthof(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
	#endif

	glDisable(GL_TEXTURE_2D);

	if (!g_bPause)
	{
		// Read next available data
		g_Context.WaitAndUpdateAll();
	}

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

		g_DepthGenerator.GetMetaData(depthMD);
		g_ImageGenerator.GetMetaData(imageMD);
		g_UserGenerator.GetUserPixels(0, sceneMD);


		DrawDepthMap(depthMD, imageMD, sceneMD, g_nPlayer);

		if (g_nPlayer != 0)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(g_nPlayer, com);
			if (com.Z == 0)
			{
				g_nPlayer = 0;
				FindPlayer();
			}
		}

	#ifdef USE_GLUT
	glutSwapBuffers();
	#endif
}
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 not initialized
	set parameters and reserve memory space
	*/
	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();

	// get the depth resolution
	static unsigned int nZRes = dmd.ZRes();
	static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));

	// Calculate the accumulative histogram
	memset(pDepthHist, 0, nZRes*sizeof(float));

	// count the number of pixels of every possible depth value
	for (nY=0; nY<g_nYRes; nY++)
	{
		for (nX=0; nX<g_nXRes; nX++)
		{
			nValue = *pDepth;

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

			pDepth++;
		}
	}

	for (nIndex=1; nIndex<nZRes; nIndex++)
	{
		pDepthHist[nIndex] += pDepthHist[nIndex-1];
	}

	// calculate percentage for every depth value
	// the larger the value is, the darker the pixel should be 
	if (nNumberOfPoints)
	{
		for (nIndex=1; nIndex<nZRes; nIndex++)
		{
			pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (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 = 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)
	{
#ifndef USE_GLES
		if (g_bPrintID)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(aUsers[i], com);
			g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

			XnUInt32 nDummy = 0;

			xnOSMemSet(strLabel, 0, sizeof(strLabel));
			if (!g_bPrintState)
			{
				// Tracking
				xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
			{
				// Tracking
				xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Tracking", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
			{
				// Calibrating
				xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
			}
			else
			{
				// Nothing
				xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%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);
		}
#endif
		if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{
			glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);

			// Draw Joints
			if (g_bMarkJoints)
			{
				// Try to draw all joints
				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);
			}

#ifndef USE_GLES
			glBegin(GL_LINES);
#endif

			// Draw Limbs
			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);
			if (!DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_WRIST))
			{
				DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
			}
			else
			{
				DrawLimb(aUsers[i], XN_SKEL_LEFT_WRIST, XN_SKEL_LEFT_HAND);
				DrawLimb(aUsers[i], XN_SKEL_LEFT_HAND, XN_SKEL_LEFT_FINGERTIP);
			}


			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
			if (!DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_WRIST))
			{
				DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
			}
			else
			{
				DrawLimb(aUsers[i], XN_SKEL_RIGHT_WRIST, XN_SKEL_RIGHT_HAND);
				DrawLimb(aUsers[i], XN_SKEL_RIGHT_HAND, XN_SKEL_RIGHT_FINGERTIP);
			}

			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);
#ifndef USE_GLES
			glEnd();
#endif
		}
	}

	if (g_bPrintFrameID)
	{
		static XnChar strFrameID[80];
		xnOSMemSet(strFrameID, 0, 80);
		XnUInt32 nDummy = 0;
		xnOSStrFormat(strFrameID, sizeof(strFrameID), &nDummy, "%d", dmd.FrameID());

		glColor4f(1, 0, 0, 1);

		glRasterPos2i(10, 10);

		glPrintString(GLUT_BITMAP_HELVETICA_18, strFrameID);
	}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player, xn::ImageMetaData& imd)
{
	texWidth = 640;
	texHeight = 480;


	LEFT = 0; RIGHT = 640;
	TOP = 0; BOTTOM = 480;

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

	pDestImage = pDepthTexBuf;

	pDepth = dmd.Data();
	pixel = imd.RGB24Data();
	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)));
		}
	}
//	printf("Debug: %i\n",focus);
	pDepth = (short unsigned int*)dmd.Data();
	///{
		// Prepare the texture map
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++)
			{

				nValue = *pDepth;
				if(nX == (int)centerScreen[0] && nY == (int)centerScreen[1]){
					if (calibrationMode){
						depthVal = nValue;
//						printf("depthVal: %i\n",depthVal);
					}
				}
					//printf("Depth: %i \n",nValue);
				label = *pLabels;
//				XnUInt32 nColorID = label % nColors;
				if (label != focus)
				{
					if(calibrationMode){
						pDestImage[0] = pixel->nRed;
						pDestImage[1] = pixel->nGreen;
						pDestImage[2] = pixel->nBlue;
						pDestImage[3] = 255;
					} else {
						pDestImage[0] = 0;
						pDestImage[1] = 0;
						pDestImage[2] = 0;
						pDestImage[3] = 0;
					}
				} else {
					pDestImage[0] = pixel->nRed;
					pDestImage[1] = pixel->nGreen;
					pDestImage[2] = pixel->nBlue;
					pDestImage[3] = 255;

					//find max/min values for width and height boundaries
					if (nX > (unsigned int)LEFT) {
						LEFT = nX;
					}

					if (nX < (unsigned int)RIGHT) {
						RIGHT = nX;
					}

					if (nY > (unsigned int)TOP) {
						TOP = nY;
					}

					if (nY < (unsigned int)BOTTOM) {
						BOTTOM = nY;
					}
				}

				pixel++;
				pDepth++;
				pLabels++;
				pDestImage+=4;
			}

			pDestImage += (texWidth - g_nXRes) *4;
		}
	//}*/

	// Display the OpenGL texture map
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, texWidth, texHeight, 0, GL_RGBA, GL_UNSIGNED_BYTE, pDepthTexBuf);

	nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	g_UserGenerator.GetCoM(aUsers[0], com);


	CENTER = com.X;

	//glDisable(GL_BLEND);
	//glDisable(GL_TEXTURE_2D);
/*
	char strLabel[20] = "";
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		XnPoint3D com;
		g_UserGenerator.GetCoM(aUsers[i], com);
		g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

		if (aUsers[i] == player)
			sprintf(strLabel, "%d (Player)", aUsers[i]);
		else
			sprintf(strLabel, "%d", aUsers[i]);

		//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 of user
//		glBegin(GL_LINES);
//		//glColor4f(1,0,0,1.0f);
//		glVertex3f(-1, 1, 0);
//		glVertex3f(1, -1, 0);
//		//glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
//		DrawLimb(1, XN_SKEL_HEAD, XN_SKEL_NECK);
//
//		DrawLimb(1, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
//		DrawLimb(1, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
//		DrawLimb(1, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
//
//		DrawLimb(1, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
//		DrawLimb(1, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
//		DrawLimb(1, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
//
//		DrawLimb(1, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
//		DrawLimb(1, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
//
//		DrawLimb(1, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
//		DrawLimb(1, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
//		DrawLimb(1, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
//
//		DrawLimb(1, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
//		DrawLimb(1, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
//		DrawLimb(1, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
//		glEnd();

	//XnSkeletonJoint eJoint1 = XN_SKEL_RIGHT_HAND;

}
Exemple #14
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)
    {
#ifndef USE_GLES
        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);
        }
#endif
        if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
        {
#ifndef USE_GLES
            glBegin(GL_LINES);
#endif
            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);


            //---------------- get skeleton data ----------------------

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_HEAD, head);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_NECK, neck);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_TORSO, torso);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_SHOULDER, leftShoulder);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_ELBOW, leftElbow);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_HAND, leftHand);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_SHOULDER, rightShoulder);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_ELBOW, rightElbow);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_HAND, rightHand);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_HIP, leftHip);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_KNEE, leftKnee);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_FOOT, leftFoot);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_HIP, rightHip);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_KNEE, rightKnee);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_FOOT, rightFoot);

            //-------------- store frame data in frameData ------------
            char headX[30], headY[30], headZ[30], headC[30];
            strcpy(frameData, "");
            char space[2] = " ";

            sprintf(headX, "%f", head.position.X);
            sprintf(headY, "%f", head.position.Y);
            sprintf(headZ, "%f", head.position.Z);
            sprintf(headC, "%f", head.fConfidence);

            strcat(frameData, headX);
            strcat(frameData, space);
            strcat(frameData, headY);
            strcat(frameData, space);
            strcat(frameData, headZ);
            strcat(frameData, space);
            strcat(frameData, headC);
            strcat(frameData, space);

            //std::cout << "Frame data: " << frameData << std::endl;
            std::cout << "frameData to be sent: " << frameData << std::endl;
            HandleConnection(frameData);



            ////------- print to console -------
            //xnOSGetTimeStamp(&nNow);
            //nNow /= 1000;
            //std::cout << "-------------time: " << nNow << "---------------\n"
            //	<< "User " << aUsers[i] <<std::endl
            //	<< "	Head: " << head.position.X << " "
            //	<< head.position.Y << " " << head.position.Z
            //	<< head.fConfidence << std::endl;


#ifndef USE_GLES
            glEnd();
#endif
        }
    }
}
/// execution loop: runs the hole time: main logic
void executeInputLoop() {
	PackedType<Vector3<double> > packedVector3;
	//get the new informations and sent it over the udp socket to the tui server
	while (true) {
	    /// wait for new kinect data
        g_Context.WaitOneUpdateAll(g_UserGenerator);

        /// max 15 users
		XnUserID aUsers[15];
		XnUInt16 nUsers = 15;
		g_UserGenerator.GetUsers(aUsers, nUsers);

        /// track only the closest person
        //todo: hau das in eine einzelne function!!
		std::vector<XnPoint3D> userCoM(nUsers);
		for (int i = 0; i < nUsers; ++i) {
            g_UserGenerator.GetCoM(aUsers[i], userCoM[i]);
		}

        //calculate the user with the minimum distance to the kinect
        float minDist = FLT_MAX;
        int minUser = -1;
        float tmp_minDist = 0.0f;

		for (int i = 0; i < nUsers; ++i){
		    if (userCoM[i].X + userCoM[i].Y + userCoM[i].Z == 0) {
                continue;
		    }
            tmp_minDist = userCoM[i].X*userCoM[i].X + userCoM[i].Y*userCoM[i].Y + userCoM[i].Z*userCoM[i].Z;
		    if(tmp_minDist < minDist && tmp_minDist > 0.0){
                minDist = tmp_minDist;
                minUser = i;
		    }
		    //std::cout << "user: "******"sending user: " << aUsers[minUser] << std::endl;
			vector<pair<int, Vector3<double> > > & v = packedVector3.getItems();
			v.clear();

			addJoint(aUsers[minUser], XN_SKEL_HEAD           , SKEL_HEAD           , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_NECK           , SKEL_NECK           , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_TORSO          , SKEL_TORSO          , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_WAIST          , SKEL_WAIST          , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_COLLAR    , SKEL_LEFT_COLLAR    , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_SHOULDER  , SKEL_LEFT_SHOULDER  , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_ELBOW     , SKEL_LEFT_ELBOW     , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_WRIST     , SKEL_LEFT_WRIST     , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_HAND      , SKEL_LEFT_HAND      , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_FINGERTIP , SKEL_LEFT_FINGERTIP , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_COLLAR   , SKEL_RIGHT_COLLAR   , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_SHOULDER , SKEL_RIGHT_SHOULDER , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_ELBOW    , SKEL_RIGHT_ELBOW    , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_WRIST    , SKEL_RIGHT_WRIST    , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_HAND     , SKEL_RIGHT_HAND     , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_FINGERTIP, SKEL_RIGHT_FINGERTIP, packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_HIP       , SKEL_LEFT_HIP       , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_KNEE      , SKEL_LEFT_KNEE      , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_ANKLE     , SKEL_LEFT_ANKLE     , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_LEFT_FOOT      , SKEL_LEFT_FOOT      , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_HIP      , SKEL_RIGHT_HIP      , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_KNEE     , SKEL_RIGHT_KNEE     , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_ANKLE    , SKEL_RIGHT_ANKLE    , packedVector3);
			addJoint(aUsers[minUser], XN_SKEL_RIGHT_FOOT     , SKEL_RIGHT_FOOT     , packedVector3);

			sendPackedVector3(packedVector3);
        }

		//if we use openGL, than we have to break up after one send
		//s.t. openGL can draw a new scene
		if(enableOpenGL) {
			break;
		} else {
			Sleep(10);
		}
	}
}
Exemple #16
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();

		}
	}
}
Exemple #17
0
// this function is called each frame
void glutDisplay (void)
{
	static ros::Duration pub_interval(1.0/pub_rate_temp_);
	static ros::Time     last_pub(0.0);
	static int           num_skipped = 0;
	
	num_skipped++;
	
	ros::Time now_time = ros::Time::now();
	
	// Update stuff from OpenNI
	XnStatus status = g_Context.WaitAndUpdateAll();
	if( status != XN_STATUS_OK ){
		ROS_ERROR_STREAM("Updating context failed: " << status);
		return;
	}
	
	xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	xn::ImageMetaData imageMD;
	g_DepthGenerator.GetMetaData(depthMD);
	g_UserGenerator.GetUserPixels(0, sceneMD);
	//g_ImageGenerator.GetMetaData(imageMD);
	
	cv::Mat depth_image;
	getDepthImage(depthMD, depth_image);
	
	double minval, maxval;
	cv::minMaxLoc(depth_image, &minval, &maxval);
	
	// Convert user pixels to an OpenCV image
	cv::Mat label_image;
	getUserLabelImage(sceneMD, label_image);
	
	sensor_msgs::PointCloud cloud;
	cloud.header.stamp    = now_time;
	cloud.header.frame_id = frame_id_;
	cloud.channels.resize(1);
	cloud.channels[0].name = "intensity";
	
	// Convert users into better format
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	
	cv::Mat      this_mask;
	XnPoint3D    center_mass;
	double       pixel_area;
	cv::Scalar   s;
	vector<user> users;
		
	if( g_bhasCal && now_time-last_pub > pub_interval )
	{
		bool has_lock = false;
		last_pub = now_time;
		ROS_DEBUG_STREAM(num_skipped << " refreshes inbetween publishing");
		num_skipped = 0;
		
		cv::imshow( "Tracked user"        , user_cal_.getImage() );
		cv::imshow( "Original calibration", original_cal_.getImage() );
		
		cv::Mat rgb, rgb_mask;
//		getRGB(rgb, rgb_mask);
		rgb = getRGB(imageMD);
		
		for (unsigned int i = 0; i < nUsers; i++)
		{
			user this_user;
			this_user.uid = aUsers[i];
			
			// Bitwise mask of pixels belonging to this user
			this_mask = (label_image == this_user.uid);
			this_user.numpixels = cv::countNonZero(this_mask);
			
			// Compare this user to the target
			this_user.pc.init(rgb, this_mask);
			double similarity  = this_user.pc.compare(user_cal_    );
			double sim_to_orig = this_user.pc.compare(original_cal_);
			this_user.similarity         = similarity;
			this_user.similarity_to_orig = sim_to_orig;
			
			/*
			ros::Time t1 = ros::Time::now();
			double emd         = this_user.pc.getEMD (user_cal_    );
			double emd_to_orig = this_user.pc.getEMD (original_cal_);
			ros::Duration d = (ros::Time::now() - t1);
			ROS_INFO_STREAM("EMD took " << (d.sec) );*/
			
			if( now_time > save_timer_ ){
				ROS_WARN_STREAM("[bk_skeletal_tracker] Say Cheezbuger");
				save_timer_ = now_time + ros::Duration(60*60*24);
				g_bSaveFrame = true;
			}
			
			if( g_bSaveFrame )
			{
				time_t t = ros::WallTime::now().sec;
				char buf[1024] = "";
				struct tm* tms = localtime(&t);
				strftime(buf, 1024, "%Y-%m-%d-%H-%M-%S", tms);
				
				std::string prefix = ( boost::format("capture_%s_user%d") % buf % this_user.uid ).str();				
				cv::Mat rgb_masked;
				rgb.copyTo(rgb_masked, this_mask);
				
				saveMat(rgb_masked             , prefix + "_rgb"  );
				saveMat(this_mask              , prefix + "_mask" );
				saveMat(this_user.pc.getHist() , prefix + "_hist" );
				saveMat(this_user.pc.getImage(), prefix + "_himg" );
			}
			
			// Mean depth
			this_user.meandepth = cv::mean(depth_image, this_mask)[0];
		
			this_user.silhouette_area = 0;
		
			// Find the area of the silhouette in cartesian space
			for( int i=0; i<this_mask.rows; i++) {
				for( int j=0; j<this_mask.cols; j++ ) {
					if( this_mask.at<uchar>(i,j) != 0 )
					{
						pixel_area = cam_model_.getDeltaX(1, depth_image.at<float>(i,j))
										   * cam_model_.getDeltaY(1, depth_image.at<float>(i,j));
						this_user.silhouette_area += pixel_area;
					}
				}
			}
			// Find the center in 3D
			g_UserGenerator.GetCoM(this_user.uid, center_mass);
			this_user.center3d.point = vecToPt(center_mass);
		
			ROS_DEBUG_STREAM(boost::format("User %d: area %.3fm^2, mean depth %.3fm")
				% (unsigned int)this_user.uid % this_user.silhouette_area % this_user.meandepth);
		
			// Screen out unlikely users based on area
			if( this_user.meandepth > min_dist_ && this_user.silhouette_area < max_area_ && this_user.silhouette_area > min_area_ )
			{
				ROS_INFO_STREAM(boost::format("User %d   new: %.0f --- orig: %.0f")
					% ((int)this_user.uid) % (100*similarity) % (100*sim_to_orig) );
				/*ROS_INFO_STREAM(boost::format("EMD       new: %.2f --- orig: %.2f")
					% (emd) % (emd_to_orig) );*/
					
				if( similarity > PersonCal::getMatchThresh() ) {
					user_cal_.update(rgb, this_mask);
				}
				else{
					if( sim_to_orig > PersonCal::getMatchThresh() ) {
						ROS_WARN_STREAM("Reset to original calibration");
						user_cal_ = original_cal_;
					}
				}
			
				std::stringstream window_name;
				window_name << "user_" << ((int)this_user.uid);
				cv::imshow(window_name.str(), this_user.pc.getImage());
			
			
				ROS_DEBUG("Accepted user");
				users.push_back(this_user);
				
				// Visualization
				geometry_msgs::Point32 p;
				p.x = this_user.center3d.point.x;
				p.y = this_user.center3d.point.y;
				p.z = this_user.center3d.point.z;
				cloud.points.push_back(p);
				cloud.channels[0].values.push_back(0.0f);
			}
		}
	
	
		// Try to associate the tracker with a user
		if( latest_tracker_.first != "" )
		{
			// Transform the tracker to this time. Note that the pos time is updated but not the restamp.
			tf::Point pt;
			tf::pointMsgToTF(latest_tracker_.second.pos.pos, pt);
			tf::Stamped<tf::Point> loc(pt, latest_tracker_.second.pos.header.stamp, latest_tracker_.second.pos.header.frame_id);
			try {
				tfl_->transformPoint(frame_id_, now_time-ros::Duration(.1), loc, latest_tracker_.second.pos.header.frame_id, loc);
				latest_tracker_.second.pos.header.stamp    = now_time;
				latest_tracker_.second.pos.header.frame_id = frame_id_;
				latest_tracker_.second.pos.pos.x = loc[0];
				latest_tracker_.second.pos.pos.y = loc[1];
				latest_tracker_.second.pos.pos.z = loc[2];
			}
			catch (tf::TransformException& ex) {
				ROS_ERROR("(finding) Could not transform person to this time");
			}
			  
			people_msgs::PositionMeasurement pos;
			if( users.size() > 0 )
			{
				std::stringstream users_ss;
				users_ss << boost::format("(finding) Tracker \"%s\" = (%.2f,%.2f) Users = ") 
					% latest_tracker_.first % latest_tracker_.second.pos.pos.x   % latest_tracker_.second.pos.pos.y;
			
				// Find the closest user to the tracker
				user closest;
				closest.distance = BIGDIST_M;
	
				foreach(user u, users)
				{
					u.distance = pow(latest_tracker_.second.pos.pos.x - u.center3d.point.x, 2.0)
						         + pow(latest_tracker_.second.pos.pos.y - u.center3d.point.y, 2.0);
					
					users_ss << boost::format("(%.2f,%.2f), ") % u.center3d.point.x % u.center3d.point.y;
					
					if( u.distance < closest.distance )
					{
						if( u.similarity > PersonCal::getMatchThresh() ) {
							closest = u;
						}
						else {
							ROS_WARN_STREAM("Ignored close user not matching (" << u.uid << ")");
						}
					}
				}//foreach