bool DataCapture::captureOne() { XnStatus rc = context_.WaitAndUpdateAll(); // want this to be WaitOneUpdateAll(RGB image) if( rc != XN_STATUS_OK ) { std::cout << "WaitAndUpdateAll: " << xnGetStatusString(rc) << std::endl; return false; } // grab image imageGen_.GetMetaData(imageMd_); const XnRGB24Pixel* rgbData = imageMd_.RGB24Data(); for( unsigned int i = 0; i < 640 * 480; ++i ) { pRgbData_[3*i] = rgbData->nRed; pRgbData_[3*i + 1] = rgbData->nGreen; pRgbData_[3*i + 2] = rgbData->nBlue; ++rgbData; } // grab depth image depthGen_.GetMetaData(depthMd_); const uint16_t* pDepthDataU16 = depthMd_.Data(); for( int i = 0; i < 640 * 480; ++i) { uint16_t d = pDepthDataU16[i]; if( d != 0 ) { pDepthData_[i] = (d * 255)/2048; } else { pDepthData_[i] = 0; // should be NAN } } return true; }
void draw() { glClear( GL_COLOR_BUFFER_BIT ); XnMapOutputMode mode; g_DepthGenerator.GetMapOutputMode(mode); Matrix4f projectionMatrix = Matrix4f(); projectionMatrix.Ortho(0, mode.nXRes, mode.nYRes, 0, -1.0, 1.0); Vector primaryPoint; primaryPoint = pointHandler->getPrimaryPoint(); Vector movement = primaryPoint - position; movement.normalize(); position += movement; position.print(); Matrix4f modelViewMatrix; modelViewMatrix.translate(position.getX(), position.getY(), 0); Matrix4f modelViewProjectionMatrix; modelViewProjectionMatrix = projectionMatrix * modelViewMatrix; //glUniformMatrix4fv(matLoc, 1, GL_FALSE, modelViewProjectionMatrix); // Use our shader glUseProgram(programObject); modelViewProjectionMatrix.uniformMatrix(matLoc); // Draw the triangle glDrawArrays(GL_LINE_LOOP, 0, 3); g_Context.WaitOneUpdateAll(g_DepthGenerator); // Update NITE tree g_pSessionManager->Update(&g_Context); PrintSessionState(g_SessionState); SDL_GL_SwapBuffers(); }
void DrawCircle(xn::UserGenerator& userGenerator, xn::DepthGenerator& depthGenerator, XnUserID player, XnSkeletonJoint eJoint, float radius, XnFloat *color3f) { XnSkeletonJointPosition joint; userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint); if (joint.fConfidence < 0.5){ return; } XnPoint3D pt; pt = joint.position; depthGenerator.ConvertRealWorldToProjective(1, &pt, &pt); float cx = pt.X; float cy = pt.Y; float r = radius; int num_segments = 16; glColor3f(color3f[0], color3f[1], color3f[2]); glBegin(GL_TRIANGLE_FAN); glVertex2f(cx , cy); for(int i = 0; i <= num_segments; i++) { float theta = 2.0f * 3.1415926f * float(i) / float(num_segments);//get the current angle float x = r * cosf(theta);//calculate the x component float y = r * sinf(theta);//calculate the y component glVertex2f(x +cx , y + cy);//output vertex } glEnd(); }
XnPoint3D getJointPoint(XnUserID player, XnSkeletonJoint eJoint) { XnPoint3D pt; if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player)) { printf("not calibrated!\n"); return pt; } if (!g_UserGenerator.GetSkeletonCap().IsTracking(player)) { printf("not tracked!\n"); return pt; } XnSkeletonJointPosition joint; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint); if (joint.fConfidence < 0.5) { return pt; } g_DepthGenerator.ConvertRealWorldToProjective(2, &joint.position, &pt); return pt; }
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(); } } }
cv::Mat xncv::captureDepth(const xn::DepthGenerator& generator) { xn::DepthMetaData meta; generator.GetMetaData(meta); return cv::Mat(meta.YRes(), meta.XRes(), cv::DataType<ushort>::type, (void*)meta.Data()); }
int main ( int argc, char ** argv ) { // // Initializing Calibration Related // // ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_FILE, ARTAG_POS_FILE ); ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_A3_FILE, ARTAG_POS_A3_FILE ); ExtrCalibrator extrCalibrator ( 6, KINECT_INTR_FILE, KINECT_DIST_FILE ); // unsigned char * kinectImgBuf = new unsigned char[colorImgWidth * colorImgHeight * 3]; // // Initializing OpenNI Settings // int ctlWndKey = -1; XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; // // Initialize Context Object // nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, g_ScriptNode, &errors ); if ( nRetVal == XN_STATUS_NO_NODE_PRESENT ) { XnChar strError[1024]; errors.ToString ( strError, 1024 ); printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError ); system ( "pause" ); return ( nRetVal ); } else if ( nRetVal != XN_STATUS_OK ) { printf ( "Open FAILED:\n%s\n", xnGetStatusString ( nRetVal ) ); system ( "pause" ); return ( nRetVal ); } // // Handle the Depth Generator Node. // nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen ); if ( nRetVal != XN_STATUS_OK ) { printf ( "No Depth Node Exists! Please Check your XML.\n" ); return ( nRetVal ); } // // Handle the Image Generator node // nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen ); if ( nRetVal != XN_STATUS_OK ) { printf ( "No Image Node Exists! Please Check your XML.\n" ); return ( nRetVal ); } // g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen ); g_DepthGen.GetMetaData ( g_DepthMD ); g_ImageGen.GetMetaData ( g_ImageMD ); assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 ); assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT ); // // Create OpenCV Showing Window and Related Data Structures // cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE ); cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE ); cv::Mat depthImgMat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 ); cv::Mat depthImgShow ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC3 ); cv::Mat colorImgMat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 ); #define ARTAG_DEBUG #ifdef ARTAG_DEBUG cv::setMouseCallback ( IMAGE_WIN_NAME, ClickOnMouse, 0 ); #endif bool flipColor = true; // // Start to Loop // while ( ctlWndKey != ESC_KEY_VALUE ) { // // Try to Get New Frame From Kinect // nRetVal = g_Context.WaitAnyUpdateAll (); g_DepthGen.GetMetaData ( g_DepthMD ); g_ImageGen.GetMetaData ( g_ImageMD ); assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() && g_DepthMD.FullYRes() == g_DepthMD.YRes() ); assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() && g_ImageMD.FullYRes() == g_ImageMD.YRes() ); GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), colorImgMat ); #ifdef SHOW_DEPTH_WINDOW GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), depthImgMat ); // GlobalUtility::ConvertDepthCvMat16uToYellowCvMat ( depthImgMat, depthImgShow ); GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( depthImgMat, depthImgShow ); cv::imshow ( DEPTH_WIN_NAME, depthImgShow ); #endif ctlWndKey = cvWaitKey ( 15 ); if ( ctlWndKey == 'f' || ctlWndKey == 'F' ) { artagHelper.Clear(); artagHelper.FindMarkerCorners ( (unsigned char *)(g_ImageMD.Data()) ); artagHelper.PrintMarkerCornersPos2dInCam (); extrCalibrator.ExtrCalib ( artagHelper ); std::cout << "\nKinect Extr Matrix:" << std::endl; extrCalibrator.PrintMatrix ( extrCalibrator.GetMatrix ( ExtrCalibrator::EXTR ) ); std::cout << "Reprojection ERROR = " << extrCalibrator.ComputeReprojectionErr ( artagHelper ) << std::endl // << extrCalibrator.ComputeReprojectionErr ( artagHelper.m_MarkerCornerPosCam2d, artagHelper.m_MarkerCornerPos3d, 24 ) << std::endl << "Valid Marker Number = " << artagHelper.GetValidMarkerNumber() << std::endl << std::endl; extrCalibrator.SaveMatrix ( ExtrCalibrator::EXTR, KINECT_EXTR_FILE ); } if ( ctlWndKey == 's' || ctlWndKey == 'S' ) { flipColor = !flipColor; } if ( flipColor ) { cv::cvtColor ( colorImgMat, colorImgMat, CV_RGB2BGR ); } artagHelper.DrawMarkersInCameraImage ( colorImgMat ); cv::imshow ( IMAGE_WIN_NAME, colorImgMat ); } g_Context.Release (); system ( "pause" ); exit ( EXIT_SUCCESS ); }
void KinectDataPub::spin(bool async) { static bool onceThrough = false; if (!onceThrough){ XnStatus nRetVal = XN_STATUS_OK; nRetVal = context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); onceThrough = true; printf("Starting to run\n"); if (needPose){ printf("Assume calibration pose\n"); } } XnUserID aUsers[MAX_NUM_USERS]; XnUInt16 nUsers; SkeletonInfo skels[MAX_NUM_USERS]; SensorMsg msg; xn::DepthMetaData depthMD; xn::SceneMetaData sceneMD; while(!xnOSWasKeyboardHit()){ context.WaitOneUpdateAll(userGen); /** * nUsers is used both as input and output parameters. * When it's used as input parameter, it indicates the size of the user ID * array, while, when it's used as output parameter, it means the number * of users recognized. Therefore, it needs to be re-initialized in a loop. */ nUsers = MAX_NUM_USERS; userGen.GetUsers(aUsers, nUsers); memset(&msg, 0, sizeof(SensorMsg)); for (XnUInt16 i = 0; i < nUsers; ++i){ if (userGen.GetSkeletonCap().IsTracking(aUsers[i])){ // get the user's skeleton skels[i].user = aUsers[i]; getSkeleton(skels[i]); } else skels[i].user = 0; // user is not tracked } depthGen.GetMetaData(depthMD); // depth map userGen.GetUserPixels(0, sceneMD); // labels msg.nUsers = nUsers; // num of users detected, either tracked or not msg.pSkels = skels; msg.pDepthMD = &depthMD; msg.pSceneMD = &sceneMD; for (int i = 0; i < subscribers.size(); ++i) subscribers[i]->notify(RAD_SENSOR_MSG, &msg); if (async) return; } }
/* The matlab mex function */ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { XnUInt64 *MXadress; double *Pos; int Jdimsc[2]; Jdimsc[0]=225; Jdimsc[1]=7; plhs[0] = mxCreateNumericArray(2, Jdimsc, mxDOUBLE_CLASS, mxREAL); Pos = mxGetPr(plhs[0]); if(nrhs==0) { printf("Open failed: Give Pointer to Kinect as input\n"); mexErrMsgTxt("Kinect Error"); } MXadress = (XnUInt64*)mxGetData(prhs[0]); if(MXadress[0]>0){ g_Context = ((xn::Context*) MXadress[0])[0]; } if(MXadress[2]>0) { g_DepthGenerator = ((xn::DepthGenerator*) MXadress[2])[0]; } else { mexErrMsgTxt("No Depth Node in Kinect Context"); } if(MXadress[4]>0) { g_UserGenerator = ((xn::UserGenerator*) MXadress[4])[0]; } else { mexErrMsgTxt("No User Node in Kinect Context"); } XnStatus nRetVal = XN_STATUS_OK; XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return; } g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return; } g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); char strLabel[50] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; int r=0; xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; // Process the data g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { //printf(strLabel, "%d - Looking for pose", aUsers[i]); XnSkeletonJointPosition joint[15]; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_HEAD, joint[0]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_NECK, joint[1]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_SHOULDER, joint[2]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_ELBOW, joint[3]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HAND, joint[4]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_SHOULDER, joint[5]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_ELBOW, joint[6]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HAND, joint[7]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_TORSO, joint[8]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HIP, joint[9]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_KNEE, joint[10]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_FOOT, joint[11]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HIP, joint[12]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_KNEE, joint[13]); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_FOOT, joint[14]); XnPoint3D pt[1]; for(int j=0; j<15; j++) { Pos[j +r]=aUsers[i]; Pos[j+Jdimsc[0] +r]=joint[j].fConfidence; Pos[j+Jdimsc[0]*2+r]=joint[j].position.X; Pos[j+Jdimsc[0]*3+r]=joint[j].position.Y; Pos[j+Jdimsc[0]*4+r]=joint[j].position.Z; pt[0] = joint[j].position; g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt); Pos[j+Jdimsc[0]*5+r]=pt[0].X; Pos[j+Jdimsc[0]*6+r]=pt[0].Y; } r+=15; } } }
void matrixCalc(void *outputs) { TML::Matrix out1(outputs, 0); TML::Matrix out2(outputs, 1); TML::Matrix out3(outputs, 2); TML::Matrix out4(outputs, 3); xn::DepthMetaData depthMD; xn::SceneMetaData sceneMD; xn::ImageMetaData imageMD; depth.GetMetaData(depthMD); user.GetUserPixels(0, sceneMD); image.GetMetaData(imageMD); context.WaitNoneUpdateAll(); t_jit_matrix_info tmi; memset(&tmi, 0, sizeof(tmi)); tmi.dimcount = 2; tmi.planecount = 1; tmi.dimstride[0] = 4; tmi.dimstride[1] = depthMD.XRes()*4; int width = tmi.dim[0] = depthMD.XRes(); int height = tmi.dim[1] = depthMD.YRes(); tmi.type = _jit_sym_float32; out1.resizeTo(&tmi); tmi.planecount = 1; tmi.dimstride[0] = 1; tmi.dimstride[1] = depthMD.XRes(); tmi.type = _jit_sym_char; out2.resizeTo(&tmi); tmi.planecount = 4; tmi.dimstride[0] = 4; tmi.dimstride[1] = depthMD.XRes()*4; tmi.type = _jit_sym_char; out3.resizeTo(&tmi); const XnDepthPixel* pDepth = depthMD.Data(); float *depthData = (float*)out1.data(); //Copy depth data int x,y; for (y=0; y<height; y++) { for (x=0; x<width; x++) { depthData[0] = (float)pDepth[0]/powf(2, 15); depthData++; pDepth++; } } //Get the users unsigned char *userData = (unsigned char*)out2.data(); const XnLabel* pLabels = sceneMD.Data(); for (y=0; y<height; y++) { for (x=0; x<width; x++) { userData[0] = pLabels[0]; userData++; pLabels++; } } //Get the colors const XnRGB24Pixel* pPixels = imageMD.RGB24Data(); unsigned char *pixData = (unsigned char*)out3.data(); for (y=0; y<height; y++) { for (x=0; x<width; x++) { pixData[0] = 0; pixData[1] = pPixels[0].nRed; pixData[2] = pPixels[0].nGreen; pixData[3] = pPixels[0].nBlue; pixData+=4; pPixels++; } } //For all the users -- output the joint info... XnUserID aUsers[15]; XnUInt16 nUsers = 15; user.GetUsers(aUsers, nUsers); int rUsers = 0; xn::SkeletonCapability sc = user.GetSkeletonCap(); int i; for (i=0; i<nUsers; i++) { if (user.GetSkeletonCap().IsTracking(aUsers[i])) rUsers++; } tmi.dimcount = 2; tmi.planecount = 3; tmi.dimstride[0] = 3*4; tmi.dimstride[1] = 24*3*4; tmi.dim[0] = 24; tmi.dim[1] = rUsers; tmi.type = _jit_sym_float32; out4.resizeTo(&tmi); float *sData = (float*)out4.data(); if (rUsers == 0) { int n; for (n=0; n<24; n++) { sData[0] = 0; sData[1] = 0; sData[2] = 0; sData+=3; } } else { for (i=0; i<nUsers; i++) { if (user.GetSkeletonCap().IsTracking(aUsers[i])) { int n; for (n=0; n<24; n++) { XnSkeletonJointPosition jp; user.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], (XnSkeletonJoint)(n+1), jp); sData[0] = (1280 - jp.position.X) / 2560; sData[1] = (1280 - jp.position.Y) / 2560; sData[2] = jp.position.Z * 7.8125 / 10000; // if (n == 0) // { // post("%f %f %f\n", sData[0], sData[1], sData[2]); // } sData+=3; } } } } //post("%i\n", rUsers); }
void start_kinect() { XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; UsersCount = 0; const char *fn = NULL; if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH; else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL; else { printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL); //return XN_STATUS_ERROR; } printf("Reading config from: '%s'\n", fn); nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); //return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); //return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth); CHECK_RC(nRetVal,"No depth"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); CHECK_RC(nRetVal,"No image"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); //return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); //return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); XnUserID aUsers[MAX_NUM_USERS]; XnUInt16 nUsers; XnSkeletonJointTransformation anyjoint; printf("Starting to run\n"); if(g_bNeedPose) { printf("Assume calibration pose\n"); } XnUInt32 epochTime = 0; while (!xnOSWasKeyboardHit()) { g_Context.WaitOneUpdateAll(g_UserGenerator); // print the torso information for the first user already tracking nUsers=MAX_NUM_USERS; g_UserGenerator.GetUsers(aUsers, nUsers); int numTracked=0; int userToPrint=-1; WriteLock w_lock(myLock); pDepthMap = g_depth.GetDepthMap(); pPixelMap = g_image.GetRGB24ImageMap(); g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); pPixelPoint = g_imageMD.RGB24Data(); for(XnUInt16 i=0; i<nUsers; i++) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE) continue; { /* Writing all new movements into structure*/ /* Head */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_HEAD,anyjoint); Skeletons[i]["Head"]["X"] = anyjoint.position.position.X; Skeletons[i]["Head"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["Head"]["Z"] = anyjoint.position.position.Z; /* Neck */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_NECK,anyjoint); Skeletons[i]["Neck"]["X"] = anyjoint.position.position.X; Skeletons[i]["Neck"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["Neck"]["Z"] = anyjoint.position.position.Z; /* Left Shoulder */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_SHOULDER,anyjoint); Skeletons[i]["LeftShoulder"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftShoulder"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftShoulder"]["Z"] = anyjoint.position.position.Z; /* Right Shoulder */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_SHOULDER,anyjoint); Skeletons[i]["RightShoulder"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightShoulder"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightShoulder"]["Z"] = anyjoint.position.position.Z; /* Torso */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,anyjoint); Skeletons[i]["Torso"]["X"] = anyjoint.position.position.X; Skeletons[i]["Torso"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["Torso"]["Z"] = anyjoint.position.position.Z; /* Left Elbow */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_ELBOW,anyjoint); Skeletons[i]["LeftElbow"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftElbow"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftElbow"]["Z"] = anyjoint.position.position.Z; /* Right Elbow */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_ELBOW,anyjoint); Skeletons[i]["RightElbow"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightElbow"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightElbow"]["Z"] = anyjoint.position.position.Z; /* Left Hip */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HIP,anyjoint); Skeletons[i]["LeftHip"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftHip"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftHip"]["Z"] = anyjoint.position.position.Z; /* Right Hip */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HIP,anyjoint); Skeletons[i]["RightHip"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightHip"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightHip"]["Z"] = anyjoint.position.position.Z; /* Left Hand */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HAND,anyjoint); Skeletons[i]["LeftHand"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftHand"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftHand"]["Z"] = anyjoint.position.position.Z; /* Right Hand */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HAND,anyjoint); Skeletons[i]["RightHand"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightHand"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightHand"]["Z"] = anyjoint.position.position.Z; /* Left Knee */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_KNEE,anyjoint); Skeletons[i]["LeftKnee"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftKnee"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftKnee"]["Z"] = anyjoint.position.position.Z; /* Right Knee */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_KNEE,anyjoint); Skeletons[i]["RightKnee"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightKnee"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightKnee"]["Z"] = anyjoint.position.position.Z; /* Left Foot */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_FOOT,anyjoint); Skeletons[i]["LeftFoot"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftFoot"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftFoot"]["Z"] = anyjoint.position.position.Z; /* Right Foot */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_FOOT,anyjoint); Skeletons[i]["RightFoot"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightFoot"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightFoot"]["Z"] = anyjoint.position.position.Z; /*printf("user %d: head at (%6.2f,%6.2f,%6.2f)\n",aUsers[i], Skeletons[i]["Head"]["X"], Skeletons[i]["Head"]["Y"], Skeletons[i]["Head"]["Z"]);*/ } } } g_scriptNode.Release(); g_depth.Release(); g_image.Release(); g_UserGenerator.Release(); g_Context.Release(); }
bool initialize(){ std::cout << "initializing kinect... " << endl; #ifdef USE_MS_SKD int iSensorCount = 0; if ( NuiGetSensorCount(&iSensorCount) < 0 ) return false; // Look at each Kinect sensor for (int i = 0; i < iSensorCount; ++i) { // Create the sensor so we can check status, if we can't create it, move on to the next if ( NuiCreateSensorByIndex(i, &g_pNuiSensor) < 0 ) continue; // Get the status of the sensor, and if connected, then we can initialize it if( 0== g_pNuiSensor->NuiStatus() ){ g_pNuiSensor = g_pNuiSensor; break; } // This sensor wasn't OK, so release it since we're not using it g_pNuiSensor->Release(); } if (NULL != g_pNuiSensor) { // Initialize the Kinect and specify that we'll be using depth if ( g_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH) >= 0 ) { // Create an event that will be signaled when depth data is available g_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL); // Open a depth image stream to receive depth frames g_pNuiSensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH, NUI_IMAGE_RESOLUTION_640x480, 0, 2, g_hNextDepthFrameEvent, &g_pDepthStreamHandle); } } else return false; #endif #ifdef USE_OPENNI // Initialize context object g_RetVal = g_Context.Init(); g_RetVal = g_DepthGenerator.Create(g_Context); if (g_RetVal != XN_STATUS_OK) printf("Failed creating DEPTH generator %s\n", xnGetStatusString(g_RetVal)); XnMapOutputMode outputMode; outputMode.nXRes = g_im_w; outputMode.nYRes = g_im_h; outputMode.nFPS = g_fps; g_RetVal = g_DepthGenerator.SetMapOutputMode(outputMode); if (g_RetVal != XN_STATUS_OK){ printf("Failed setting the DEPTH output mode %s\n", xnGetStatusString(g_RetVal)); return false; } g_RetVal = g_Context.StartGeneratingAll(); if (g_RetVal != XN_STATUS_OK){ printf("Failed starting generating all %s\n", xnGetStatusString(g_RetVal)); return false; } #endif #ifdef USE_LIBFREENECT depth_mid = (uint16_t*)malloc(640*480*2); depth_front = (uint16_t*)malloc(640*480*2); if (freenect_init(&f_ctx, NULL) < 0) { printf("freenect_init() failed\n"); return 1; } freenect_set_log_level(f_ctx, FREENECT_LOG_ERROR); freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); int nr_devices = freenect_num_devices (f_ctx); printf ("Number of devices found: %d\n", nr_devices); int user_device_number = 0; if (nr_devices < 1) { freenect_shutdown(f_ctx); return false; } if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) { printf("Could not open device\n"); freenect_shutdown(f_ctx); return false; } bool res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL); if (res) { printf("pthread_create failed\n"); freenect_shutdown(f_ctx); return false; } freenect_raw_tilt_state state; freenect_get_tilt_status(&state); freenect_angle = freenect_get_tilt_degs(&state); #endif g_im3D.create(g_im_h,g_im_w,CV_32FC3); g_imD.create(g_im_h,g_im_w,CV_16UC1); return true; }
int main ( int argc, char * argv[] ) { // // Initialize OpenNI Settings // XnStatus nRetVal = XN_STATUS_OK; xn::ScriptNode scriptNode; xn::EnumerationErrors errors; // // Initialize Context Object // nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, scriptNode, &errors ); if ( nRetVal == XN_STATUS_NO_NODE_PRESENT ) { XnChar strError[1024]; errors.ToString(strError, 1024); printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError ); system ( "pause" ); return ( nRetVal ); } else if ( nRetVal != XN_STATUS_OK ) { printf ( "Open failed: %s\n", xnGetStatusString(nRetVal) ); system ( "pause" ); return ( nRetVal ); } // // Handle Image & Depth Generator Node // bool colorFlag = true; bool depthFlag = true; nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen ); if ( nRetVal != XN_STATUS_OK ) { printf("No depth node exists!\n"); depthFlag = false; } nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen ); if ( nRetVal != XN_STATUS_OK ) { printf("No image node exists!\n"); colorFlag = false; } // g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen ); if ( depthFlag ) { g_DepthGen.GetMetaData ( g_DepthMD ); assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT ); } if ( colorFlag ) { g_ImageGen.GetMetaData ( g_ImageMD ); assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 ); } g_DepthImgShow = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC1 ); g_DepthImgMat = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 ); g_ColorImgMat = cv::Mat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 ); // // Start to Loop // bool flipColor = true; int ctlWndKey = -1; g_StartTickCount = GetTickCount(); g_HeadTrackingFrameCount = 0; while ( ctlWndKey != ESC_KEY_VALUE ) { nRetVal = g_Context.WaitOneUpdateAll ( g_DepthGen ); // nRetVal = g_Context.WaitAnyUpdateAll(); #ifdef HANDLING_IMAGE_DATA if ( colorFlag ) { g_ImageGen.GetMetaData ( g_ImageMD ); assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() ); assert ( g_ImageMD.FullYRes() == g_ImageMD.YRes() ); GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), g_ColorImgMat ); if ( ctlWndKey == 's' || ctlWndKey == 'S' ) { // Switch flipColor = !flipColor; } if ( flipColor ) { cv::cvtColor ( g_ColorImgMat, g_ColorImgMat, CV_RGB2BGR ); } cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE ); cv::imshow ( IMAGE_WIN_NAME, g_ColorImgMat ); } #endif #ifdef HANDLING_DEPTH_DATA if ( depthFlag ) { g_DepthGen.GetMetaData(g_DepthMD); // assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() ); // assert ( g_DepthMD.FullYRes() == g_DepthMD.YRes() ); GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), g_DepthImgMat ); GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( g_DepthImgMat, g_DepthImgShow ); /* cv::putText(colorImgMat, GlobalUtility::DoubleToString(g_ImageMD.FPS()) + " FPS", cv::Point(10, 450), cv::FONT_ITALIC, 0.7, cv::Scalar(255, 255, 255, 0), 2, 8, false); */ cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE ); cv::imshow ( DEPTH_WIN_NAME, g_DepthImgShow ); } #endif XnFieldOfView fov; g_DepthGen.GetFieldOfView( fov ); std::cout << "HFov = " << fov.fHFOV << std::endl << "VFov = " << fov.fVFOV << std::endl; ctlWndKey = cvWaitKey ( 5 ); g_HeadTrackingFrameCount++; g_CurrTickCount = GetTickCount(); std::cout << "FPS = " << 1000 / ( ( double )( g_CurrTickCount - g_StartTickCount ) / ( double )( g_HeadTrackingFrameCount ) ) << std::endl; } g_Context.Release (); exit ( EXIT_SUCCESS ); }
int main() { const unsigned int nBackgroundTrain = 30; const unsigned short touchDepthMin = 10; const unsigned short touchDepthMax = 20; const unsigned int touchMinArea = 50; const bool localClientMode = true; // connect to a local client const double debugFrameMaxDepth = 4000; // maximal distance (in millimeters) for 8 bit debug depth frame quantization const char* windowName = "Debug"; const Scalar debugColor0(0,0,128); const Scalar debugColor1(255,0,0); const Scalar debugColor2(255,255,255); int xMin = 110; int xMax = 560; int yMin = 120; int yMax = 320; Mat1s depth(480, 640); // 16 bit depth (in millimeters) Mat1b depth8(480, 640); // 8 bit depth Mat3b rgb(480, 640); // 8 bit depth Mat3b debug(480, 640); // debug visualization Mat1s foreground(640, 480); Mat1b foreground8(640, 480); Mat1b touch(640, 480); // touch mask Mat1s background(480, 640); vector<Mat1s> buffer(nBackgroundTrain); CHECK_RC(initOpenNI("niConfig.xml"), "initOpenNI"); // TUIO server object TuioServer* tuio; if (localClientMode) { tuio = new TuioServer(); } else { tuio = new TuioServer("192.168.0.1",3333,false); } TuioTime time; // create some sliders namedWindow(windowName); createTrackbar("xMin", windowName, &xMin, 640); createTrackbar("xMax", windowName, &xMax, 640); createTrackbar("yMin", windowName, &yMin, 480); createTrackbar("yMax", windowName, &yMax, 480); // create background model (average depth) for (unsigned int i=0; i<nBackgroundTrain; i++) { CHECK_RC(xnContext.WaitAndUpdateAll(), "xnContext.WaitAndUpdateAll()"); depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); buffer[i] = depth; } average(buffer, background); while ( waitKey(1) != 27 ) { // read available data xnContext.WaitAndUpdateAll(); // update 16 bit depth matrix depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); //xnImgeGenertor.GetGrayscale8ImageMap() // update rgb image //rgb.data = (uchar*) xnImgeGenertor.GetRGB24ImageMap(); // segmentation fault here //cvtColor(rgb, rgb, CV_RGB2BGR); // extract foreground by simple subtraction of very basic background model foreground = background - depth; // find touch mask by thresholding (points that are close to background = touch points) touch = (foreground > touchDepthMin) & (foreground < touchDepthMax); // extract ROI Rect roi(xMin, yMin, xMax - xMin, yMax - yMin); Mat touchRoi = touch(roi); // find touch points vector< vector<Point2i> > contours; vector<Point2f> touchPoints; findContours(touchRoi, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, Point2i(xMin, yMin)); for (unsigned int i=0; i<contours.size(); i++) { Mat contourMat(contours[i]); // find touch points by area thresholding if ( contourArea(contourMat) > touchMinArea ) { Scalar center = mean(contourMat); Point2i touchPoint(center[0], center[1]); touchPoints.push_back(touchPoint); } } // send TUIO cursors time = TuioTime::getSessionTime(); tuio->initFrame(time); for (unsigned int i=0; i<touchPoints.size(); i++) { // touch points float cursorX = (touchPoints[i].x - xMin) / (xMax - xMin); float cursorY = 1 - (touchPoints[i].y - yMin)/(yMax - yMin); TuioCursor* cursor = tuio->getClosestTuioCursor(cursorX,cursorY); // TODO improve tracking (don't move cursors away, that might be closer to another touch point) if (cursor == NULL || cursor->getTuioTime() == time) { tuio->addTuioCursor(cursorX,cursorY); } else { tuio->updateTuioCursor(cursor, cursorX, cursorY); } } tuio->stopUntouchedMovingCursors(); tuio->removeUntouchedStoppedCursors(); tuio->commitFrame(); // draw debug frame depth.convertTo(depth8, CV_8U, 255 / debugFrameMaxDepth); // render depth to debug frame cvtColor(depth8, debug, CV_GRAY2BGR); debug.setTo(debugColor0, touch); // touch mask rectangle(debug, roi, debugColor1, 2); // surface boundaries for (unsigned int i=0; i<touchPoints.size(); i++) { // touch points circle(debug, touchPoints[i], 5, debugColor2, CV_FILLED); } // render debug frame (with sliders) imshow(windowName, debug); //imshow("image", rgb); } return 0; }
// this function is called each frame void glutDisplay (void) { if(gScene == NULL) return; gScene->simulate(1.0f/30.0f); glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // 射影マトリックス glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(100.0f, (float)glutGet(GLUT_WINDOW_WIDTH)/(float)glutGet(GLUT_WINDOW_HEIGHT), 1.0f, 10000.0f); // 視点の位置 glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(gEye.x, gEye.y, gEye.z, gEye.x + gDir.x, gEye.y + gDir.y, gEye.z + gDir.z, 0.0f, 1.0f, 0.0f); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } for(int i=0;i<2;i++){ for(int j=0;j<15;j++){ oldjpoint[i][j]=jpoint[i][j]; } } // Process the data g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); CalculateJoint(); head.x=(jpoint[0][0].X); head.y=(jpoint[0][0].Y); head.z=(jpoint[0][0].Z); neck.x=(jpoint[0][1].X); neck.y=(jpoint[0][1].Y); neck.z=(jpoint[0][1].Z); rshoulder.x=(jpoint[0][2].X); rshoulder.y=(jpoint[0][2].Y); rshoulder.z=(jpoint[0][2].Z); relbow.x=(jpoint[0][3].X*2+oldjpoint[0][3].X)/3; relbow.y=(jpoint[0][3].Y*2+oldjpoint[0][3].Y)/3; relbow.z=(jpoint[0][3].Z*2+oldjpoint[0][3].Z)/3; rhand.x=(jpoint[0][4].X*2+oldjpoint[0][4].X)/3; rhand.y=(jpoint[0][4].Y*2+oldjpoint[0][4].Y)/3; rhand.z=(jpoint[0][4].Z*2+oldjpoint[0][4].Z)/3; lshoulder.x=(jpoint[0][5].X*2+oldjpoint[0][5].X)/3; lshoulder.y=(jpoint[0][5].Y*2+oldjpoint[0][5].Y)/3; lshoulder.z=(jpoint[0][5].Z*2+oldjpoint[0][5].Z)/3; lelbow.x=(jpoint[0][6].X*2+oldjpoint[0][6].X)/3; lelbow.y=(jpoint[0][6].Y*2+oldjpoint[0][6].Y)/3; lelbow.z=(jpoint[0][6].Z*2+oldjpoint[0][6].Z)/3; lhand.x=(jpoint[0][7].X*2+oldjpoint[0][7].X)/3; lhand.y=(jpoint[0][7].Y*2+oldjpoint[0][7].Y)/3; lhand.z=(jpoint[0][7].Z*2+oldjpoint[0][7].Z)/3; torso.x=(jpoint[0][8].X*2+oldjpoint[0][8].X)/3; torso.y=(jpoint[0][8].Y*2+oldjpoint[0][8].Y)/3; torso.z=(jpoint[0][8].Z*2+oldjpoint[0][8].Z)/3; rhip.x=(jpoint[0][9].X*2+oldjpoint[0][9].X)/3; rhip.y=(jpoint[0][9].Y*2+oldjpoint[0][9].Y)/3; rhip.z=(jpoint[0][9].Z*2+oldjpoint[0][9].Z)/3; rknee.x=(jpoint[0][10].X*2+oldjpoint[0][10].X)/3; rknee.y=(jpoint[0][10].Y*2+oldjpoint[0][10].Y)/3; rknee.z=(jpoint[0][10].Z*2+oldjpoint[0][10].Z)/3; rfoot.x=(jpoint[0][11].X*2+oldjpoint[0][11].X)/3; rfoot.y=(jpoint[0][11].Y*2+oldjpoint[0][11].Y)/3; rfoot.z=(jpoint[0][11].Z*2+oldjpoint[0][11].Z)/3; lhip.x=(jpoint[0][12].X*2+oldjpoint[0][12].X)/3; lhip.y=(jpoint[0][12].Y*2+oldjpoint[0][12].Y)/3; lhip.z=(jpoint[0][12].Z*2+oldjpoint[0][12].Z)/3; lknee.x=(jpoint[0][13].X*2+oldjpoint[0][13].X)/3; lknee.y=(jpoint[0][13].Y*2+oldjpoint[0][13].Y)/3; lknee.z=(jpoint[0][13].Z*2+oldjpoint[0][13].Z)/3; lfoot.x=(jpoint[0][14].X*2+oldjpoint[0][14].X)/3; lfoot.y=(jpoint[0][14].Y*2+oldjpoint[0][14].Y)/3; lfoot.z=(jpoint[0][14].Z*2+oldjpoint[0][14].Z)/3; printf("%f, %f, %f\n",rightreduction.x, rightreduction.y, rightreduction.z); printf("%f, %f, %f\n",leftreduction.x, leftreduction.y, leftreduction.z); if(jpoint[0][8].X!=0.0&&jpoint[0][8].Y!=0.0&&jpoint[0][8].Z!=0.0){ Head->setGlobalPosition(NxVec3(-head.x+positionx, -head.y+positiony, -head.z+positionz)); Head->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); Neck->setGlobalPosition(NxVec3(-neck.x+positionx, -neck.y+positiony, -neck.z+positionz)); Neck->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightshoulder->setGlobalPosition(NxVec3(-rshoulder.x+positionx, -rshoulder.y+positiony, -rshoulder.z+positionz)); rightshoulder->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightelbow->setGlobalPosition(NxVec3(-relbow.x+positionx, -relbow.y+positiony, -relbow.z+positionz)); rightelbow->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); righthand->setGlobalPosition(NxVec3(-rhand.x+positionx, -rhand.y+positiony, -rhand.z+positionz)); righthand->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftshoulder->setGlobalPosition(NxVec3(-lshoulder.x+positionx, -lshoulder.y+positiony, -lshoulder.z+positionz)); leftshoulder->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftelbow->setGlobalPosition(NxVec3(-lelbow.x+positionx, -lelbow.y+positiony, -lelbow.z+positionz)); leftelbow->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); lefthand->setGlobalPosition(NxVec3(-lhand.x+positionx, -lhand.y+positiony, -lhand.z+positionz)); lefthand->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); Torso->setGlobalPosition(NxVec3(-torso.x+positionx, -torso.y+positiony, -torso.z+positionz)); Torso->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); righthip->setGlobalPosition(NxVec3(-rhip.x+positionx, -rhip.y+positiony, -rhip.z+positionz)); righthip->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightknee->setGlobalPosition(NxVec3(-rknee.x+positionx, -rknee.y+positiony, -rknee.z+positionz)); rightknee->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightfoot->setGlobalPosition(NxVec3(-rfoot.x+positionx, -rfoot.y+positiony, -rfoot.z+positionz)); rightfoot->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); lefthip->setGlobalPosition(NxVec3(-lhip.x+positionx, -lhip.y+positiony, -lhip.z+positionz)); lefthip->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftknee->setGlobalPosition(NxVec3(-lknee.x+positionx, -lknee.y+positiony, -lknee.z+positionz)); leftknee->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftfoot->setGlobalPosition(NxVec3(-lfoot.x+positionx, -lfoot.y+positiony, -lfoot.z+positionz)); leftfoot->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); } glPushMatrix(); glBegin(GL_POLYGON); glColor4f(0.2f,1.2f,0.1f,1.0f); // 地面の色 glVertex3f( 10000.0f,-1.0f, 10000.0f); glVertex3f( 10000.0f,-1.0f,-10000.0f); glVertex3f(-10000.0f,-1.0f,-10000.0f); glVertex3f(-10000.0f,-1.0f, 10000.0f); glEnd(); glPopMatrix(); //地平線の描画 glPushMatrix(); glColor4f(1.0f, 1.0f, 1.0f,1.0f); glLineWidth(1); glBegin(GL_LINES); for(int i=-10000; i< 10000; i+=500) { glVertex3f( i , -0.2f,-10000 ); glVertex3f( i , -0.2f, 10000 ); glVertex3f(-10000, -0.2f, i ); glVertex3f( 10000, -0.2f, i ); } glEnd(); glPopMatrix(); switch(gameflag){ case 0: // スタート画面 glViewport( 0, 0, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT) ); glMatrixMode( GL_PROJECTION ); glLoadIdentity(); glOrtho( 0.0f, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT), 0.0f, 0.0f, 1.0f ); glMatrixMode( GL_MODELVIEW ); glLoadIdentity(); glPushMatrix(); glColor3d(1.0, 1.0, 0.0); glRasterPos3d(GL_WIN_SIZE_X/2-80, GL_WIN_SIZE_Y/2, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,"Push 's' -> START"); glPopMatrix(); printf("Push 's' -> START\n"); break; case 1: // ゲーム終了時 glViewport( 0, 0, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT) ); glMatrixMode( GL_PROJECTION ); glLoadIdentity(); glOrtho( 0.0f, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT), 0.0f, 0.0f, 1.0f ); glMatrixMode( GL_MODELVIEW ); glLoadIdentity(); glPushMatrix(); glColor3d(0.0, 0.0, 1.0); glRasterPos3d(GL_WIN_SIZE_X/2-60, GL_WIN_SIZE_Y/2+20, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,"GAME OVER"); glPopMatrix(); glPushMatrix(); glColor3d(1.0, 1.0, 0.0); glRasterPos3d(GL_WIN_SIZE_X/2-65, GL_WIN_SIZE_Y/2, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,gametime); glPopMatrix(); glPushMatrix(); glColor3d(1.0, 0.0, 0.0); glRasterPos3d(GL_WIN_SIZE_X/2-90, GL_WIN_SIZE_Y/2-20, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,"Push 'r' -> RESTART"); glPopMatrix(); printf("GAME OVER\ntime -> %f\n",(double)(t2 - t1) / CLOCKS_PER_SEC); break; case 2: // ゲーム中の画面 glEnable(GL_DEPTH_TEST); // アクターの描画 int nbActors = gScene->getNbActors(); NxActor** actors = gScene->getActors(); while(nbActors--) { NxActor* actor = *actors++; if(!actor->userData) continue; glPushMatrix(); glEnable(GL_LIGHTING); float glMat[16]; actor->getGlobalPose().getColumnMajor44(glMat); glMultMatrixf(glMat); myData* mydata = (myData*)actor->userData; int shape = mydata->shape; int color = mydata->color; switch(shape){ case 1: glColor4f(0.0f,0.0f,1.0f,1.0f); if(mydata->houkou==0){ link(pole[0], pole[4]); } else if(mydata->houkou==1){ tlink(3000.0,2000.0,6000.0,-3000.0,2000.0,6000.0); tlink(-3000.0,2000.0,4000.0,-3000.0,2000.0,6000.0); } else if(mydata->houkou==2){ ylink(3000.0,2000.0,4000.0,-3000.0,2000.0,4000.0); ylink(3000.0,2000.0,6000.0,-3000.0,2000.0,6000.0); } else if(mydata->houkou==3){ // パネル glColor4f(0.0f,1.0f,1.0f,1.0f); cuboid(mydata->width*2,mydata->length*2, mydata->height*2); } else if(mydata->houkou==4){ } break; case 2: if(mydata->ball == 1){ glColor4f(1.0f,0.0f,0.0f,1.0f); // ボールの色 glutSolidSphere(mydata->size,15,15); } else if(mydata->ball == 0){ glColor4f(1.0f,1.0f,0.0f,1.0f); glutSolidSphere(mydata->size,15,15); } break; case 4: glColor4f(1.0f,0.0f,0.0f,1.0f); cylinder(50, 100, 50); break; } glDisable(GL_LIGHTING); glPopMatrix(); } glDisable(GL_DEPTH_TEST); t2 = clock(); balltimer2 = clock(); btime=(double)(balltimer2 - balltimer1) / CLOCKS_PER_SEC; gtime=(double)(t2 - t1) / CLOCKS_PER_SEC; printf("%f\n",gtime); if(gtime<=10.0){ balldire=2.0; ballspeed=250.0; } else if(gtime<=20.0){ balldire=1.5; ballspeed=300.0; } else if(gtime<=30.0){ balldire=1.0; ballspeed=350.0; } else if(gtime<=40.0){ balldire=0.5; ballspeed=400.0; } break; } // 描画の終了 gScene->flushStream(); gScene->fetchResults(NX_RIGID_BODY_FINISHED, true); glutSwapBuffers(); }
int main() { const unsigned int nBackgroundTrain = 30; const unsigned short touchDepthMin = 10; const unsigned short touchDepthMax = 20; const unsigned int touchMinArea = 50; const bool localClientMode = true; // connect to a local client const double debugFrameMaxDepth = 4000; // maximal distance (in millimeters) for 8 bit debug depth frame quantization const char* windowName = "Debug"; const char* colorWindowName = "image"; const Scalar debugColor0(0, 0, 128); const Scalar debugColor1(255, 0, 0); const Scalar debugColor2(255, 255, 255); const Scalar debugColor3(0, 255, 255); const Scalar debugColor4(255, 0, 255); int xMin = 50; int xMax = 550; int yMin = 50; int yMax = 300; Mat1s depth(480, 640); // 16 bit depth (in millimeters) Mat1b depth8(480, 640); // 8 bit depth Mat3b rgb(480, 640); // 8 bit depth Mat3b debug(480, 640); // debug visualization Mat1s foreground(640, 480); Mat1b foreground8(640, 480); Mat1b touch(640, 480); // touch mask Mat1s background(480, 640); vector<Mat1s> buffer(nBackgroundTrain); IplImage * image = cvCreateImage(cvSize(640, 480), 8, 3); IplImage * convertedImage = cvCreateImage(cvSize(640, 480), 8, 3); initOpenNI("niConfig.xml"); // TUIO server object TuioServer* tuio; if (localClientMode) { tuio = new TuioServer(); } else { tuio = new TuioServer("192.168.0.2", 3333, false); } TuioTime time; namedWindow(colorWindowName); createTrackbar("xMin", colorWindowName, &xMin, 640); createTrackbar("xMax", colorWindowName, &xMax, 640); createTrackbar("yMin", colorWindowName, &yMin, 480); createTrackbar("yMax", colorWindowName, &yMax, 480); // create some sliders namedWindow(windowName); createTrackbar("xMin", windowName, &xMin, 640); createTrackbar("xMax", windowName, &xMax, 640); createTrackbar("yMin", windowName, &yMin, 480); createTrackbar("yMax", windowName, &yMax, 480); Keyboard * piano = new Keyboard(); (*piano).initKeyMap(); system("qjackctl &"); sleep(4); JackByTheNotes * notesJack = new JackByTheNotes(); notesJack->connect(); sleep(2); system("sudo jack_connect Piano:Rubinstein system:playback_1 &"); map<double, timeval> keys; // create background model (average depth) for (unsigned int i = 0; i < nBackgroundTrain; i++) { xnContext.WaitAndUpdateAll(); depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); buffer[i] = depth; } average(buffer, background); while (waitKey(1) != 27) { // read available data xnContext.WaitAndUpdateAll(); // update 16 bit depth matrix depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); //xnImgeGenertor.GetGrayscale8ImageMap() XnRGB24Pixel* xnRgb = const_cast<XnRGB24Pixel*>(xnImgeGenertor.GetRGB24ImageMap()); // IplImage * image = cvCreateImage(cvSize(640, 480), 8, 3); // IplImage * convertedImage = cvCreateImage(cvSize(640, 480), 8, 3); cvSetData (image, xnRgb, 640 * 3); cvConvertImage(image, convertedImage, CV_CVTIMG_SWAP_RB); bool color = true; rgb = convertedImage; // cvtColor(rgb,rgb,CV_RGB2BGR); // update rgb image // rgb.data = (uchar*) xnImgeGenertor.GetRGB24ImageMap(); // segmentation fault here // cvCvtColor(rgb, rgb, CV_BGR2RGB); // extract foreground by simple subtraction of very basic background model foreground = background - depth; // find touch mask by thresholding (points that are close to background = touch points) touch = (foreground > touchDepthMin) & (foreground < touchDepthMax); // extract ROI Rect roi(xMin, yMin, xMax - xMin, yMax - yMin); Mat touchRoi = touch(roi); // find touch points vector<vector<Point2i> > contours; vector<Point2f> touchPoints; findContours(touchRoi, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, Point2i(xMin, yMin)); for (unsigned int i = 0; i < contours.size(); i++) { Mat contourMat(contours[i]); // find touch points by area thresholding if (contourArea(contourMat) > touchMinArea) { Scalar center = mean(contourMat); Point2i touchPoint(center[0], center[1]); touchPoints.push_back(touchPoint); } } // send TUIO cursors time = TuioTime::getSessionTime(); tuio->initFrame(time); for (unsigned int i = 0; i < touchPoints.size(); i++) { // touch points float cursorX = (touchPoints[i].x - xMin) / (xMax - xMin); float cursorY = 1 - (touchPoints[i].y - yMin) / (yMax - yMin); TuioCursor* cursor = tuio->getClosestTuioCursor(cursorX, cursorY); // TODO improve tracking (don't move cursors away, that might be closer to another touch point) if (cursor == NULL || cursor->getTuioTime() == time) { tuio->addTuioCursor(cursorX, cursorY); } else { tuio->updateTuioCursor(cursor, cursorX, cursorY); } } tuio->stopUntouchedMovingCursors(); tuio->removeUntouchedStoppedCursors(); tuio->commitFrame(); // draw debug frame depth.convertTo(depth8, CV_8U, 255 / debugFrameMaxDepth); // render depth to debug frame cvtColor(depth8, debug, CV_GRAY2BGR); debug.setTo(debugColor0, touch); // touch mask rectangle(debug, roi, debugColor1, 2); // surface boundaries if (color) rectangle(rgb, roi, debugColor1, 2); // surface boundaries // draw 10 white keys within the roi int stride = (xMax - xMin) / 10; for (int keys = 1; keys < 10; keys++) { Point lower(xMin + keys * stride, yMax); if (keys == 3 || keys == 7) { Point upper(xMin + keys * stride, yMin); line(debug, upper, lower, debugColor3, 2, 0); if (color) line(rgb, upper, lower, debugColor3, 2, 0); continue; } else { Point upper(xMin + keys * stride, (yMin + yMax) / 2); line(debug, upper, lower, debugColor3, 2, 0); if (color) line(rgb, upper, lower, debugColor3, 2, 0); } Point blkUpper(xMin + keys * stride - stride / 3, yMin); Point blkLower(xMin + keys * stride + stride / 3, (yMin + yMax) / 2); rectangle(debug, blkUpper, blkLower, debugColor4, 2); if (color) rectangle(rgb, blkUpper, blkLower, debugColor4, 2); } for (unsigned int i = 0; i < touchPoints.size(); i++) { // touch points circle(debug, touchPoints[i], 5, debugColor2, CV_FILLED); if (color) circle(rgb, touchPoints[i], 5, debugColor2, CV_FILLED); double frequency = piano->keyFrequency(touchPoints[i].y - 50, touchPoints[i].x - 50); cout << frequency << " " << touchPoints[i].y - 50 << " " << touchPoints[i].x - 50 << endl; if (keys.find(frequency) == keys.end()) { Note * note = new Note(frequency, 2, 4000); notesJack->playNote(*note); timeval now; gettimeofday(&now, NULL); keys[frequency] = now; } else { timeval now; gettimeofday(&now, NULL); if ((now.tv_sec - keys[frequency].tv_sec) * 1000 + (now.tv_usec - keys[frequency].tv_usec) / 1000 > 1000) { Note * note = new Note(frequency, 2, 4000); notesJack->playNote(*note); keys[frequency] = now; } } } // render debug frame (with sliders) // IplImage grayScale = debug; // cvFlip(&grayScale, NULL, 1); // Mat gray(&grayScale); // imshow(windowName, gray); // // IplImage colorful = rgb; // cvFlip(&colorful, NULL, 1); // Mat real(&colorful); // imshow("image", real); imshow(windowName, debug); imshow("image", rgb); // cvShowImage("image", image); } return 0; }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; if(!bInitialized) { texWidth = getClosestPowerOfTwo(dmd.XRes()); texHeight = getClosestPowerOfTwo(dmd.YRes()); // printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; // printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight); bInitialized = true; topLeftX = dmd.XRes(); topLeftY = 0; bottomRightY = dmd.YRes(); bottomRightX = 0; texXpos =(float)dmd.XRes()/texWidth; texYpos =(float)dmd.YRes()/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // Calculate the accumulative histogram memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float)); for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { g_pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; pDestImage[0] = nHistValue * Colors[nColorID][0]; pDestImage[1] = nHistValue * Colors[nColorID][1]; pDestImage[2] = nHistValue * Colors[nColorID][2]; } else { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.75,0.75,0.75,1); glEnable(GL_TEXTURE_2D); DrawTexture(dmd.XRes(),dmd.YRes(),0,0); glDisable(GL_TEXTURE_2D); char strLabel[20] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { XnPoint3D com; g_UserGenerator.GetCoM(aUsers[i], com); g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com); if (aUsers[i] == player) sprintf(strLabel, "%d (Player)", aUsers[i]); else sprintf(strLabel, "%d", aUsers[i]); glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(com.X, com.Y); glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } // Draw skeleton of user if (player != 0) { glBegin(GL_LINES); glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1); // gesture static int gesture = 0; static XnPoint3D previousLeftHandPt; XnPoint3D newLeftHandPt; newLeftHandPt = getJointPoint(player, XN_SKEL_LEFT_HAND); if(previousLeftHandPt.X > 0 && previousLeftHandPt.X < 640) if(previousLeftHandPt.X - newLeftHandPt.X > 60) gesture = 1; else if(previousLeftHandPt.X - newLeftHandPt.X < -60) gesture = 2; else if(previousLeftHandPt.Y - newLeftHandPt.Y > 60) gesture = 3; else if(previousLeftHandPt.Y - newLeftHandPt.Y < -60) gesture = 4; else gesture = 0; if(gesture != 0) printf("gesture: %d\n", gesture); previousLeftHandPt = newLeftHandPt; // head XnPoint3D pt = getJointPoint(player, XN_SKEL_HEAD); // save saveLocation(pt, newLeftHandPt, gesture); DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK); DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER); DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW); DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND); DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER); DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW); DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND); DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO); DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO); DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP); DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE); DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT); DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP); DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE); DrawLimb(player, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT); glEnd(); } }
int main(int argc, char* argv[]) { glue.Init(argc, argv, 640, 240, "TrackHand"); xn::Context context; XnStatus status = context.Init(); bmg::OnError(status, []{ std::cout << "Couldn't init OpenNi!" << std::endl; exit(1); }); xn::ImageGenerator image_generator; status = image_generator.Create(context); bmg::OnError(status, []{ std::cout << "Couldn't create image generator!" << std::endl; }); status = depth_generator.Create(context); bmg::OnError(status, []{ std::cout << "Couldn't create depth generator!" << std::endl; }); xn::ImageMetaData image_metadata; xn::DepthMetaData depth_metadata; // Create gesture & hands generators status = gesture_generator.Create(context); bmg::OnError(status, []{ std::cout << "Couldn't create gesture generator!" << std::endl; }); status = hands_generator.Create(context); bmg::OnError(status, []{ std::cout << "Couldn't create hands generator!" << std::endl; }); // Register to callbacks XnCallbackHandle h1, h2; gesture_generator .RegisterGestureCallbacks(Gesture_Recognized, Gesture_Process, NULL, h1); hands_generator .RegisterHandCallbacks(Hand_Create, Hand_Update, Hand_Destroy, NULL, h2); status = context.StartGeneratingAll(); bmg::OnError(status, []{ std::cout << "Couldn't generate all data!" << std::endl; }); status = gesture_generator.AddGesture(GESTURE, NULL); bmg::OnError(status, []{ std::cout << "Couldn't add gesture!" << std::endl; }); glue.BindDisplayFunc([&]{ glue.BeginDraw(); // here goes code for app main loop XnStatus status = context.WaitAndUpdateAll(); bmg::OnError(status, []{ std::cout << "Couldn't update and wait for new data!" << std::endl; }); image_generator.GetMetaData(image_metadata); unsigned imageX = image_metadata.XRes(); unsigned imageY = image_metadata.YRes(); glue.DrawOnTexture( (void*)image_metadata.RGB24Data(), imageX, imageY, imageX, imageY, 320, 0, 640, 240); depth_generator.GetMetaData(depth_metadata); unsigned depthX = depth_metadata.XRes(); unsigned depthY = depth_metadata.YRes(); XnRGB24Pixel* transformed_depth_map = new XnRGB24Pixel[depthX * depthY]; bmg::CalculateDepth( depth_generator.GetDepthMap(), depthX, depthY, MAX_DEPTH, transformed_depth_map); glue.DrawOnTexture( (void*)transformed_depth_map, depthX, depthY, depthX, depthY, 0, 0, 320, 240); delete [] transformed_depth_map; if (hand_recognized) { // Draw point over tracked hand glue.DrawPointOverRegion(static_cast<unsigned>(projective_point.X), static_cast<unsigned>(projective_point.Y), 0, 0); glue.DrawPointOverRegion(static_cast<unsigned>(projective_point.X), static_cast<unsigned>(projective_point.Y), 320, 0); } glue.EndDraw(); }); glue.BindKeyboardFunc([](unsigned char key, int x, int y){ switch(key) { case 27: exit(1); } }); glue.Run(); context.Release(); }
XnStatus XnFileDevice::UpdateS2DTables(const xn::DepthGenerator& depth) { XnStatus nRetVal = XN_STATUS_OK; XnUInt64 nTemp; XnDouble dTemp; // get config XnShiftToDepthConfig config; nRetVal = depth.GetIntProperty(XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE, nTemp); XN_IS_STATUS_OK(nRetVal); config.nZeroPlaneDistance = (XnDepthPixel)nTemp; nRetVal = depth.GetRealProperty(XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE, dTemp); XN_IS_STATUS_OK(nRetVal); config.fZeroPlanePixelSize = (XnFloat)dTemp; nRetVal = depth.GetRealProperty(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, dTemp); XN_IS_STATUS_OK(nRetVal); config.fEmitterDCmosDistance = (XnFloat)dTemp; nRetVal = depth.GetIntProperty(XN_STREAM_PROPERTY_MAX_SHIFT, nTemp); XN_IS_STATUS_OK(nRetVal); config.nDeviceMaxShiftValue = (XnUInt32)nTemp; config.nDeviceMaxDepthValue = depth.GetDeviceMaxDepth(); nRetVal = depth.GetIntProperty(XN_STREAM_PROPERTY_CONST_SHIFT, nTemp); XN_IS_STATUS_OK(nRetVal); config.nConstShift = (XnUInt32)nTemp; nRetVal = depth.GetIntProperty(XN_STREAM_PROPERTY_PIXEL_SIZE_FACTOR, nTemp); XN_IS_STATUS_OK(nRetVal); config.nPixelSizeFactor = (XnUInt32)nTemp; nRetVal = depth.GetIntProperty(XN_STREAM_PROPERTY_PARAM_COEFF, nTemp); XN_IS_STATUS_OK(nRetVal); config.nParamCoeff = (XnUInt32)nTemp; nRetVal = depth.GetIntProperty(XN_STREAM_PROPERTY_SHIFT_SCALE, nTemp); XN_IS_STATUS_OK(nRetVal); config.nShiftScale = (XnUInt32)nTemp; config.nDepthMinCutOff = 0; config.nDepthMaxCutOff = (XnDepthPixel)config.nDeviceMaxDepthValue; if (!m_ShiftToDepth.bIsInitialized) { nRetVal = XnShiftToDepthInit(&m_ShiftToDepth, &config); XN_IS_STATUS_OK(nRetVal); } else { nRetVal = XnShiftToDepthUpdate(&m_ShiftToDepth, &config); XN_IS_STATUS_OK(nRetVal); } // notify nRetVal = m_pNotifications->OnNodeGeneralPropChanged(m_pNotificationsCookie, depth.GetName(), XN_STREAM_PROPERTY_S2D_TABLE, m_ShiftToDepth.nShiftsCount * sizeof(XnDepthPixel), m_ShiftToDepth.pShiftToDepthTable); XN_IS_STATUS_OK(nRetVal); nRetVal = m_pNotifications->OnNodeGeneralPropChanged(m_pNotificationsCookie, depth.GetName(), XN_STREAM_PROPERTY_D2S_TABLE, m_ShiftToDepth.nDepthsCount * sizeof(XnUInt16), m_ShiftToDepth.pDepthToShiftTable); XN_IS_STATUS_OK(nRetVal); return (XN_STATUS_OK); }
int main(int argc, char **argv) { commInit("192.168.1.255", 54321); XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; const char *fn = NULL; if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH; else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL; else { printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL); return XN_STATUS_ERROR; } printf("Reading config from: '%s'\n", fn); nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal,"No depth"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); XnUserID aUsers[MAX_NUM_USERS]; XnUInt16 nUsers; int j; printf("Starting to run\n"); if(g_bNeedPose) { printf("Assume calibration pose\n"); } XnUInt32 epochTime = 0; while (!xnOSWasKeyboardHit()) { g_Context.WaitOneUpdateAll(g_UserGenerator); // print the torso information for the first user already tracking nUsers=MAX_NUM_USERS; g_UserGenerator.GetUsers(aUsers, nUsers); int numTracked=0; int userToPrint=-1; for(XnUInt16 i=0; i<nUsers; i++) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE) continue; if(getJoints(aUsers[i])){ /** printf("Left Elbow: %.2f\nLeft Shoulder Roll: %.2f\nLeft Shoulder Pitch: %.2f\nHead Pitch: %.2f\n", findAngle(jointArr[6], jointArr[4], jointArr[8], 0), findAngle(jointArr[4], jointArr[10], jointArr[6], 0), findAngle(jointArr[4], jointArr[10], jointArr[6], 1), findAngle(jointArr[2], jointArr[1], jointArr[0], 1) ); **/ float headAngle = findAngle(jointArr[2], jointArr[1], jointArr[0], 1); float leftElbowAngle = findAngle(jointArr[6], jointArr[4], jointArr[8], 0); float leftShoulderRoll = findAngle(jointArr[4], jointArr[10], jointArr[6], 0); float leftShoulderPitch = findAngle(jointArr[4], jointArr[10], jointArr[6], 1); float rightElbowAngle = findAngle(jointArr[5], jointArr[3], jointArr[7], 0); float rightShoulderRoll = findAngle(jointArr[3], jointArr[9], jointArr[5], 0); float rightShoulderPitch = findAngle(jointArr[3], jointArr[9], jointArr[5], 1); ostringstream ostr; ostr << "{"<<headAngle<<",{"<<leftShoulderPitch<<","<<leftShoulderRoll<<","<< leftElbowAngle<<"},{"<<rightShoulderPitch<<","<<rightShoulderRoll<<","<<rightElbowAngle<<"},}"; string send = ostr.str(); //cout<<send; //printRotation(aUsers[i]); commSend(send); usleep(200000); } } } g_scriptNode.Release(); g_DepthGenerator.Release(); g_UserGenerator.Release(); g_Context.Release(); }
int main(int argc, char **argv) { sender.setup(HOST, PORT); XnStatus rc = XN_STATUS_OK; rc = g_Context.InitFromXmlFile(SAMPLE_XML_PATH); CHECK_RC(rc, "InitFromXml"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(rc, "Find depth generator"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); CHECK_RC(rc, "Find user generator"); XnCallbackHandle h; g_UserGenerator.RegisterUserCallbacks(NewUser, LostUser, NULL, h); g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); XnCallbackHandle hCalib; XnCallbackHandle hPose; g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(&CalibrationStart, &CalibrationEnd, NULL, hCalib); g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(&PoseDetected, NULL, NULL, hPose); rc = g_Context.StartGeneratingAll(); CHECK_RC(rc, "StartGenerating"); xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); fXRes = depthMD.XRes(); fYRes = depthMD.YRes(); fMaxDepth = depthMD.ZRes(); nCutOffMin = 0; nCutOffMax = fMaxDepth; nPointerX = fXRes / 2; nPointerY = fXRes / 2; nPointerDiffX = (WIN_SIZE_X / fXRes / 2) + 1; nPointerDiffY = (WIN_SIZE_Y / fXRes / 2) + 1; if (argc == 2) { nCutOffMax = atol(argv[1]); } srand(XnUInt32(time(NULL))); glutInit(&argc, argv); glutInitDisplayString("stencil depth>16 double rgb samples=0"); glutInitWindowSize(WIN_SIZE_X, WIN_SIZE_Y); glutCreateWindow("Prime Sense Stick Figure Sample"); glutSetCursor(GLUT_CURSOR_NONE); init_opengl(); glut_helpers_initialize(); cb.passive_motion_function = MotionCallback; cb.keyboard_function = key; camera.configure_buttons(0); camera.set_camera_mode(true); camera.set_parent_rotation( & camera.trackball.r); camera.enable(); object.configure_buttons(1); object.translator.t[2] = -1; object.translator.scale *= .1f; object.trackball.r = rotationf(vec3f(2.0,0.01,0.01), to_radians(180)); object.set_parent_rotation( & camera.trackball.r); object.disable(); light.configure_buttons(0); light.translator.t = vec3f(.5, .5, -1); light.set_parent_rotation( & camera.trackball.r); light.disable(); reshaper.zNear = 1; reshaper.zFar = 100; // make sure all interactors get glut events glut_add_interactor(&cb); glut_add_interactor(&camera); glut_add_interactor(&reshaper); glut_add_interactor(&light); glut_add_interactor(&object); camera.translator.t = vec3f(0, 0, 0); camera.trackball.r = rotationf(vec3f(0, 0, 0), to_radians(0)); light.translator.t = vec3f (0, 1.13, -2.41); light.trackball.r = rotationf(vec3f(0.6038, -0.1955, -0.4391), to_radians(102)); glutIdleFunc(idle); glutDisplayFunc(display); // Per frame code is in display glutMainLoop(); return (0); }
int main(int argc, char **argv) { ros::init(argc, argv, "PersonTracker"); ros::NodeHandle nh; ros::Rate loop_rate(10); ROS_INFO("Initalizing Arm Connection...."); ros::Publisher leftArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n0/position", 1000); ros::Publisher rightArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n1/position", 1000); yourmom = nh.advertise<prlite_kinematics::some_status_thing>("kinect_hack_status", 1000); ros::Duration(2.0).sleep(); prlite_kinematics::SphereCoordinate coord; prlite_kinematics::some_status_thing status; coord.radius = 35.0; coord.phi = 0.0; coord.theta = 0.0; status.lulz = 0; //initialized/reset yourmom.publish(status); leftArmPublisher.publish(coord); rightArmPublisher.publish(coord); ROS_INFO("Initalizing Kinect + NITE...."); XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); ROS_INFO("Ready To Go!\n"); while (ros::ok()) { // Update OpenNI g_Context.WaitAndUpdateAll(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); // Print positions XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // Read joint positions for person (No WRISTs) XnSkeletonJointPosition torsoPosition, lShoulderPosition, rShoulderPosition, neckPosition, headPosition, lElbowPosition, rElbowPosition; XnSkeletonJointPosition rWristPosition, lWristPosition, rHipPosition, lHipPosition, lKneePosition, rKneePosition; XnSkeletonJointPosition lFootPosition, rFootPosition; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_TORSO, torsoPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_NECK, neckPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, headPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, lWristPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, rWristPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HIP, lHipPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HIP, rHipPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_KNEE, lKneePosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneePosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_FOOT, lFootPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootPosition); // Read Joint Orientations XnSkeletonJointOrientation torsoOrientation, lShoulderOrientation, rShoulderOrientation, lHipOrientation, rHipOrientation; XnSkeletonJointOrientation lWristOrientation, rWristOrientation, lElbowOrientation, rElbowOrientation; XnSkeletonJointOrientation headOrientation, neckOrientation; XnSkeletonJointOrientation lKneeOrientation, rKneeOrientation, lFootOrientation, rFootOrientation; g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_TORSO, torsoOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_HEAD, headOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_NECK, neckOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HIP, lHipOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HIP, rHipOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HAND, lWristOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HAND, rWristOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_KNEE, lKneeOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneeOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_FOOT, lFootOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootOrientation); XnFloat* m = torsoOrientation.orientation.elements; KDL::Rotation torsoO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lShoulderOrientation.orientation.elements; KDL::Rotation lShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rShoulderOrientation.orientation.elements; KDL::Rotation rShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lHipOrientation.orientation.elements; KDL::Rotation lHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rHipOrientation.orientation.elements; KDL::Rotation rHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = headOrientation.orientation.elements; KDL::Rotation headO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = neckOrientation.orientation.elements; KDL::Rotation neckO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lElbowOrientation.orientation.elements; KDL::Rotation lElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rElbowOrientation.orientation.elements; KDL::Rotation rElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lWristOrientation.orientation.elements; KDL::Rotation lWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rWristOrientation.orientation.elements; KDL::Rotation rWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lKneeOrientation.orientation.elements; KDL::Rotation lKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rKneeOrientation.orientation.elements; KDL::Rotation rKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lFootOrientation.orientation.elements; KDL::Rotation lFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rFootOrientation.orientation.elements; KDL::Rotation rFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); double qx = 0.0, qy = 0.0, qz = 0.0, qw = 0.0; // Get Points in 3D space XnPoint3D torso = torsoPosition.position; XnPoint3D lShould = lShoulderPosition.position; XnPoint3D rShould = rShoulderPosition.position; XnPoint3D lElbow = lElbowPosition.position; XnPoint3D rElbow = rElbowPosition.position; XnPoint3D lWrist = lWristPosition.position; XnPoint3D rWrist = rWristPosition.position; XnPoint3D neck = neckPosition.position; XnPoint3D head = headPosition.position; XnPoint3D lHip = lHipPosition.position; XnPoint3D rHip = rHipPosition.position; XnPoint3D lKnee = lKneePosition.position; XnPoint3D rKnee = rKneePosition.position; XnPoint3D lFoot = lFootPosition.position; XnPoint3D rFoot = rFootPosition.position; // ---------- ARM CONTROLLER HACK ------------------ // Calculate arm length of user double lenL = calc3DDist( lShould, lElbow ) + calc3DDist( lElbow, lWrist ); double lenR = calc3DDist( rShould, rElbow ) + calc3DDist( rElbow, rWrist ); double distL = calc3DDist(lShould, lWrist); double distR = calc3DDist(rShould, rWrist); // Calculate positions prlite_kinematics::SphereCoordinate lCoord; prlite_kinematics::SphereCoordinate rCoord; lCoord.radius = 35 * (distL / lenL); rCoord.radius = 35 * (distR / lenR); lCoord.theta = -atan2(lShould.X - lWrist.X, lShould.Z - lWrist.Z); rCoord.theta = -atan2(rShould.X - rWrist.X, rShould.Z - rWrist.Z); if(lCoord.theta > 1.0) lCoord.theta = 1.0; if(lCoord.theta < -1.0) lCoord.theta = -1.0; if(rCoord.theta > 1.0) rCoord.theta = 1.0; if(rCoord.theta < -1.0) rCoord.theta = -1.0; lCoord.phi = -atan2(lShould.Y - lWrist.Y, lShould.X - lWrist.X); rCoord.phi = -atan2(rShould.Y - rWrist.Y, rShould.X - rWrist.X); if(lCoord.phi > 1.25) lCoord.phi = 1.25; if(lCoord.phi < -0.33) lCoord.phi = -0.33; if(rCoord.phi > 1.2) rCoord.phi = 1.25; if(rCoord.phi < -0.33) rCoord.phi = -0.33; ROS_INFO("User %d: Left (%lf,%lf,%lf), Right (%lf,%lf,%lf)", i, lCoord.radius, lCoord.theta, lCoord.phi, rCoord.radius, rCoord.theta, rCoord.phi); // Publish to arms leftArmPublisher.publish(rCoord); rightArmPublisher.publish(lCoord); // ---------- END HACK ----------------- // Publish Transform static tf::TransformBroadcaster br; tf::Transform transform; std::stringstream body_name, neck_name, lshould_name, rshould_name, head_name, relbow_name, lelbow_name, lwrist_name, rwrist_name; std::stringstream lhip_name, rhip_name, lknee_name, rknee_name, lfoot_name, rfoot_name; body_name << "Torso (" << i << ")" << std::ends; neck_name << "Neck (" << i << ")" << std::ends; head_name << "Head (" << i << ")" << std::ends; lshould_name << "Shoulder L (" << i << ")" << std::ends; rshould_name << "Shoulder R (" << i << ")" << std::ends; lelbow_name << "Elbow L (" << i << ")" << std::ends; relbow_name << "Elbow R (" << i << ")" << std::ends; lwrist_name << "Wrist L (" << i << ")" << std::ends; rwrist_name << "Wrist R (" << i << ")" << std::ends; lhip_name << "Hip L (" << i << ")" << std::ends; rhip_name << "Hip R (" << i << ")" << std::ends; lknee_name << "Knee L (" << i << ")" << std::ends; rknee_name << "Knee R (" << i << ")" << std::ends; lfoot_name << "Foot L (" << i << ")" << std::ends; rfoot_name << "Foot R (" << i << ")" << std::ends; // Publish Torso torsoO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(torso.X / ADJUST_VALUE, torso.Y / ADJUST_VALUE, torso.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", body_name.str().c_str())); // Publish Left Shoulder lShouldO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lShould.X / ADJUST_VALUE, lShould.Y / ADJUST_VALUE, lShould.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lshould_name.str().c_str())); // Publish Right Shoulder rShouldO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rShould.X / ADJUST_VALUE, rShould.Y / ADJUST_VALUE, rShould.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rshould_name.str().c_str())); // Publish Left Elbow lElbowO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lElbow.X / ADJUST_VALUE, lElbow.Y / ADJUST_VALUE, lElbow.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lelbow_name.str().c_str())); // Publish Right Elbow rElbowO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rElbow.X / ADJUST_VALUE, rElbow.Y / ADJUST_VALUE, rElbow.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", relbow_name.str().c_str())); // Publish Left Wrist lWristO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lWrist.X / ADJUST_VALUE, lWrist.Y / ADJUST_VALUE, lWrist.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lwrist_name.str().c_str())); // Publish Right Wrist rWristO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rWrist.X / ADJUST_VALUE, rWrist.Y / ADJUST_VALUE, rWrist.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rwrist_name.str().c_str())); // Publish Neck neckO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(neck.X / ADJUST_VALUE, neck.Y / ADJUST_VALUE, neck.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", neck_name.str().c_str())); // Publish Head headO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(head.X / ADJUST_VALUE, head.Y / ADJUST_VALUE, head.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", head_name.str().c_str())); // Publish Left Hip lHipO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lHip.X / ADJUST_VALUE, lHip.Y / ADJUST_VALUE, lHip.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lhip_name.str().c_str())); // Publish Right Hip rHipO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rHip.X / ADJUST_VALUE, rHip.Y / ADJUST_VALUE, rHip.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rhip_name.str().c_str())); // Publish Left Knee lKneeO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lKnee.X / ADJUST_VALUE, lKnee.Y / ADJUST_VALUE, lKnee.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lknee_name.str().c_str())); // Publish Right Knee rKneeO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rKnee.X / ADJUST_VALUE, rKnee.Y / ADJUST_VALUE, rKnee.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rknee_name.str().c_str())); // Publish Left Foot lFootO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lFoot.X / ADJUST_VALUE, lFoot.Y / ADJUST_VALUE, lFoot.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lfoot_name.str().c_str())); // Publish Right Foot rFootO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rFoot.X / ADJUST_VALUE, rFoot.Y / ADJUST_VALUE, rFoot.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rfoot_name.str().c_str())); } } ros::spinOnce(); //loop_rate.sleep(); } g_Context.Shutdown(); return 0; }
bool DataCapture::initialise() { context_.Shutdown(); XnStatus rc = context_.Init(); if( rc != XN_STATUS_OK ) { std::cout << "Init: " << xnGetStatusString(rc) << std::endl; return false; } rc = depthGen_.Create(context_); if( rc != XN_STATUS_OK ) { std::cout << "depthGen.Create: " << xnGetStatusString(rc) << std::endl; return false; } rc = imageGen_.Create(context_); if( rc != XN_STATUS_OK ) { std::cout << "imageGen.Create: " << xnGetStatusString(rc) << std::endl; return false; } rc = imageGen_.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); if( rc != XN_STATUS_OK ) { std::cout << "SetPixelFormat: " << xnGetStatusString(rc) << std::endl; return false; } XnMapOutputMode imgMode; imgMode.nXRes = 640; // XN_VGA_X_RES imgMode.nYRes = 480; // XN_VGA_Y_RES imgMode.nFPS = 30; rc = imageGen_.SetMapOutputMode(imgMode); if( rc != XN_STATUS_OK ) { std::cout << "image SetMapOutputMode: " << xnGetStatusString(rc) << std::endl; return false; } rc = depthGen_.SetMapOutputMode(imgMode); if( rc != XN_STATUS_OK ) { std::cout << "depth SetMapOutputMode: " << xnGetStatusString(rc) << std::endl; return false; } depthGen_.GetMetaData(depthMd_); std::cout << "Depth offset " << depthMd_.XOffset() << " " << depthMd_.YOffset() << std::endl; // set the depth image viewpoint depthGen_.GetAlternativeViewPointCap().SetViewPoint(imageGen_); // read off the depth camera field of view. This is the FOV corresponding to // the IR camera viewpoint, regardless of the alternative viewpoint settings. XnFieldOfView fov; rc = depthGen_.GetFieldOfView(fov); std::cout << "Fov: " << fov.fHFOV << " " << fov.fVFOV << std::endl; pDepthData_ = new char [640 * 480]; pRgbData_ = new char [640 * 480 * 3]; return true; }
void glut_display() { xn::DepthMetaData pDepthMapMD; xn::ImageMetaData pImageMapMD; #ifdef DEBUGOUT ofstream datei; #endif glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); gluPerspective(45, WINDOW_SIZE_X/WINDOW_SIZE_Y, 1000, 5000); // glOrtho(0, WINDOW_SIZE_X, WINDOW_SIZE_Y, 0, -128, 128); glMatrixMode(GL_TEXTURE); glLoadIdentity(); // glTranslatef(-12.8/640.0, 9.0/480.0, 0); // glTranslatef(-12.8/630.0, 9.0/480.0,0); glScalef(scalex, scaley, 1.0); glTranslatef(transx/630, transy/480, 0.0); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); rot_angle+=0.7; // Warten auf neue Daten vom Tiefengenerator nRetVal = context.WaitAndUpdateAll(); checkError("Fehler beim Aktualisieren der Daten", nRetVal); // Aktuelle Depth Metadaten auslesen depth.GetMetaData(pDepthMapMD); // Aktuelle Depthmap auslesen const XnDepthPixel* pDepthMap = depth.GetDepthMap(); if(maxdepth==-1) maxdepth = getMaxDepth(pDepthMap); // Aktuelle Image Metadaten auslesen image.GetMetaData(pImageMapMD); //Aktuelles Bild auslesen const XnRGB24Pixel* pImageMap = image.GetRGB24ImageMap(); glColor3f(1, 1, 1); // XnDepthPixel maxdepth = depth.GetDeviceMaxDepth(); const unsigned int xres = pDepthMapMD.XRes(); const unsigned int yres = pDepthMapMD.YRes(); #ifdef DEBUGOUT datei.open("daniel.txt", ios::out); #endif for(unsigned int y=0; y<yres-1; y++) { for(unsigned int x=0; x<xres; x++) { aDepthMap[x+y*xres] = static_cast<GLubyte>(static_cast<float>(pDepthMap[x+y*xres])/static_cast<float>(maxdepth)*255); } } /* glEnable(GL_TEXTURE_2D); glPushMatrix(); glLoadIdentity(); glTranslatef(-800, 0, -2000); glBindTexture(GL_TEXTURE_2D, texture_rgb); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImageMap); glBegin(GL_QUADS); glTexCoord2f(0,1); glVertex3f(0,0,0); glTexCoord2f(1,1); glVertex3f(640,0,0); glTexCoord2f(1,0); glVertex3f(640,480,0); glTexCoord2f(0,0); glVertex3f(0,480,0); glEnd(); glPopMatrix(); glPushMatrix(); glLoadIdentity(); glTranslatef(-800, 600, -2000); glBindTexture(GL_TEXTURE_2D, texture_depth); glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE8, 640, 480, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, aDepthMap); glBegin(GL_QUADS); glTexCoord2f(0,1); glVertex3f(0,0,0); glTexCoord2f(1,1); glVertex3f(640,0,0); glTexCoord2f(1,0); glVertex3f(640,480,0); glTexCoord2f(0,0); glVertex3f(0,480,0); glEnd(); glPopMatrix();*/ glPushMatrix(); glLoadIdentity(); glTranslatef(-100, -100, -2000); glRotatef(cx,0,1,0); glRotatef(cy,1,0,0); glTranslatef(-320, -240, 1000); glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D, texture_rgb); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImageMap); glBegin(GL_POINTS); for(unsigned int y=0; y<yres-1; y++) { for(unsigned int x=0; x<630; x++) { if(pDepthMap[x+y*xres]!=0) { glTexCoord2f(static_cast<float>(x)/static_cast<float>(630), static_cast<float>(y)/static_cast<float>(480)); glVertex3f(x, (yres-y), -pDepthMap[x+y*xres]/2.00); #ifdef DEBUGOUT datei << t_gamma[pDepthMap[x+y*xres]] << endl; #endif } } } glEnd(); glPopMatrix(); glDisable(GL_TEXTURE_2D); glutSwapBuffers(); #ifdef DEBUGOUT datei.close(); exit(-1); #endif }
cv::Point xncv::worldToProjective(const XnPoint3D& point, const xn::DepthGenerator& depth) { XnPoint3D p; depth.ConvertRealWorldToProjective(1, &point, &p); return cv::Point(static_cast<int>(p.X), static_cast<int>(p.Y)); }
int main(int argc, char **argv) { nRetVal = XN_STATUS_OK; /* Context initialisieren (Kameradaten) */ nRetVal = context.Init(); checkError("Fehler beim Initialisieren des Context", nRetVal)?0:exit(-1); /* Tiefengenerator erstellen */ nRetVal = depth.Create(context); checkError("Fehler beim Erstellen des Tiefengenerators", nRetVal)?0:exit(-1); /* Tiefengenerator einstellen */ XnMapOutputMode outputModeDepth; outputModeDepth.nXRes = 640; outputModeDepth.nYRes = 480; outputModeDepth.nFPS = 30; nRetVal = depth.SetMapOutputMode(outputModeDepth); checkError("Fehler beim Konfigurieren des Tiefengenerators", nRetVal)?0:exit(-1); /* Imagegenerator erstellen */ nRetVal = image.Create(context); checkError("Fehler beim Erstellen des Bildgenerators", nRetVal)?0:exit(-1); /* Imagegenerator einstellen */ XnMapOutputMode outputModeImage; outputModeImage.nXRes = 640; outputModeImage.nYRes = 480; outputModeImage.nFPS = 30; nRetVal = image.SetMapOutputMode(outputModeImage); checkError("Fehler beim Konfigurieren des Bildgenerators", nRetVal)?0:exit(-1); /* Starten der Generatoren - volle Kraft vorraus! */ nRetVal = context.StartGeneratingAll(); checkError("Fehler beim Starten der Generatoren", nRetVal)?0:exit(-1); /* Glut initialisieren */ glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); glutInitWindowSize(WINDOW_SIZE_X, WINDOW_SIZE_Y); glutInitWindowPosition(300,150); win = glutCreateWindow("kinect-head-tracking"); glClearColor(0, 0, 0, 0.0); //Hintergrundfarbe: Hier ein leichtes Blau glEnable(GL_DEPTH_TEST); //Tiefentest aktivieren glDepthFunc(GL_LEQUAL); // glEnable(GL_CULL_FACE); //Backface Culling aktivieren // glEnable(GL_ALPHA_TEST); // glAlphaFunc(GL_GEQUAL, 1); /* Texturen */ glGenTextures(1, &texture_rgb); glBindTexture(GL_TEXTURE_2D, texture_rgb); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP); glGenTextures(1, &texture_depth); glBindTexture(GL_TEXTURE_2D, texture_depth); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP); glutDisplayFunc(glut_display); glutIdleFunc(glut_idle); glutMouseFunc(glut_mouse); glutMotionFunc(glut_mouse_motion); glutKeyboardFunc(glut_keyboard); glutMainLoop(); return 0; }
int main(int argc, char **argv) { XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; if( USE_RECORED_DATA ){ g_Context.Init(); g_Context.OpenFileRecording(RECORD_FILE_PATH); xn::Player player; // Player nodeの取得 nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_PLAYER, player); CHECK_RC(nRetVal, "Find player"); LOG_D("PlaybackSpeed: %d", player.GetPlaybackSpeed()); xn:NodeInfoList nodeList; player.EnumerateNodes(nodeList); for( xn::NodeInfoList::Iterator it = nodeList.Begin(); it != nodeList.End(); ++it){ if( (*it).GetDescription().Type == XN_NODE_TYPE_IMAGE ){ nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator); CHECK_RC(nRetVal, "Find image node"); LOG_D("%s", "ImageGenerator created."); } else if( (*it).GetDescription().Type == XN_NODE_TYPE_DEPTH ){ nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth node"); LOG_D("%s", "DepthGenerator created."); } else{ LOG_D("%s %s %s", ::xnProductionNodeTypeToString((*it).GetDescription().Type ), (*it).GetInstanceName(), (*it).GetDescription().strName); } } } else{ LOG_I("Reading config from: '%s'", CONFIG_XML_PATH); nRetVal = g_Context.InitFromXmlFile(CONFIG_XML_PATH, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT){ XnChar strError[1024]; errors.ToString(strError, 1024); LOG_E("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK){ LOG_E("Open failed: %s", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal,"No depth"); // ImageGeneratorの作成 nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator); CHECK_RC(nRetVal, "Find image generator"); } // UserGeneratorの取得 nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if(nRetVal!=XN_STATUS_OK){ nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Create user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)){ LOG_E("%s", "Supplied user generator doesn't support skeleton"); return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); g_SkeletonCap = g_UserGenerator.GetSkeletonCap(); nRetVal = g_SkeletonCap.RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_SkeletonCap.RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); if (g_SkeletonCap.NeedPoseForCalibration()){ g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)){ LOG_E("%s", "Pose required, but not supported"); return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_SkeletonCap.GetCalibrationPose(g_strPose); } g_SkeletonCap.SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); // 表示用の画像データの作成 XnMapOutputMode mapMode; g_ImageGenerator.GetMapOutputMode(mapMode); g_rgbImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3); LOG_I("%s", "Starting to run"); if(g_bNeedPose){ LOG_I("%s", "Assume calibration pose"); } xn::Recorder recorder; if( DO_RECORED && !USE_RECORED_DATA ){ // レコーダーの作成 LOG_I("%s", "Setup Recorder"); nRetVal = recorder.Create(g_Context); CHECK_RC(nRetVal, "Create recorder"); // 保存設定 nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, RECORD_FILE_PATH); CHECK_RC(nRetVal, "Set recorder destination file"); // 深度、ビデオカメラ入力を保存対象として記録開始 nRetVal = recorder.AddNodeToRecording(g_DepthGenerator, XN_CODEC_NULL); CHECK_RC(nRetVal, "Add depth node to recording"); nRetVal = recorder.AddNodeToRecording(g_ImageGenerator, XN_CODEC_NULL); CHECK_RC(nRetVal, "Add image node to recording"); LOG_I("%s", "Recorder setup done."); } while (!xnOSWasKeyboardHit()) { g_Context.WaitOneUpdateAll(g_UserGenerator); if( DO_RECORED && !USE_RECORED_DATA ){ nRetVal = recorder.Record(); CHECK_RC(nRetVal, "Record"); } // ビデオカメラ画像の生データを取得 xn::ImageMetaData imageMetaData; g_ImageGenerator.GetMetaData(imageMetaData); // メモリコピー xnOSMemCopy(g_rgbImage->imageData, imageMetaData.RGB24Data(), g_rgbImage->imageSize); // BGRからRGBに変換して表示 cvCvtColor(g_rgbImage, g_rgbImage, CV_RGB2BGR); // UserGeneratorからユーザー識別ピクセルを取得 xn::SceneMetaData sceneMetaData; g_UserGenerator.GetUserPixels(0, sceneMetaData); XnUserID allUsers[MAX_NUM_USERS]; XnUInt16 nUsers = MAX_NUM_USERS; g_UserGenerator.GetUsers(allUsers, nUsers); for (int i = 0; i < nUsers; i++) { // キャリブレーションに成功しているかどうか if (g_SkeletonCap.IsTracking(allUsers[i])) { // スケルトンを描画 DrawSkelton(allUsers[i], i); } } // 表示 cvShowImage("User View", g_rgbImage); // ESCもしくはqが押されたら終了させる if (cvWaitKey(10) == 27) { break; } } if( !USE_RECORED_DATA ){ g_scriptNode.Release(); } g_DepthGenerator.Release(); g_UserGenerator.Release(); g_Context.Release(); if (g_rgbImage != NULL) { cvReleaseImage(&g_rgbImage); } g_Context.Shutdown(); }
// 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 }
int main(int argc, char **argv) { XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; TotalFrames = 0; static struct rusage ru; long double t; double w; struct timeval timStart; struct timeval timEnd; struct timeval Wall; bool Sample = true; XnFPSData xnFPS; XnUserID aUsers[MAX_NUM_USERS]; XnUInt16 nUsers; XnSkeletonJointTransformation torsoJoint; if (argc > 1) { //parse the cmd line if (strcasecmp(argv[1],"--help") == 0) { PrintHelpHeader(argv[0]); return 1; } numOfUser = atoi(argv[1]); if(numOfUser == 0) { PrintHelpHeader(argv[0]); return 1; } else if(numOfUser > 2) { printf("Maximal Users allowed is 2\n"); return 1; } } else { numOfUser = 4; } const char *fn = NULL; if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH; else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL; else { printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL); return XN_STATUS_ERROR; } printf("Reading config from: '%s'\n", fn); nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal,"No depth"); #if (XN_PLATFORM != XN_PLATFORM_MACOSX) //we want out benchmark application will be running only on one CPU core cpu_set_t mask; CPU_ZERO(&mask); CPU_SET(1,&mask); sched_setaffinity(0,sizeof(mask),&mask); #endif //initialize the FPS calculator nRetVal = xnFPSInit(&xnFPS, 90); CHECK_RC(nRetVal, "FPS Init"); //ensure the User generator exists nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } //register to generators callbacks XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); //ensure calibration if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } //set skeleton profile (all joints) g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); //start to generate nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); printf("%c[%d;%d;%dmPrimeSense Skeleton Benchmark Application\n ", 0x1B, BRIGHT,BLUE,BG_BLACK); printf("%c[%d;%d;%dmSet Maximal users to %d ", 0x1B, BRIGHT,BLUE,BG_BLACK,numOfUser); printf("%c[%dm\n", 0x1B, 0); printf("Starting to run\n"); if(g_bNeedPose) { printf("Assume calibration pose\n"); } XnUInt32 epochTime = 0; //each 30 frames (1 second) we start the CPU resources usages while (!xnOSWasKeyboardHit()) { if (Sample) { //get the beginning sample of CPU resources getrusage(RUSAGE_SELF, &ru); timStart=ru.ru_utime; t=(double)timStart.tv_sec * 1000000.0 + (double)timStart.tv_usec \ + (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec; //get the wall clock time gettimeofday(&Wall,NULL); w=(double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec; Sample = false; } g_Context.WaitOneUpdateAll(g_UserGenerator); xnFPSMarkFrame(&xnFPS); // print the torso information for the first user already tracking every 1 second to prevent CPU of printf if(TotalFrames % 30 == 0) { nUsers=MAX_NUM_USERS; g_UserGenerator.GetUsers(aUsers, nUsers); int numTracked=0; int userToPrint=-1; for(XnUInt16 i=0; i<nUsers; i++) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE) continue; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint); printf("User %d Located At distance of %6.2f mm from the sensor\n",aUsers[i],torsoJoint.position.position.Z); } //get the finish sample of the CPU resources getrusage(RUSAGE_SELF, &ru); timEnd=ru.ru_utime; t = (double)timEnd.tv_sec * 1000000.0 + (double)timEnd.tv_usec \ + (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec - t; //get the wall clock gettimeofday(&Wall,NULL); w = (double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec - w; XnDouble fps=xnFPSCalc(&xnFPS); //print stuff. printf("%c[%d;%d;%dmCPU Utilization=%3.2f%%\t", 0x1B, BRIGHT,RED,BG_BLACK,(double)100*t/w); printf("%c[%d;%d;%dmFPS=%3.2f ", 0x1B, BRIGHT,RED,BG_BLACK,(double)fps); printf("%c[%dm\n", 0x1B, 0); Sample= true; } TotalFrames++; } g_scriptNode.Release(); g_DepthGenerator.Release(); g_UserGenerator.Release(); g_Context.Release(); }
int main(int argc, char **argv) { XnStatus rc = XN_STATUS_OK; rc = g_Context.InitFromXmlFile(SAMPLE_XML_PATH); CHECK_RC(rc, "InitFromXml"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(rc, "Find depth generator"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator); CHECK_RC(rc, "Find Image generator"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); CHECK_RC(rc, "Find user generator"); // Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator); //g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint(); if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON) || !g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("User generator doesn't support either skeleton or pose detection.\n"); return XN_STATUS_ERROR; } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); rc = g_Context.StartGeneratingAll(); CHECK_RC(rc, "StartGenerating"); XnCallbackHandle hUserCBs, hCalibrationCBs, hPoseCBs; g_UserGenerator.RegisterUserCallbacks(NewUser, LostUser, NULL, hUserCBs); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(CalibrationStarted, CalibrationEnded, NULL, hCalibrationCBs); g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(PoseDetected, NULL, NULL, hPoseCBs); #ifdef USE_GLUT // Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator); //g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint(); glInit(&argc, argv); glutMainLoop(); #else if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context)) { printf("Error initing opengles\n"); CleanupExit(); } glDisable(GL_DEPTH_TEST); // glEnable(GL_TEXTURE_2D); glEnableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_COLOR_ARRAY); while ((!_kbhit()) && (!g_bQuit)) { glutDisplay(); eglSwapBuffers(display, surface); } opengles_shutdown(display, surface, context); CleanupExit(); #endif }