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
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
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
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
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
00274 sprintf(strLabel, "%d", aUsers[i]);
00275 }
00276 else if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
00277 {
00278
00279 sprintf(strLabel, "%d - Tracking", aUsers[i]);
00280 }
00281 else if (userGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
00282 {
00283
00284 sprintf(strLabel, "%d - Calibrating...", aUsers[i]);
00285 }
00286 else
00287 {
00288
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 }