KinectDisplay.cpp
Go to the documentation of this file.
00001 #include <GL/glut.h>
00002 
00003 #include "KinectDisplay.h"
00004 
00005 XnBool g_bDrawBackground = TRUE;
00006 XnBool g_bDrawPixels = TRUE;
00007 XnBool g_bDrawSkeleton = TRUE;
00008 XnBool g_bPrintID = TRUE;
00009 XnBool g_bPrintState = TRUE;
00010 
00011 #define MAX_DEPTH 10000
00012 float g_pDepthHist[MAX_DEPTH];
00013 unsigned int getClosestPowerOfTwo(unsigned int n)
00014 {
00015         unsigned int m = 2;
00016         while(m < n) m<<=1;
00017 
00018         return m;
00019 }
00020 GLuint initTexture(void** buf, int& width, int& height)
00021 {
00022         GLuint texID = 0;
00023         glGenTextures(1,&texID);
00024 
00025         width = getClosestPowerOfTwo(width);
00026         height = getClosestPowerOfTwo(height); 
00027         *buf = new unsigned char[width*height*4];
00028         glBindTexture(GL_TEXTURE_2D,texID);
00029 
00030         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
00031         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00032 
00033         return texID;
00034 }
00035 
00036 GLfloat texcoords[8];
00037 void DrawRectangle(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
00038 {
00039         GLfloat verts[8] = {    topLeftX, topLeftY,
00040                 topLeftX, bottomRightY,
00041                 bottomRightX, bottomRightY,
00042                 bottomRightX, topLeftY
00043         };
00044         glVertexPointer(2, GL_FLOAT, 0, verts);
00045         glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
00046 
00047         //TODO: Maybe glFinish needed here instead - if there's some bad graphics crap
00048         glFlush();
00049 }
00050 void DrawTexture(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
00051 {
00052         glEnableClientState(GL_TEXTURE_COORD_ARRAY);
00053         glTexCoordPointer(2, GL_FLOAT, 0, texcoords);
00054 
00055         DrawRectangle(topLeftX, topLeftY, bottomRightX, bottomRightY);
00056 
00057         glDisableClientState(GL_TEXTURE_COORD_ARRAY);
00058 }
00059 
00060 XnFloat Colors[][3] =
00061 {
00062         {0,1,1},
00063         {0,0,1},
00064         {0,1,0},
00065         {1,1,0},
00066         {1,0,0},
00067         {1,.5,0},
00068         {.5,1,0},
00069         {0,.5,1},
00070         {.5,0,1},
00071         {1,1,.5},
00072         {1,1,1}
00073 };
00074 XnUInt32 nColors = 10;
00075 
00076 void glPrintString(void *font, char *str)
00077 {
00078         int i,l = strlen(str);
00079 
00080         for(i=0; i<l; i++)
00081         {
00082                 glutBitmapCharacter(font,*str++);
00083         }
00084 }
00085 
00086 
00087 void kinect_display_drawDepthMapGL(const xn::DepthMetaData& dmd,
00088                                    const xn::SceneMetaData& smd)
00089 {
00090         static bool bInitialized = false;       
00091         static GLuint depthTexID;
00092         static unsigned char* pDepthTexBuf;
00093         static int texWidth, texHeight;
00094 
00095   float topLeftX;
00096   float topLeftY;
00097   float bottomRightY;
00098   float bottomRightX;
00099         float texXpos;
00100         float texYpos;
00101 
00102         if(!bInitialized)
00103         {
00104 
00105                 texWidth =  getClosestPowerOfTwo(dmd.XRes());
00106                 texHeight = getClosestPowerOfTwo(dmd.YRes());
00107 
00108                 printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
00109                 depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
00110 
00111                 printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
00112                 bInitialized = true;
00113 
00114                 topLeftX = dmd.XRes();
00115                 topLeftY = 0;
00116                 bottomRightY = dmd.YRes();
00117                 bottomRightX = 0;
00118                 texXpos =(float)dmd.XRes()/texWidth;
00119                 texYpos  =(float)dmd.YRes()/texHeight;
00120 
00121                 memset(texcoords, 0, 8*sizeof(float));
00122                 texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
00123 
00124         }
00125         unsigned int nValue = 0;
00126         unsigned int nHistValue = 0;
00127         unsigned int nIndex = 0;
00128         unsigned int nX = 0;
00129         unsigned int nY = 0;
00130         unsigned int nNumberOfPoints = 0;
00131         XnUInt16 g_nXRes = dmd.XRes();
00132         XnUInt16 g_nYRes = dmd.YRes();
00133 
00134         unsigned char* pDestImage = pDepthTexBuf;
00135 
00136         const XnDepthPixel* pDepth = dmd.Data();
00137         const XnLabel* pLabels = smd.Data();
00138 
00139         // Calculate the accumulative histogram
00140         memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
00141         for (nY=0; nY<g_nYRes; nY++)
00142         {
00143                 for (nX=0; nX<g_nXRes; nX++)
00144                 {
00145                         nValue = *pDepth;
00146 
00147                         if (nValue != 0)
00148                         {
00149                                 g_pDepthHist[nValue]++;
00150                                 nNumberOfPoints++;
00151                         }
00152 
00153                         pDepth++;
00154                 }
00155         }
00156 
00157         for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
00158         {
00159                 g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
00160         }
00161         if (nNumberOfPoints)
00162         {
00163                 for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
00164                 {
00165                         g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
00166                 }
00167         }
00168 
00169         pDepth = dmd.Data();
00170         if (g_bDrawPixels)
00171         {
00172                 XnUInt32 nIndex = 0;
00173                 // Prepare the texture map
00174                 for (nY=0; nY<g_nYRes; nY++)
00175                 {
00176                         for (nX=0; nX < g_nXRes; nX++, nIndex++)
00177                         {
00178 
00179                                 pDestImage[0] = 0;
00180                                 pDestImage[1] = 0;
00181                                 pDestImage[2] = 0;
00182                                 if (g_bDrawBackground || *pLabels != 0)
00183                                 {
00184                                         nValue = *pDepth;
00185                                         XnLabel label = *pLabels;
00186                                         XnUInt32 nColorID = label % nColors;
00187                                         if (label == 0)
00188                                         {
00189                                                 nColorID = nColors;
00190                                         }
00191 
00192                                         if (nValue != 0)
00193                                         {
00194                                                 nHistValue = g_pDepthHist[nValue];
00195 
00196                                                 pDestImage[0] = nHistValue * Colors[nColorID][0]; 
00197                                                 pDestImage[1] = nHistValue * Colors[nColorID][1];
00198                                                 pDestImage[2] = nHistValue * Colors[nColorID][2];
00199                                         }
00200                                 }
00201 
00202                                 pDepth++;
00203                                 pLabels++;
00204                                 pDestImage+=3;
00205                         }
00206 
00207                         pDestImage += (texWidth - g_nXRes) *3;
00208                 }
00209         }
00210         else
00211         {
00212                 xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
00213         }
00214 
00215         glBindTexture(GL_TEXTURE_2D, depthTexID);
00216         glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
00217 
00218         // Display the OpenGL texture map
00219         glColor4f(0.75,0.75,0.75,1);
00220 
00221         glEnable(GL_TEXTURE_2D);
00222         DrawTexture(dmd.XRes(),dmd.YRes(),0,0); 
00223         glDisable(GL_TEXTURE_2D);
00224 }
00225 
00226 
00227 void DrawLimb(xn::UserGenerator& userGenerator,
00228               xn::DepthGenerator& depthGenerator,
00229               XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
00230 {
00231         if (!userGenerator.GetSkeletonCap().IsTracking(player))
00232         {
00233                 printf("not tracked!\n");
00234                 return;
00235         }
00236 
00237         XnSkeletonJointPosition joint1, joint2;
00238         userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
00239         userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
00240 
00241         if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
00242         {
00243                 return;
00244         }
00245 
00246         XnPoint3D pt[2];
00247         pt[0] = joint1.position;
00248         pt[1] = joint2.position;
00249 
00250         depthGenerator.ConvertRealWorldToProjective(2, pt, pt);
00251         glVertex3i(pt[0].X, pt[0].Y, 0);
00252         glVertex3i(pt[1].X, pt[1].Y, 0);
00253 }
00254 
00255 void kinect_display_drawSkeletonGL(xn::UserGenerator& userGenerator,
00256                                   xn::DepthGenerator& depthGenerator)
00257 {
00258         char strLabel[50] = "";
00259         XnUserID aUsers[15];
00260         XnUInt16 nUsers = 15;
00261         userGenerator.GetUsers(aUsers, nUsers);
00262         for (int i = 0; i < nUsers; ++i)
00263         {
00264                 if (g_bPrintID)
00265                 {
00266                         XnPoint3D com;
00267                         userGenerator.GetCoM(aUsers[i], com);
00268                         depthGenerator.ConvertRealWorldToProjective(1, &com, &com);
00269 
00270                         xnOSMemSet(strLabel, 0, sizeof(strLabel));
00271                         if (!g_bPrintState)
00272                         {
00273                                 // Tracking
00274                                 sprintf(strLabel, "%d", aUsers[i]);
00275                         }
00276                         else if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
00277                         {
00278                                 // Tracking
00279                                 sprintf(strLabel, "%d - Tracking", aUsers[i]);
00280                         }
00281                         else if (userGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
00282                         {
00283                                 // Calibrating
00284                                 sprintf(strLabel, "%d - Calibrating...", aUsers[i]);
00285                         }
00286                         else
00287                         {
00288                                 // Nothing
00289                                 sprintf(strLabel, "%d - Looking for pose", aUsers[i]);
00290                         }
00291 
00292 
00293                         glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
00294 
00295                         glRasterPos2i(com.X, com.Y);
00296                         glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
00297                 }
00298 
00299                 if (g_bDrawSkeleton && userGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
00300                 {
00301                         glBegin(GL_LINES);
00302                         glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
00303                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
00304 
00305                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
00306                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
00307                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
00308 
00309                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
00310                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
00311                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
00312 
00313                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
00314                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
00315 
00316                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
00317                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
00318                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
00319 
00320                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
00321                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
00322                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
00323 
00324                         DrawLimb(userGenerator, depthGenerator, aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
00325 
00326                         glEnd();
00327                 }
00328         }
00329 }


pi_tracker
Author(s): Patrick Goebel
autogenerated on Tue Jan 7 2014 11:27:49