void SegmentUser(XnUserID user, cv::Mat *input, const xn::SceneMetaData& smd) { const XnLabel* pLabels = smd.Data(); for(int y = 0; y < input->rows; y++) { unsigned char *row = input->ptr<unsigned char>(y); for (int x = 0; x < input->cols; x++) { XnLabel label = *pLabels++; if (label != user) { *row++ = 0; *row++ = 0; *row++ = 0; } else { row+=3; } } } }
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(); } } }
void SceneDrawer::updateScene(const xn::DepthMetaData &dmd, const xn::SceneMetaData &smd) { if (!initialized) init(dmd.XRes(), dmd.YRes(), dmd.ZRes()); unsigned int val = 0, numOfPoints = 0; unsigned int xRes = dmd.XRes(), yRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // calculate the accumulative depth histogram memset(pDepthHist, 0, zRes*sizeof(float)); int numOfIterations = xRes * yRes; for (int i = 0; i < numOfIterations; ++i, ++pDepth){ val = *pDepth; if (val != 0){ pDepthHist[val]++; numOfPoints++; } } for (int i = 1; i < zRes; ++i) pDepthHist[i] += pDepthHist[i-1]; if (numOfPoints > 0){ for (int i = 0; i < zRes; ++i) pDepthHist[i] = floor(256.0f*(1.0f-pDepthHist[i]/(float)numOfPoints)); } // turn depth map to a colored texture image pDepth = dmd.Data(); XnUInt32 ci; XnLabel label; unsigned int histVal; for (int i = 0; i < numOfIterations; ++i, ++pDepth, ++pLabels, pDestImage += 3){ val = *pDepth; label = *pLabels; if (label != 0) ci = label % nColors; else ci = nColors; if (val != 0){ histVal = pDepthHist[val]; pDestImage[0] = histVal * colors[ci][0]; pDestImage[1] = histVal * colors[ci][1]; pDestImage[2] = histVal * colors[ci][2]; } else pDestImage[0] = pDestImage[1] = pDestImage[2] = 0; } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); }
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(); } }
void PublishPeopleImage(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, image_transport::Publisher& pub) { const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); unsigned int nValue = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); cv::Mat peopleSegmentation(g_nYRes, g_nXRes, CV_8UC3); // Prepare the texture map for (unsigned int nY=0; nY<g_nYRes; nY++) { uchar* pDestImage = peopleSegmentation.ptr(nY); for (unsigned int nX=0; nX < g_nXRes; nX++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (*pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (nValue != 0) { pDestImage[0] = 255 * Colors[nColorID][0]; pDestImage[1] = 255 * Colors[nColorID][1]; pDestImage[2] = 255 * Colors[nColorID][2]; } } pDepth++; pLabels++; pDestImage+=3; } } // todo: stop and start with respect to odometry: segmentation works best if robot is standing still // publish try { IplImage img = (IplImage)peopleSegmentation; sensor_msgs::ImagePtr msg = (sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8")); msg->header.stamp = ros::Time::now(); pub.publish(msg); } catch (sensor_msgs::CvBridgeException error) { ROS_ERROR("[openni_tracker] Could not convert IplImage to ROS message"); } // cv_bridge::CvImage bridgeImage; did not work // bridgeImage.image = peopleSegmentation.clone(); // sensor_msgs::ImagePtr msg = bridgeImage.toImageMsg(); // pub.publish(msg); // display for checking the output // cv::namedWindow("Test"); // imshow("Test", peopleSegmentation); // uchar key = cv::waitKey(10); // if (key == 'r') // { // g_UserGenerator.StopGenerating(); // std::cout << "stop\n"; // } // if (key == 'c') // { // g_UserGenerator.StartGenerating(); // std::cout << "start\n"; // } }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, ros::Publisher pub_body, edwin::SceneAnalysis scene, edwin::People person) { 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))); } } XnPoint3D coms[20]; XnUInt32 labels[20] = {0}; for (int i = 0; i < 20; ++i) { coms[i] = xnCreatePoint3D(0,0,0); } 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]; if (label < 20 && label > 0) { coms[label].X += nX; coms[label].Y += nY; coms[label].Z += *pDepth; labels[label]++; } } 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[3] = ""; //Initialize the scene array // scene.crowd = []; for (int i = 0; i < 20; ++i) { if (labels[i] == 0) continue; coms[i].X /= labels[i]; coms[i].Y /= labels[i]; coms[i].Z /= labels[i]; sprintf(strLabel, "%d", i); glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(coms[i].X, coms[i].Y); // glRasterPos2i(320, 240); //testing for midpoint of Kinect person.ID = i; person.xpos = coms[i].X; person.ypos = coms[i].Y; person.zpos = coms[i].Z; scene.crowd[i] = person; glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } pub_body.publish(scene); for(int i = 0; i < 20; ++i) { scene.crowd[i].ID = 0; } }
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 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 DepthMapLogger::DumpDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd) { static char name_str[20], comment_str[255]; // Don't do anything if the h5 file is not open if(!p_h5_file_ || !p_frames_group_) { return; } // References to various bits of the HDF5 output H5File &file(*p_h5_file_); Group &frames_group(*p_frames_group_); // This frame's index is the number of frames we've previously saved hsize_t this_frame_idx = frames_group.getNumObjs(); // Create this frame's group snprintf(name_str, 20, "frame_%06lld", this_frame_idx); snprintf(comment_str, 255, "Data for frame %lld", this_frame_idx); Group this_frame_group(frames_group.createGroup(name_str)); this_frame_group.setComment(".", comment_str); // Create attributes for this group Attribute idx_attr = this_frame_group.createAttribute("idx", PredType::NATIVE_HSIZE, DataSpace()); idx_attr.write(PredType::NATIVE_HSIZE, &this_frame_idx); // Create this frame's datasets DSetCreatPropList creat_props; uint16_t fill_value(0); creat_props.setFillValue(PredType::NATIVE_UINT16, &fill_value); hsize_t rows(static_cast<hsize_t>(dmd.YRes())), cols(static_cast<hsize_t>(dmd.XRes())); hsize_t creation_dims[2] = { rows, cols }; hsize_t max_dims[2] = { rows, cols }; DataSpace mem_space(2, creation_dims, max_dims); DataSet depth_ds(this_frame_group.createDataSet( "depth", PredType::NATIVE_UINT16, mem_space, creat_props)); DataSet label_ds(this_frame_group.createDataSet( "label", PredType::NATIVE_UINT16, mem_space, creat_props)); // Get depth and label buffers const uint16_t *p_depths = dmd.Data(); const uint16_t *p_labels = smd.Data(); // Write depth data depth_ds.write(p_depths, PredType::NATIVE_UINT16); // Write label data label_ds.write(p_labels, PredType::NATIVE_UINT16); // Convert non-zero depth values into 3D point positions XnPoint3D *pts = new XnPoint3D[rows*cols]; uint16_t *pt_labels = new uint16_t[rows*cols]; size_t n_pts(0); for(size_t depth_idx(0); depth_idx < rows*cols; ++depth_idx) { // Skip zero depth values if(p_depths[depth_idx] == 0) { continue; } // Store projective-values pts[n_pts].X = depth_idx % cols; pts[n_pts].Y = depth_idx / cols; pts[n_pts].Z = p_depths[depth_idx]; pt_labels[n_pts] = p_labels[depth_idx]; ++n_pts; } g_DepthGenerator.ConvertProjectiveToRealWorld(n_pts, pts, pts); if (n_pts > 0) { // Create points dataset hsize_t pts_creation_dims[2] = { n_pts, 3 }; hsize_t pts_max_dims[2] = { n_pts, 3 }; DataSpace pts_mem_space(2, pts_creation_dims, pts_max_dims); DataSet pts_ds(this_frame_group.createDataSet( "points", PredType::NATIVE_FLOAT, pts_mem_space, creat_props)); hsize_t pt_labels_creation_dims[1] = { n_pts }; hsize_t pt_labels_max_dims[1] = { n_pts }; DataSpace pt_labels_mem_space(1, pt_labels_creation_dims, pt_labels_max_dims); DataSet pt_labels_ds(this_frame_group.createDataSet( "point_labels", PredType::NATIVE_UINT16, pt_labels_mem_space, creat_props)); // Write points data pts_ds.write(pts, PredType::NATIVE_FLOAT); pt_labels_ds.write(pt_labels, PredType::NATIVE_UINT16); } // Create groups to store detected users Group users_group(this_frame_group.createGroup("users")); // Dump each user in turn char strLabel[50] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { // Create a group for this user snprintf(name_str, 20, "user_%02d", aUsers[i]); Group this_user_group(users_group.createGroup(name_str)); // Create attributes for this group Attribute user_idx_attr = this_user_group.createAttribute( "idx", PredType::NATIVE_UINT16, DataSpace()); uint16_t this_user_idx(aUsers[i]); user_idx_attr.write(PredType::NATIVE_UINT16, &this_user_idx); // Write state (if any) H5std_string strwritebuf; if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { strwritebuf = "tracking"; } else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i])) { // Calibrating strwritebuf = "calibrating"; } else { // Nothing strwritebuf = "looking"; } StrType strdatatype(PredType::C_S1, strwritebuf.size()); // of length 256 characters Attribute user_state_attr = this_user_group.createAttribute( "state", strdatatype, DataSpace()); user_state_attr.write(strdatatype, strwritebuf); static Joint joints[g_NumJointTypes]; int n_joints_found = 0; // Try to dump all joints for(int jt_idx=0; jt_idx < g_NumJointTypes; ++jt_idx) { if(DumpJoint(aUsers[i], g_JointTypes[jt_idx], joints[n_joints_found])) { ++n_joints_found; } } if (n_joints_found > 0) { // Create joints dataset hsize_t joints_dim[] = { n_joints_found }; DataSpace joints_space(1, joints_dim); DataSet joints_ds(this_user_group.createDataSet("joints", joint_dt_, joints_space)); joints_ds.write(joints, joint_dt_); } } }
void SceneDrawer::depthMapCreating(unsigned char *pDestImage, const xn::DepthMetaData &dmd, const xn::SceneMetaData &smd) { unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); unsigned int nValue = 0; static unsigned int nZRes = dmd.ZRes(); static float* pDepthHist = (float*)malloc(nZRes* sizeof(float)); // Calculate the accumulative histogram memset(pDepthHist, 0, nZRes*sizeof(float)); for (int nY=0; nY<g_nYRes; nY++) for (int nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } for (int i=1; i<nZRes; i++) pDepthHist[i] += pDepthHist[i-1]; if (nNumberOfPoints) for (int i=1; i<nZRes; i++) pDepthHist[i] = (unsigned int)(256 * (1.0f - (pDepthHist[i] / nNumberOfPoints))); pDepth = dmd.Data(); // Prepare the texture map for (int nY=0; nY<g_nYRes; nY++) { for (int nX=0; nX < g_nXRes; nX++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (drawBackground || *pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) nColorID = nColors; if (nValue != 0) { pDestImage[0] = pDepthHist[nValue] * Colors[nColorID][0]; pDestImage[1] = pDepthHist[nValue] * Colors[nColorID][1]; pDestImage[2] = pDepthHist[nValue] * Colors[nColorID][2]; } } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (Width - g_nXRes) *3; } }
void SimKinect::DrawDepthMapWithUsers(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd) { static bool bInitialized = false; //image used in opencv; 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()); pDepthTexBuf = (unsigned char*)malloc(dmd.XRes()*dmd.YRes()*3*sizeof(unsigned char)); // printf("Initializing 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; } 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 nXRes = dmd.XRes(); XnUInt16 nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); int* p_depth_map = depth_map; //Calculate the accumulative histogram memset(pDepthHist, 0, MAX_DEPTH*sizeof(float)); for (nY=0; nY<nYRes; nY++) { for (nX=0; nX<nXRes; nX++) { nValue = *pDepth; *p_depth_map++ = nValue; if (nValue != 0) { pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { pDepthHist[nIndex] += pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); if (bDrawPixels) { // Prepare the texture map for (nY=0; nY<nYRes; nY++) { for (nX=0; nX < nXRes; nX++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (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; } } } else { xnOSMemSet(pDepthTexBuf, 0, 3*nXRes*nYRes); } //Draw depth image unsigned char* pix_ptr = pDepthTexBuf; for ( int i=0 ; i<video_size_height ; i++ ) { unsigned char* row_pointer = depth_frame+i*video_size_width*3; for ( int j=0 ; j<video_size_width*3; j+=3 ) { row_pointer[j] = *pix_ptr++; row_pointer[j+1]=*pix_ptr++; row_pointer[j+2]=*pix_ptr++; } } }
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 } } }
void Gestures::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( 1280 ); 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(m_texcoords, 0, 8*sizeof(float)); m_texcoords[0] = texXpos, m_texcoords[1] = texYpos, m_texcoords[2] = texXpos, m_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(m_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) { m_pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { m_pDepthHist[nIndex] += m_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { m_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (m_pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); if (m_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 (m_bDrawBackground || *pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % m_nColors; if (label == 0) { nColorID = m_nColors; } if (nValue != 0) { nHistValue = m_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); }