void Loop(void) { XnStatus nRetVal = XN_STATUS_OK; while (g_notDone) { if ((nRetVal = g_context.WaitOneUpdateAll(g_depth)) != XN_STATUS_OK) //if ((nRetVal = g_context.WaitAndUpdateAll()) != XN_STATUS_OK) { fprintf(stderr,"Could not update data: %s\n", xnGetStatusString(nRetVal)); continue; } if (g_haveDepth) { const XnDepthPixel* pDepthMap = g_depth.GetDepthMap(); ProcessDepthFrame(pDepthMap, g_depthWidth, g_depthHeight); FindFingertip(); } if (g_haveImage) { const XnRGB24Pixel* pImageMap = g_image.GetRGB24ImageMap(); ProcessImageFrame(pImageMap, g_depthWidth, g_depthHeight); } ShowFrame(); CheckKeys(); } }
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 }
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(); }
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; }