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