// ----------------------------------------------------------------------------------------------------- // generateFrame // ----------------------------------------------------------------------------------------------------- bool CameraDevice::generateFrame(IplImage* imgRGB, IplImage* imgDepth) { XnStatus nRetVal = XN_STATUS_OK; const XnDepthPixel* pDepthMap = NULL; const XnRGB24Pixel* pImageMap = NULL; xnFPSMarkFrame(&g_xnFPS); nRetVal = g_context.WaitAndUpdateAll(); if (nRetVal==XN_STATUS_OK) { g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); pDepthMap = g_depthMD.Data(); pImageMap = g_image.GetRGB24ImageMap(); printf("Frame %02d (%dx%d) Depth at middle point: %u. FPS: %f\r", g_depthMD.FrameID(), g_depthMD.XRes(), g_depthMD.YRes(), g_depthMD(g_depthMD.XRes()/2, g_depthMD.YRes()/2), xnFPSCalc(&g_xnFPS)); // convert to OpenCV buffers convertImageRGB(pImageMap, imgRGB); convertImageDepth(pDepthMap, imgDepth); return true; } return false; }
XnStatus prepare(char useScene, char useDepth, char useHistogram) { //TODO handle possible failures! if (useDepth) { mDepthGen.GetMetaData(depthMD); nXRes = depthMD.XRes(); nYRes = depthMD.YRes(); pDepth = depthMD.Data(); if (useHistogram) { calcHist(); // rewind the pointer pDepth = depthMD.Data(); } } if (useScene) { mUserGen.GetUserPixels(0, sceneMD); nXRes = sceneMD.XRes(); nYRes = sceneMD.YRes(); pLabels = sceneMD.Data(); } }
int main() { XnStatus nRetVal = XN_STATUS_OK; Context context; nRetVal = context.Init(); CHECK_RC(nRetVal, "Initialize context"); DepthGenerator depth; nRetVal = depth.Create(context); CHECK_RC(nRetVal, "Create depth generator"); nRetVal = context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGeneratingAll"); DepthMetaData depthMD; while (!xnOSWasKeyboardHit()) { nRetVal = context.WaitOneUpdateAll(depth); if (nRetVal != XN_STATUS_OK) { printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal)); continue; } depth.GetMetaData(depthMD); const XnDepthPixel* pDepthMap = depthMD.Data(); printf("Frame %d Middle point is: %u.\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2)); } context.Shutdown(); return 0; }
void GeneratePointCloud(DepthGenerator& rDepthGen, const XnDepthPixel* pDepth, VISION_DATA &pData) { DepthMetaData mDepthMD; rDepthGen.GetMetaData(mDepthMD); pData.timeStamp = mDepthMD.Timestamp(); unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes(); XnPoint3D* pDepthPointSet = new XnPoint3D[uPointNum]; unsigned int i, j, idxshift, idx; for( j = 0; j < mDepthMD.FullYRes(); ++j) { idxshift = j * mDepthMD.FullXRes(); for(i = 0; i < mDepthMD.FullXRes(); ++i) { idx = idxshift + i; pDepthPointSet[idx].X = i; pDepthPointSet[idx].Y = j; pDepthPointSet[idx].Z = pDepth[idx]; } } XnPoint3D* p3DPointSet = new XnPoint3D[uPointNum]; rDepthGen.ConvertProjectiveToRealWorld(uPointNum, pDepthPointSet, p3DPointSet); memcpy(pData.pointCloud, p3DPointSet, uPointNum*3*sizeof(float)); delete[] pDepthPointSet; delete[] p3DPointSet; }
// Updates to the latest image obtained from the Kinect int kinectUpdate(void) { XnStatus nRetVal = context.WaitAndUpdateAll(); g_image.GetMetaData(g_imageMD); //nRetVal = context.WaitOneUpdateAll(depth); depth.GetMetaData(depthMD); return nRetVal; }
int main() { XnStatus nRetVal = XN_STATUS_OK; Context context; EnumerationErrors errors; nRetVal = 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); } DepthGenerator depth; nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(nRetVal, "Find depth generator"); XnFPSData xnFPS; nRetVal = xnFPSInit(&xnFPS, 180); CHECK_RC(nRetVal, "FPS Init"); DepthMetaData depthMD; while (!xnOSWasKeyboardHit()) { nRetVal = context.WaitOneUpdateAll(depth); if (nRetVal != XN_STATUS_OK) { printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal)); continue; } xnFPSMarkFrame(&xnFPS); depth.GetMetaData(depthMD); const XnDepthPixel* pDepthMap = depthMD.Data(); printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS)); } context.Shutdown(); return 0; }
// Gets the colour and depth data from the Kinect sensor. bool GetColorAndDepthImages(ColorImage& colorImage, DepthImage& depthImage) { XnStatus rc = XN_STATUS_OK; // Read a new frame, blocking operation rc = deviceContext.WaitAnyUpdateAll(); if (rc != XN_STATUS_OK) { /*LOGE("Read failed: %s\n", xnGetStatusString(rc));*/ throw rc; } // Get handles to new data static ImageMetaData colorImageMetaData; static DepthMetaData depthImageMetaData; colorImageGenerator.GetMetaData(colorImageMetaData); depthImageGenerator.GetMetaData(depthImageMetaData); // Validate images if (!depthImageGenerator.IsValid() || !colorImageGenerator.IsValid()) { /*LOGE("Error: Color or depth image is invalid.");*/ throw 1; } if (colorImageMetaData.Timestamp() <= mostRecentRGB) return false; // Fetch pointers to data const XnRGB24Pixel* pColorImage = colorImageMetaData.RGB24Data(); //g_depth.GetRGB24ImageMap() const XnDepthPixel* pDepthImage = depthImageMetaData.Data();// g_depth.GetDepthMap(); // Copy data over to arrays memcpy(colorImage.data, pColorImage, sizeof(colorImage.data)); memcpy(depthImage.data, pDepthImage, sizeof(depthImage.data)); colorImage.rows = colorImage.maxRows; colorImage.cols = colorImage.maxCols; depthImage.rows = depthImage.maxRows; depthImage.cols = depthImage.maxCols; mostRecentRGB = colorImageMetaData.Timestamp(); return true; }
void glutDisplay (void){ glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); SceneMetaData sceneMD; DepthMetaData depthMD; ImageMetaData imageMD; g_DepthGenerator.GetMetaData(depthMD); glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); glDisable(GL_TEXTURE_2D); //XnStatus rc = g_Context.WaitOneUpdateAll(g_DepthGenerator); XnStatus rc = g_Context.WaitAnyUpdateAll(); CHECK_RC("Wait Data",rc); g_DepthGenerator.GetMetaData(depthMD); if(g_UserGenerator.IsValid()) g_UserGenerator.GetUserPixels(0, sceneMD); g_ImageGenerator.GetMetaData(imageMD); DrawDepthMap(depthMD, sceneMD); DrawImage(imageMD); glutSwapBuffers(); }//glutdisplay
XnStatus prepare(char useScene, char useDepth, char useImage, char useIr, char useHistogram) { //TODO handle possible failures! Gotcha! if (useDepth) { mDepthGen.GetMetaData(depthMD); nXRes = depthMD.XRes(); nYRes = depthMD.YRes(); pDepth = depthMD.Data(); if (useHistogram) { calcHist(); // rewind the pointer pDepth = depthMD.Data(); } } if (useScene) { mUserGen.GetUserPixels(0, sceneMD); nXRes = sceneMD.XRes(); nYRes = sceneMD.YRes(); pLabels = sceneMD.Data(); } if (useImage) { mImageGen.GetMetaData(imageMD); nXRes = imageMD.XRes(); nYRes = imageMD.YRes(); pRGB = imageMD.RGB24Data(); // HISTOGRAM????? } if (useIr) { mIrGen.GetMetaData(irMD); nXRes = irMD.XRes(); nYRes = irMD.YRes(); pIR = irMD.Data(); // HISTOGRAM???? } }
/* * Class: org_OpenNI_Samples_Assistant_NativeMethods * Method: initFromContext * Signature: (JZZ)I */ JNIEXPORT jint JNICALL Java_org_OpenNI_Samples_Assistant_NativeMethods_initFromContext (JNIEnv *env, jclass cls, jlong pContext, jboolean _hasUserGen, jboolean _hasDepthGen) { LOGD("init_start"); hasUserGen = _hasUserGen; hasDepthGen = _hasDepthGen; mContext = new Context((XnContext*) pContext); if (!(hasUserGen || hasDepthGen)) return XN_STATUS_BAD_PARAM; int rc; if (hasUserGen) { rc = mContext->FindExistingNode(XN_NODE_TYPE_USER, mUserGen); if (rc != XN_STATUS_OK) { //TODO log&retval printf("No user node exists!"); return 1; } mUserGen.GetUserPixels(0, sceneMD); } if (hasDepthGen) { rc = mContext->FindExistingNode(XN_NODE_TYPE_DEPTH, mDepthGen); if (rc != XN_STATUS_OK) { //TODO log&retval printf("No depth node exists! Check your XML."); return 1; } mDepthGen.GetMetaData(depthMD); } initGraphics(); LOGD("init_end"); return XN_STATUS_OK; }
void takePhoto() { static int index = 1; char fname[256] = {0,}; sprintf(fname, "kinect%03d.txt", index++); g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); int const nx = g_depthMD.XRes(); int const ny = g_depthMD.YRes(); assert(nx == g_imageMD.XRes()); assert(ny == g_imageMD.YRes()); const XnDepthPixel* pDepth = g_depthMD.Data(); const XnUInt8* pImage = g_imageMD.Data(); FILE * file = fopen(fname, "wb"); fprintf(file, "%d\n%d\n\n", nx, ny); for (int y = 0, di = 0, ri = 0, gi = 1, bi = 2; y < ny; y++) { for (int x = 0; x < nx; x++, di++, ri += 3, gi += 3, bi += 3) { int const r = pImage[ri]; int const g = pImage[gi]; int const b = pImage[bi]; int const d = pDepth[di]; assert(r >= 0); assert(g >= 0); assert(b >= 0); assert(d >= 0); assert(r <= 0xFF); assert(g <= 0xFF); assert(b <= 0xFF); assert(d <= 0xFFFF); fprintf(file, "%3d %3d %3d %5d\n", r, g, b, d); } fprintf(file, "\n"); } fflush(file); fclose(file); }
/* * Function: run * * Starts and continues generating data from the Kinect. * A loop runs and updates data whenever new data is available from one of the Kinect * devices, and then the data is processed to check for patient movement. * The loop is controlled by the "quit" global boolean, which is set to false by the * signal handler "stop()" */ void KinectMonitor::run() { XnStatus status; SceneMetaData scene; DepthMetaData depth; // Start the device status = context.StartGeneratingAll(); // Running loop while( !quit ) { // Wait for any new incoming data context.WaitOneUpdateAll(depthGenerator); // Mark the new frame xnFPSMarkFrame(&xnFPS); // Get the depth data from the device depthGenerator.GetMetaData(depth); // Get the recognized users XnUInt16 numUsers = 15; XnUserID users[numUsers]; userGenerator.GetUsers(users, numUsers); // Only track the patient if they are alone if( numUsers != 1) continue; // Get the user data userGenerator.GetUserPixels(users[0], scene); // Update patient position previous = current; current = getPosition(users[0]); // Raise alerts based on the patient's state and position if( previous != current ) { if( current == TURNED && out == false ) { // Patient is turned printf("Patient getting out of bed.\n"); } else if( out && bedSet ) { printf("Patient is out of bed.\n"); } } } }
void readFrame() { if (!g_Depth.IsValid() && !g_Image.IsValid() && !g_IR.IsValid() && !g_Audio.IsValid()) // @@@dded return; XnStatus rc = XN_STATUS_OK; if (g_pPrimary != NULL) { rc = g_Context.WaitOneUpdateAll(*g_pPrimary); } else { rc = g_Context.WaitAnyUpdateAll(); } if (rc != XN_STATUS_OK) { printf("Error: %s\n", xnGetStatusString(rc)); } if (g_Depth.IsValid()) { g_Depth.GetMetaData(g_DepthMD); } if (g_Image.IsValid()) { g_Image.GetMetaData(g_ImageMD); } if (g_IR.IsValid()) { g_IR.GetMetaData(g_irMD); } if (g_Audio.IsValid()) { g_Audio.GetMetaData(g_AudioMD); } }
// Set up OpenNI to obtain 8-bit mono images from the Kinect's RGB camera int kinectInit(void) { XnStatus nRetVal = XN_STATUS_OK; ScriptNode scriptNode; EnumerationErrors errors; printf("Reading config from: '%s'\n", SAMPLE_XML_PATH_LOCAL); nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH_LOCAL, scriptNode, &errors); nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); //g_image.SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT); g_image.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); g_image.GetMetaData(g_imageMD); nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); depth.GetMetaData(depthMD); // nRetVal = depth.GetAlternativeViewPointCap().SetViewPoint(g_image); //nRetVal = depth.GetFrameSyncCap().FrameSyncWith(g_image); return nRetVal; }
//-------------------------------------------------------------- void testApp::setup(){ XnStatus rc; EnumerationErrors errors; rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, g_scriptNode, &errors); if (rc == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return ; } else if (rc != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(rc)); return; } rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth); if (rc != XN_STATUS_OK) { printf("No depth node exists! Check your XML."); return; } g_depth.GetMetaData(g_depthMD); // Texture map init g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512; g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512; g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel)); std::cout << " w:" << g_depthMD.FullXRes() << " h:" << g_depthMD.FullYRes() << std::endl; pixels = (unsigned char*)malloc(640*480*3*sizeof(unsigned char)); tex.allocate(640, 480, GL_RGB); }
XnStatus Init_Kinect(EventOutSFNode* skltn,EventOutSFNode* hnz,EventOutSFNode* flr){ XnStatus rc=XN_STATUS_OK; EnumerationErrors errors; DepthMetaData g_depthMD; ImageMetaData g_imageMD; rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors); CHECK_RC(rc, "InitFromXml"); rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth); CHECK_RC(rc,"XN_NODE_TYPE_DEPTH"); rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); CHECK_RC(rc,"XN_NODE_TYPE_IMAGE"); rc= g_context.FindExistingNode(XN_NODE_TYPE_USER,g_user); CHECK_RC(rc,"XN_NODE_TYPE_USER"); rc=g_context.FindExistingNode(XN_NODE_TYPE_SCENE,g_scene); CHECK_RC(rc,"XN_NODE_TYPE_SCENE"); rc=g_context.FindExistingNode(XN_NODE_TYPE_HANDS,g_hands); CHECK_RC(rc,"XN_NODE_TYPE_HANDS"); rc=g_context.FindExistingNode(XN_NODE_TYPE_GESTURE,g_gesture); CHECK_RC(rc,"XN_NODE_TYPE_GESTURE"); g_depth.GetMetaData(g_depthMD); g_fps=g_depthMD.FPS(); g_image.GetMetaData(g_imageMD); rc=init_skeleton(); CHECK_RC(rc,"INIT SKELETON"); rc=init_hands(); CHECK_RC(rc,"INIT HANDS"); pix_w=g_depthMD.FullXRes(); pix_h=g_depthMD.FullYRes(); if(pix_h==0||pix_w==0){return XN_STATUS_ERROR;} g_skltn=skltn; g_hnz=hnz; g_flr=flr; if(NULL==g_skltn||NULL==g_hands||NULL==g_flr)return XN_STATUS_ERROR; isInit=true; return rc; }
int main(int argc, char* argv[]) { EnumerationErrors errors; //rc = context.Init(); rc = context.InitFromXmlFile(strPathToXML,&errors); if (rc == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (rc); } else if (rc != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(rc)); return (rc); } /* UNCOMMENT TO GET FILE READING //rc = context.OpenFileRecording(strInputFile); //CHECK_RC(rc, "Open input file"); //rc = context.FindExistingNode(XN_NODE_TYPE_PLAYER, player); //CHECK_RC(rc, "Get player node"); */ rc = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(rc, "Find depth generator"); rc = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image); CHECK_RC(rc, "Find image generator"); depth.GetMetaData(depthMD); image.GetMetaData(imageMD); //rc = player.SetRepeat(FALSE); XN_IS_STATUS_OK(rc); //rc = player.GetNumFrames(image.GetName(), nNumFrames); //CHECK_RC(rc, "Get player number of frames"); //printf("%d\n",nNumFrames); //rc = player.GetNumFrames(depth.GetName(), nNumFrames); //CHECK_RC(rc, "Get player number of frames"); //printf("%d\n",nNumFrames); // Hybrid mode isn't supported if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes()) { printf ("The device depth and image resolution must be equal!\n"); return 1; } // RGB is the only image format supported. if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { printf("The device image format must be RGB24\n"); return 1; } avi = cvCreateVideoWriter(strOutputFile, 0, 30, cvSize(640,480), TRUE); depthMetersMat = cvCreateMat(480, 640, CV_16UC1); kinectDepthImage = cvCreateImage( cvSize(640,480),16,1 ); depthMetersMat2 = cvCreateMat(480, 640, CV_16UC1); kinectDepthImage2 = cvCreateImage( cvSize(640,480),16,1 ); colorArr[0] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U); colorArr[1] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U); colorArr[2] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U); //prepare_for_face_detection(); int b; int g; int r; while ((rc = image.WaitAndUpdateData()) != XN_STATUS_EOF && (rc = depth.WaitAndUpdateData()) != XN_STATUS_EOF) { if (rc != XN_STATUS_OK) { printf("Read failed: %s\n", xnGetStatusString(rc)); break; } depth.GetMetaData(depthMD); image.GetMetaData(imageMD); //XnUInt32 a; //a = g_imageMD.FPS; printf("%d\n",imageMD.FrameID()); //a = g_depthMD.DataSize(); //printf("%d\n",a); pDepth = depthMD.Data(); pImageRow = imageMD.RGB24Data(); for (unsigned int y=0; y<imageMD.YRes(); y++) { pPixel = pImageRow; uchar* Bptr = colorArr[0].ptr<uchar>(y); uchar* Gptr = colorArr[1].ptr<uchar>(y); uchar* Rptr = colorArr[2].ptr<uchar>(y); for(unsigned int x=0;x<imageMD.XRes();++x , ++pPixel){ Bptr[x] = pPixel->nBlue; Gptr[x] = pPixel->nGreen; Rptr[x] = pPixel->nRed; depthMetersMat->data.s[y * XN_VGA_X_RES + x ] = 7*pDepth[y * XN_VGA_X_RES + x]; depthMetersMat2->data.s[y * XN_VGA_X_RES + x ] = pDepth[y * XN_VGA_X_RES + x]; } pImageRow += imageMD.XRes(); } cv::merge(colorArr,3,colorImage); iplImage = colorImage; //cvThreshold(depthMetersMat2, depthMetersMat2, 150, 1500, THRESH_BINARY); cvGetImage(depthMetersMat,kinectDepthImage); cvGetImage(depthMetersMat2,kinectDepthImage2); depthImage = Bw2Image(kinectDepthImage2); printf("1. Middle pixel is %u millimeters away\n",depthImage[240][320]); rgbImage = RgbImage(&iplImage); // we want to see on up to 2000 MM int THRESH = 2000; for (unsigned int y=0; y<imageMD.YRes(); y++) { for(unsigned int x=0;x<imageMD.XRes();++x){ if ( depthImage[y][x] >= THRESH ) { depthImage[y][x] = 0; } else { float tmp = depthImage[y][x]; tmp = tmp / THRESH * (65536)*(-1) + 65536; depthImage[y][x] = (unsigned int)tmp; } } } // THE PART ABOUT FILTERING COLOURS IN HSV TO SEE ONLY SPECIFIC ONE // AFTER ONE FEW MORPHOLOGICAL OPERATIONS TO MAKE IT LOOK BETTER IplImage* imgHSV = cvCreateImage(cvGetSize(&iplImage), 8, 3); cvCvtColor(&iplImage, imgHSV, CV_BGR2HSV); imgThreshed = cvCreateImage(cvGetSize(&iplImage), 8, 1); //cvInRangeS(imgHSV, cvScalar(100, 60, 80), cvScalar(110, 255, 255), imgThreshed); // BLUE cvInRangeS(imgHSV, cvScalar(29, 95, 95), cvScalar(35, 255, 255), imgThreshed); // YELLOW //cvInRangeS(imgHSV, cvScalar(29, 60, 60), cvScalar(35, 255, 255), imgThreshed); // YELLOW DARK //cvInRangeS(imgHSV, cvScalar(150, 70, 70), cvScalar(160, 255, 255), imgThreshed); // PINK //cvInRangeS(imgHSV, cvScalar(40, 76, 76), cvScalar(70, 255, 255), imgThreshed); // GREEN IplConvKernel* kernel = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT, NULL); //cvDilate(imgThreshed,imgThreshed,kernel); //cvErode(imgThreshed,imgThreshed,kernel); Mat mat = Mat(imgThreshed); blur(Mat(imgThreshed),mat,cvSize(3,3)); imgThreshed = &IplImage(mat); //cvInRangeS(imgThreshed,cvScalar(100),cvScalar(255),imgThreshed); //cvErode(imgThreshed,imgThreshed,kernel); cvDilate(imgThreshed,imgThreshed,kernel); cvDilate(imgThreshed,imgThreshed,kernel); cvErode(imgThreshed,imgThreshed,kernel); cvErode(imgThreshed,imgThreshed,kernel); mat = Mat(imgThreshed); blur(Mat(imgThreshed),mat,cvSize(6,6)); imgThreshed = &IplImage(mat); cvInRangeS(imgThreshed,cvScalar(100),cvScalar(255),imgThreshed); cvReleaseImage(&imgHSV); BwImage threshed = BwImage(imgThreshed); if ( initialize == true ) { normalizeReferenceFace(); int currentID = 0; for ( int y = 30; y<480; y++ ) { for ( int x = 30; x<640; x++ ) { bool g2g = true; //printf("%d %d %d\n",ID, y,x); if ( threshed[y][x]!=0 ) { for ( int ID2 = 0; ID2<nbOfPoints; ID2++) { if ( (abs(markers[ID2].y-y)<proximityLimit) && (abs(markers[ID2].x-x)<proximityLimit)) { g2g = false; } } if (currentID >= nbOfPoints || g2g == false ) { break; } markers[currentID].y=y; markers[currentID].x=x; currentID++; printf("WHITE PIXEL INITIALIZED %d: %d %d\n",currentID, x,y); } } } if (isDebugConf==true || currentID == nbOfMarkers) { printf("%d PIXELS INITIALIZED\n", currentID); initialize = false; //printf("%d,%d\n", currentID, nbOfPoints); //return 0; } else { printf("WAITING FOR %d PIXELS TO APPEAR, %d SO FAR \n",nbOfMarkers, currentID); continue; } // FIND TOP RIGHT AND CHIN PIXEL int refPixID = 0; int chinPixID = 0; for ( int i = 0; i < nbOfMarkers; i++) { if ( (markers[i].x + markers[i].y)*(markers[i].x + markers[i].y) < (markers[refPixID].x + markers[refPixID].y)* (markers[refPixID].x + markers[refPixID].y)) { refPixID = i; } if (markers[i].y > markers[chinPixID].y) { chinPixID = i; } } float width = (markers[1].x-markers[0].x)*2; float heigth = abs(markers[1].y-markers[0].y); // WE GOT WIDTH & HEIGTH OF THE FACE, LETS ADJUST POINTS // SET 0 to REF, SET 1 to CHIN MyPoint tmp = MyPoint(markers[refPixID].x,markers[refPixID].y); markers[refPixID].x = markers[0].x; markers[refPixID].y = markers[0].y; markers[0].x = tmp.x; markers[0].y = tmp.y; tmp = MyPoint(markers[chinPixID].x,markers[chinPixID].y); markers[chinPixID].x = markers[1].x; markers[chinPixID].y = markers[1].y; markers[1].x = tmp.x; markers[1].y = tmp.y; // REST OF THE POINTS for ( int i = 2; i < nbOfPoints; i++) { int cost = 0; int lowestCost = 0; int closestPixID = -1; for ( int j = 2; j < nbOfMarkers; j++ ) { cost = (markers[j].x-points[i].x*width)*(markers[j].x-points[i].x*width) + (markers[j].y-points[i].y*heigth)*(markers[j].y-points[i].y*heigth); if ( cost < lowestCost ) { lowestCost = cost; closestPixID = j; } if (closestPixID == -1) { //printf("COS JEST SPORO NIE W PORZADKU, CHECK HERE\n"); break; } tmp.x = markers[i].x; tmp.y = markers[i].y; markers[i].x=markers[closestPixID].x; markers[i].x=markers[closestPixID].y; markers[closestPixID].x = tmp.x; markers[closestPixID].y = tmp.y; } } } for ( int currentPixelID = 0; currentPixelID < nbOfMarkers; currentPixelID++) { if (markers[currentPixelID].x == 0) { continue; } if ( threshed[markers[currentPixelID].y][markers[currentPixelID].x] < 128 ) { printf("PIXEL %d LOST\n",currentPixelID); for ( int neighbSize = 2; neighbSize < maxNeighbSize; neighbSize = neighbSize + 2 ) { int x1 = markers[currentPixelID].x - neighbSize/2; if ( x1 < intoDepthX(0) ) { x1 = (int)intoDepthX(0); } int y1 = (int)(markers[currentPixelID].y-neighbSize/2); if ( y1 < intoDepthY(0) ) { y1 = intoDepthY(0); } int y2 = markers[currentPixelID].y+neighbSize/2; if ( y2 > intoDepthY(480) ) { y2 = intoDepthY(480); } int x2 = markers[currentPixelID].x+neighbSize/2; if ( x2 > intoDepthX(640) ) { y2 = intoDepthX(640); } bool found = false; for ( int y = y1; y < y2; y++) { for ( int x = x1; x < x2; x++) { bool g2g = true; if (threshed[y][x] > 128) { for ( int ID2 = 0; ID2<nbOfMarkers; ID2++) { if ( currentPixelID == ID2 ) continue; if ( (abs(markers[ID2].y-y)<proximityLimit) && (abs(markers[ID2].x-x)<proximityLimit)) { g2g = false; break; } } if ( g2g ) { markers[currentPixelID].x = x; markers[currentPixelID].y = y; found = true; printf("Pixel %d, FOUND\n",currentPixelID); break; } } } if (found == true ) { break; } } if (found == true ) { break; } } } paintMarkerOnBoth(markers[currentPixelID]); } faceImage = cvCreateImage(cvGetSize(&iplImage), 8, 1); paintFace(); // normal kinect depth cvShowImage("Depth_Kinect", kinectDepthImage); // depth within 80 - 200 mm, normalized cvShowImage("Depth_Kinect_2", kinectDepthImage2); // rgb with tracking points cvShowImage("RGB_Kinect", &iplImage); // colour detector cvShowImage("RGB_Threshed", imgThreshed); // attempt to draw a face cvShowImage("Face Image", faceImage); cvWaitKey(50); // wait 20 ms if ( avi == NULL) { printf ("dupa%d \n",1); } //cvWriteFrame (avi, &iplImage); } // cvReleaseImageHeader(kinectDepthImage); cvReleaseVideoWriter(&avi); // cvReleaseHaarClassifierCascade( &cascade ); context.Shutdown(); return 0; }
void glutDisplay (void) { XnStatus rc = XN_STATUS_OK; // Read a new frame rc = g_context.WaitAnyUpdateAll(); if (rc != XN_STATUS_OK) { printf("Read failed: %s\n", xnGetStatusString(rc)); return; } g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); const XnDepthPixel* pDepth = g_depthMD.Data(); const XnUInt8* pImage = g_imageMD.Data(); unsigned int nImageScale = GL_WIN_SIZE_X / g_depthMD.FullXRes(); // Copied from SimpleViewer // Clear the OpenGL buffers glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0); // Calculate the accumulative histogram (the yellow display...) xnOSMemSet(g_pDepthHist, 0, MAX_DEPTH*sizeof(float)); unsigned int nNumberOfPoints = 0; for (XnUInt y = 0; y < g_depthMD.YRes(); ++y) { for (XnUInt x = 0; x < g_depthMD.XRes(); ++x, ++pDepth) { if (*pDepth != 0) { g_pDepthHist[*pDepth]++; nNumberOfPoints++; } } } for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints))); } } xnOSMemSet(g_pTexMap, 0, g_nTexMapX*g_nTexMapY*sizeof(XnRGB24Pixel)); // check if we need to draw image frame to texture if (g_nViewState == DISPLAY_MODE_OVERLAY || g_nViewState == DISPLAY_MODE_IMAGE) { const XnRGB24Pixel* pImageRow = g_imageMD.RGB24Data(); XnRGB24Pixel* pTexRow = g_pTexMap + g_imageMD.YOffset() * g_nTexMapX; for (XnUInt y = 0; y < g_imageMD.YRes(); ++y) { const XnRGB24Pixel* pImage = pImageRow; XnRGB24Pixel* pTex = pTexRow + g_imageMD.XOffset(); for (XnUInt x = 0; x < g_imageMD.XRes(); ++x, ++pImage, ++pTex) { *pTex = *pImage; } pImageRow += g_imageMD.XRes(); pTexRow += g_nTexMapX; } } // check if we need to draw depth frame to texture if (g_nViewState == DISPLAY_MODE_OVERLAY || g_nViewState == DISPLAY_MODE_DEPTH) { const XnDepthPixel* pDepthRow = g_depthMD.Data(); XnRGB24Pixel* pTexRow = g_pTexMap + g_depthMD.YOffset() * g_nTexMapX; for (XnUInt y = 0; y < g_depthMD.YRes(); ++y) { const XnDepthPixel* pDepth = pDepthRow; XnRGB24Pixel* pTex = pTexRow + g_depthMD.XOffset(); for (XnUInt x = 0; x < g_depthMD.XRes(); ++x, ++pDepth, ++pTex) { if (*pDepth != 0) { int nHistValue = g_pDepthHist[*pDepth]; pTex->nRed = nHistValue; pTex->nGreen = nHistValue; pTex->nBlue = 0; } } pDepthRow += g_depthMD.XRes(); pTexRow += g_nTexMapX; } } // Create the OpenGL texture map glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP_SGIS, GL_TRUE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, g_nTexMapX, g_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, g_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glBegin(GL_QUADS); int nXRes = g_depthMD.FullXRes(); int nYRes = g_depthMD.FullYRes(); // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)nXRes/(float)g_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)nXRes/(float)g_nTexMapX, (float)nYRes/(float)g_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)nYRes/(float)g_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); // Swap the OpenGL display buffers glutSwapBuffers(); }
int main(int argc, char* argv[]) { XnStatus nRetVal = XN_STATUS_OK; nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH); if (nRetVal != XN_STATUS_OK) { printf("Log couldn't be opened: %s. Running without log", xnGetStatusString(nRetVal)); } if (argc < 3) { printf("usage: %s <inputFile> <outputFile>\n", argv[0]); return -1; } const char* strInputFile = argv[1]; const char* strOutputFile = argv[2]; Context context; nRetVal = context.Init(); CHECK_RC(nRetVal, "Init"); // open input file Player player; nRetVal = context.OpenFileRecording(strInputFile, player); CHECK_RC(nRetVal, "Open input file"); // Get depth node from recording DepthGenerator depth; nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(nRetVal, "Find depth generator"); // Create mock node based on depth node from recording MockDepthGenerator mockDepth; nRetVal = mockDepth.CreateBasedOn(depth); CHECK_RC(nRetVal, "Create mock depth node"); // create recorder Recorder recorder; nRetVal = recorder.Create(context); CHECK_RC(nRetVal, "Create recorder"); nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, strOutputFile); CHECK_RC(nRetVal, "Set recorder destination file"); // add depth node to recorder nRetVal = recorder.AddNodeToRecording(mockDepth); CHECK_RC(nRetVal, "Add node to recording"); nRetVal = player.SetRepeat(FALSE); XN_IS_STATUS_OK(nRetVal); XnUInt32 nNumFrames = 0; nRetVal = player.GetNumFrames(depth.GetName(), nNumFrames); CHECK_RC(nRetVal, "Get player number of frames"); DepthMetaData depthMD; while ((nRetVal = depth.WaitAndUpdateData()) != XN_STATUS_EOF) { CHECK_RC(nRetVal, "Read next frame"); // Get depth meta data depth.GetMetaData(depthMD); //-----------------------------------------------// // Transform depth! This is the interesting part // //-----------------------------------------------// /* Enable the depth data to be modified. This is done implicitly by depthMD.WritableDepthMap(), but we're calling it just to be clear. */ nRetVal = depthMD.MakeDataWritable(); CHECK_RC(nRetVal, "Make depth data writable"); transformDepthMD(depthMD); // Pass the transformed data to the mock depth generator nRetVal = mockDepth.SetData(depthMD); CHECK_RC(nRetVal, "Set mock node new data"); /* We need to call recorder.Record explicitly because we're not using WaitAndUpdateAll(). */ nRetVal = recorder.Record(); CHECK_RC(nRetVal, "Record"); printf("Recorded: frame %u out of %u\r", depthMD.FrameID(), nNumFrames); } printf("\n"); return 0; }
void captureDepthMap(unsigned char* g_ucDepthBuffer) { SceneMetaData smd; DepthMetaData dmd; _depth.GetMetaData(dmd); //printf("AS3OpenNI :: Frame %d Middle point is: %u. FPS: %f\n", dmd.FrameID(), dmd(dmd.XRes() / 2, dmd.YRes() / 2), xnFPSCalc(&xnFPS)); _depth.GetMetaData(dmd); _userGenerator.GetUserPixels(0, smd); 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(); 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 (_drawPixels) { XnUInt32 nIndex = 0; for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { g_ucDepthBuffer[0] = 0; g_ucDepthBuffer[1] = 0; g_ucDepthBuffer[2] = 0; g_ucDepthBuffer[3] = 0x00; if (_depthMapBackground || *pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; if(_depthMapDetect) { g_ucDepthBuffer[0] = nHistValue * Colors[nColorID][0]; g_ucDepthBuffer[1] = nHistValue * Colors[nColorID][1]; g_ucDepthBuffer[2] = nHistValue * Colors[nColorID][2]; } else { g_ucDepthBuffer[0] = nHistValue; g_ucDepthBuffer[1] = nHistValue; g_ucDepthBuffer[2] = nHistValue; } g_ucDepthBuffer[3] = 0xFF; } } pDepth++; pLabels++; g_ucDepthBuffer+=4; } } } }
int main(int argc, char* argv[]) { XnStatus rc; EnumerationErrors errors; rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors); if (rc == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (rc); } else if (rc != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(rc)); return (rc); } rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth); rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); // Hybrid mode isn't supported in this sample if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes()) { printf ("The device depth and image resolution must be equal!\n"); return 1; } // RGB is the only image format supported. if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { printf("The device image format must be RGB24\n"); return 1; } // Texture map init g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512; g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512; g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel)); // OpenGL init glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); glutCreateWindow ("OpenNI Simple Viewer"); glutFullScreen(); glutSetCursor(GLUT_CURSOR_NONE); glutKeyboardFunc(glutKeyboard); glutDisplayFunc(glutDisplay); glutIdleFunc(glutIdle); glDisable(GL_DEPTH_TEST); glEnable(GL_TEXTURE_2D); // Per frame code is in glutDisplay glutMainLoop(); return 0; }
//---------------------------------------------------- // 描画処理 //---------------------------------------------------- void glutDisplay (void){ xnFPSMarkFrame(&g_xnFPS); // FPSの計測開始? XnStatus rc = XN_STATUS_OK; // 更新されたノードを待つ(どれでもいい) rc = g_context.WaitAnyUpdateAll(); if (rc != XN_STATUS_OK){ printf("Read failed: %s\n", xnGetStatusString(rc)); printf("test\n"); return; } // イメージ・デプス・ユーザのデータを取得 g_image.GetMetaData(g_imageMD); g_depth.GetMetaData(g_depthMD); g_user.GetUserPixels(0, g_sceneMD); // カラー・デプスバッファをクリア glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // 設定 setDepthHistgram(g_depth, g_depthMD, g_pDepthHist); // ヒストグラムの計算・作成 setTexture(); // テクスチャ設定 // 描画 drawImage(); // イメージデータの描画 // デバッグモードの文字は描画の行列と隔離 glMatrixMode(GL_PROJECTION); // 射影変換の行列の設定 //glLoadIdentity(); // スタックのクリア glMatrixMode(GL_MODELVIEW); // モデルビュー変換の行列の設定 glLoadIdentity(); if(g_debugMode) glDebug(); // デバッグモード // 一度だけスクリーンショットをとる if(g_screenShotImageMode){ ostringstream fname; fname << OUT_IMAGE_PATH ;//出力ファイル名 std::string name = fname.str(); g_glScreenShot.screenshot(name.c_str(), 24); g_screenShotImageMode = !g_screenShotImageMode; // トグル } // 一度だけ深さデータを取得する if(g_screenShotDepthMode){ ofstream ofs(OUT_DEPTH_PATH); const XnDepthPixel* pDepth = g_depthMD.Data(); for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; y ++){ for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; x ++, pDepth ++){ if(*pDepth < 2000){ ofs << (int)((*pDepth) * 2) << ','; }else{ ofs << (*pDepth) << ','; } } ofs << endl; } g_screenShotDepthMode = !g_screenShotDepthMode; // トグル } // Swap the OpenGL display buffers glutSwapBuffers(); }
int main(int argc, char* argv[]) { XnStatus nRetVal = XN_STATUS_OK; nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH); if (nRetVal != XN_STATUS_OK) { printf("Log couldn't be opened: %s. Running without log", xnGetStatusString(nRetVal)); } if (argc < 3) { printf("usage: %s <inputFile> <outputFile>\n", argv[0]); return -1; } const char* strInputFile = argv[1]; const char* strOutputFile = argv[2]; Context context; nRetVal = context.Init(); CHECK_RC(nRetVal, "Init"); // open input file Player player; nRetVal = context.OpenFileRecording("/media/6B58CB581C0AACF6/7.oni", player); CHECK_RC(nRetVal, "Open input file"); // Get depth node from recording DepthGenerator depth; nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(nRetVal, "Find depth generator"); // Create mock node based on depth node from recording MockDepthGenerator mockDepth; nRetVal = mockDepth.CreateBasedOn(depth); CHECK_RC(nRetVal, "Create mock depth node"); ImageGenerator image; nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image); CHECK_RC(nRetVal, "Find depth generator"); // Create mock node based on depth node from recording MockImageGenerator mockImage; nRetVal = mockImage.CreateBasedOn(image); CHECK_RC(nRetVal, "Create mock depth node"); // create recorder Recorder recorder; nRetVal = recorder.Create(context); CHECK_RC(nRetVal, "Create recorder"); nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, "/home/shaghayegh/up.oni"); CHECK_RC(nRetVal, "Set recorder destination file"); // add depth node to recorder nRetVal = recorder.AddNodeToRecording(mockDepth); CHECK_RC(nRetVal, "Add node to recording"); // nRetVal = recorder.AddNodeToRecording(mockImage); // CHECK_RC(nRetVal, "Add node to recording"); nRetVal = player.SetRepeat(FALSE); XN_IS_STATUS_OK(nRetVal); XnUInt32 nNumFrames = 0; nRetVal = player.GetNumFrames(depth.GetName(), nNumFrames); CHECK_RC(nRetVal, "Get player number of frames"); DepthMetaData depthMD; ImageMetaData imageMD; int frameNum = 0; String path = "/media/6B58CB581C0AACF6/ebook/Articles/activity_recognition/data1/0512164529/"; while ((nRetVal = depth.WaitAndUpdateData()) != XN_STATUS_EOF) { ++frameNum; CHECK_RC(nRetVal, "Read next frame"); // Get depth meta data depth.GetMetaData(depthMD); image.GetMetaData(imageMD); //-----------------------------------------------// // Transform depth! This is the interesting part // //-----------------------------------------------// /* Enable the depth data to be modified. This is done implicitly by depthMD.WritableDepthMap(), but we're calling it just to be clear. */ nRetVal = depthMD.MakeDataWritable(); CHECK_RC(nRetVal, "Make depth data writable"); // nRetVal = imageMD.MakeDataWritable(); // CHECK_RC(nRetVal, "Make depth data writable"); String ficheroActualRGB; // ficheroActualRGB = path +"RGB_" + boost::to_string(frameNum) + ".png"; String ficheroActualDepth = path +"Depth_"+ boost::to_string(frameNum) + ".png"; // Mat matFrameImage = imread(ficheroActualRGB, 1); // resize(matFrameImage, matFrameImage, Size(640, 480), 0, 0, INTER_CUBIC); Mat matFrameDepth = imread(ficheroActualDepth,1); resize(matFrameDepth, matFrameDepth, Size(480, 640), 0, 0, INTER_CUBIC); transformDepthMD(matFrameDepth,depthMD); // transformImageMD(matFrameImage,imageMD); // Pass the transformed data to the mock depth generator nRetVal = mockDepth.SetData(depthMD); CHECK_RC(nRetVal, "Set mock node new data"); // nRetVal = mockImage.SetData(imageMD); // CHECK_RC(nRetVal, "Set mock node new data"); /* We need to call recorder.Record explicitly because we're not using WaitAndUpdateAll(). */ nRetVal = recorder.Record(); CHECK_RC(nRetVal, "Record"); printf("Recorded: frame %u out of %u\r", depthMD.FrameID(), nNumFrames); } printf("\n"); return 0; }
//---------------------------------------------------- // OpenNI関連の初期化 //---------------------------------------------------- void xnInit(void){ XnStatus rc; EnumerationErrors errors; rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors); if (rc == XN_STATUS_NO_NODE_PRESENT){ XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); exit(1); }else if (rc != XN_STATUS_OK){ printf("Open failed: %s\n", xnGetStatusString(rc)); exit(1); } //playerInit(); rc = xnFPSInit(&g_xnFPS, 180); // FPSの初期化 //CHECK_RC(rc, "FPS Init"); // デプス・イメージ・ユーザジェネレータの作成 rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth); errorCheck(rc, "g_depth"); // エラーチェック rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); errorCheck(rc, "g_image"); rc = g_context.FindExistingNode(XN_NODE_TYPE_USER, g_user); //rc = g_user.Create(g_context); errorCheck(rc, "g_user"); // ユーザー検出機能をサポートしているか確認 if (!g_user.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { //throw std::runtime_error("ユーザー検出をサポートしてません"); cout << "ユーザー検出をサポートしてません" << endl; exit(1); } // レコーダーの設定 //rc = setRecorder(g_recorder, rc); // ユーザコールバックの登録 XnCallbackHandle userCallbacks; g_user.RegisterUserCallbacks(UserDetected, UserLost, NULL, userCallbacks); // デプス・イメージ・ユーザデータの取得 g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); g_user.GetUserPixels(0, g_sceneMD); // Hybrid mode isn't supported in this sample // イメージとデプスの大きさが違うとエラー if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes()){ printf ("The device depth and image resolution must be equal!\n"); exit(1); } // RGB is the only image format supported. // フォーマットの確認 if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24){ printf("The device image format must be RGB24\n"); exit(1); } // Texture map init // フルスクリーン画面の大きさ調整 g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes() - 1) / 512) + 1) * 512; // 大きさによって512の倍数に調整(1024) g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes() - 1) / 512) + 1) * 512; // 512 g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel)); // スクリーンの大きさ分の色情報の容量を確保 // 座標ポインタの初期化 g_pPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D)); // 座標を入れるポインタを作成 g_pBackTex = (XnRGB24Pixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnRGB24Pixel)); // 背景画像を入れるポインタを作成 g_pBackPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D)); // 背景座標を入れるポインタを作成 g_pBackDepth = (XnDepthPixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnDepthPixel)); // 背景座標を入れるポインタを作成 }
int _tmain(int argc, _TCHAR* argv[]) { XnStatus nRetVal = XN_STATUS_OK; Context context; nRetVal = context.Init(); DepthGenerator depth; nRetVal = depth.Create(context); XnMapOutputMode mapMode; mapMode.nXRes = XN_VGA_X_RES; mapMode.nYRes = XN_VGA_Y_RES; mapMode.nFPS = 30; nRetVal = depth.SetMapOutputMode(mapMode); nRetVal = context.StartGeneratingAll(); XnUInt32 nMiddleIndex = XN_VGA_X_RES * XN_VGA_Y_RES/2 + XN_VGA_X_RES/2; int count = 0; const XnDepthPixel* pDepthMap; while (count<50) { // Update to next frame nRetVal = context.WaitOneUpdateAll(depth); pDepthMap = depth.GetDepthMap(); printf("Middle pixel is %u millimeters away\n", pDepthMap[nMiddleIndex]); count++; } DepthMetaData g_depthMD; depth.GetMetaData(g_depthMD); cout<<g_depthMD.FullXRes(); cout<<g_depthMD.FullYRes(); const double maxDepth = 3000; const int xScale = 1; const int yScale = 1; const int xActualRes = XN_VGA_X_RES; const int yActualRes = XN_VGA_Y_RES; const int xRes = xActualRes/xScale; const int yRes = yActualRes/yScale; const int sizeOfMap = xRes * yRes + 2*xRes + 2*(yRes-2) +2 ; //multiply by two for back face vertex* vertices = new vertex[sizeOfMap]; double xRealScale = 1.0; double yRealScale = 1.0; for (int j = 0; j < yRes; j++) { for (int i = 0; i < xRes; i++) { double x = xScale * xRealScale * i; double y = yScale * yRealScale * j; double z = -(double) g_depthMD.DepthMap()[xActualRes * j * yScale + i * xScale]; if (z == 0) z = -maxDepth; if (z > 1800) z = -maxDepth; vertices[xRes*j + i] = vertex(x,y,z); } } int offset = xRes*yRes; vector<int> indices; //bottom facets for (int i = 0; i< xRes; i++) { double x = xRealScale * xScale* i; double y = 0; double z = -maxDepth - 100; //placeholder vertices[offset + i] = vertex(x,y,z); } //bottom facet winding for (int i = 0; i< xRes-1; i++) { indices.push_back(i); indices.push_back(i+1); indices.push_back(xRes*yRes + 1 + i); indices.push_back(i); indices.push_back(xRes*yRes + 1 + i); indices.push_back(xRes*yRes + i); } offset = offset + xRes; //right side facets for (int i = 1; i< yRes; i++) { double x = xRealScale * xScale * (xRes-1); double y = yRealScale * yScale * i; double z = -maxDepth - 100; //placeholder vertices[offset + i -1] = vertex(x,y,z); } offset = offset + yRes -1; //right facet winding for (int i = 0; i< yRes-1; i++) { indices.push_back(xRes-1 + i * xRes); //2 good indices.push_back(xRes-1 + xRes * (i+1)); //5 indices.push_back(xRes*yRes + xRes + i); //12 indices.push_back(xRes-1 + i * xRes); //2 good indices.push_back(xRes*yRes + xRes + i -1); //11 indices.push_back(xRes*yRes + xRes + i); //12 } //top side facets for (int i = 1; i< xRes; i++) { double x = xRealScale * xScale * (xRes-1 - i); double y = yRealScale * yScale * (yRes-1); double z = -maxDepth - 100; //placeholder vertices[offset + i -1] = vertex(x,y,z); } //top facet winding for (int i = 0; i< xRes-1; i++) { indices.push_back(xRes*yRes-1 - i); //8 good indices.push_back(xRes*yRes -1 + xRes + yRes + i); //14 indices.push_back(xRes*yRes -2 - i); //7 indices.push_back(xRes*yRes-1 - i); //8 good indices.push_back(xRes*yRes -1 + xRes + yRes + i -1); //13 indices.push_back(xRes*yRes -1 + xRes + yRes + i); //14 } offset = offset + xRes - 1; //check //left side facet for (int i = 1; i< yRes+1; i++) { double x = 0; double y = yRealScale * yScale * (yRes - i); //check double z = -maxDepth - 100; //placeholder vertices[offset + i -1] = vertex(x,y,z); //check } //left side winding for (int i = 0; i < yRes-1; i++) { indices.push_back(xRes*yRes - (xRes*(i+2))); //3 indices.push_back(xRes*yRes - (xRes*(i+1))); //6 indices.push_back(xRes*yRes + 2*xRes + yRes -3 +i); //15 indices.push_back(xRes*yRes + 2*xRes + yRes -3 + i + 1); //16 indices.push_back(xRes*yRes - (xRes*(i+2))); indices.push_back(xRes*yRes + 2*xRes + yRes -3 +i); //15 //indices.push_back(xRes*yRes - xRes * (i+1)); //6 //indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i); //15 //indices.push_back(xRes*yRes - xRes * (i+2)); //3 //indices.push_back(xRes*yRes - xRes * (i+2)); //3 //indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i); //15 //indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i +1); //16 } indices.push_back(xRes*yRes); //6 indices.push_back(0); //15 indices.push_back(xRes*yRes + 2*xRes + 2*yRes -4); //bottom face indices.push_back(xRes*yRes); //6 indices.push_back(xRes*yRes + xRes-1); //15 indices.push_back(xRes*yRes + xRes + yRes -2); //3 indices.push_back(xRes*yRes); //6 indices.push_back(xRes*yRes + xRes + yRes -2); //15 indices.push_back(xRes*yRes + 2*xRes + yRes -3); //3 printf("Created %u vertices\n",sizeOfMap); //front face - this shows the actual depth map for (int i = 0; i < xRes-1; i++) { for (int j = 0; j < yRes-1; j++) { indices.push_back(j * xRes + i); indices.push_back((j+1) * xRes + i); indices.push_back(j * xRes + i + 1); indices.push_back(j * xRes + i + 1); indices.push_back((j+1) * xRes + i); indices.push_back((j+1) * xRes + i + 1); } } int numFacets = indices.size()/3; printf("Created %u facets\n",numFacets); //int offset = sizeOfMap-1; // number of vertices on front face ////back face ////for (int i = 0; i < xRes-1; i++) { //// for (int j = 0; j < yRes-1; j++) { //// indices.push_back(j * xRes + i + 1 + offset); //// indices.push_back((j+1) * xRes + i + offset); //// indices.push_back(j * xRes + i + offset); //// //// indices.push_back((j+1) * xRes + i + 1 + offset); //// indices.push_back((j+1) * xRes + i + offset); //// indices.push_back(j * xRes + i + 1 + offset); //// } ////} //int n = indices.size()/3; ofstream stlOut; stlOut.open ("stlOut.stl"); printf("Writing to STL...\n"); stlOut << "solid kinectout\n"; // printf("Face Count: %u Vertex Count: %u", numFacets, xRes*yRes); for (int ii = 0; ii < numFacets; ++ii) { int v1 = indices[3*ii]; int v2 = indices[3*ii + 1]; int v3 = indices[3*ii + 2]; vertex p1 = vertices[v1]; vertex p2 = vertices[v2]; vertex p3 = vertices[v3]; vertex dir1 = p1.subtract(p2); vertex dir2 = p3.subtract(p2); vertex n = dir1.cross(dir2).normalize(); stlOut << " facet normal "<< n.x << " " << n.y << " " << n.z << "\n"; stlOut << " outer loop\n"; stlOut << " vertex " << p1.x << " " << p1.y << " " << p1.z << "\n"; stlOut << " vertex " << p2.x << " " << p2.y << " " << p2.z << "\n"; stlOut << " vertex " << p3.x << " " << p3.y << " " << p3.z << "\n"; stlOut << " endloop\n"; stlOut << " endfacet\n"; } stlOut << "endsolid kinectout"; printf("Complete!\n"); stlOut.close(); }
int main(int argc, char *argv[]) { //--------------------------------------------------------------------// //------------------------- SETUP REQUIRED NODES ---------------------// //--------------------------------------------------------------------// // Setup the command line parameters. setupParams(argc, argv); // Setup all the sockets. setupSockets(); // Setup the capture socket server for Mac. #if (XN_PLATFORM == XN_PLATFORM_MACOSX) if(_featureDepthMapCapture || _featureRGBCapture) { if(_useSockets) { g_AS3Network = network(); g_AS3Network.init(setupServer); } } #endif // Setup the status. XnStatus _status = XN_STATUS_OK; EnumerationErrors _errors; // Context Init and Add license. _status = _context.Init(); CHECK_RC(_status, "AS3OpenNI :: Initialize context"); _context.SetGlobalMirror(_mirror); XnChar vendor[XN_MAX_NAME_LENGTH]; XnChar license[XN_MAX_LICENSE_LENGTH]; _license.strVendor[XN_MAX_NAME_LENGTH] = strcmp(vendor, "PrimeSense"); _license.strKey[XN_MAX_LICENSE_LENGTH] = strcmp(license, "0KOIk2JeIBYClPWVnMoRKn5cdY4="); _status = _context.AddLicense(_license); CHECK_RC(_status, "AS3OpenNI :: Added license"); // Set it to VGA maps at 30 FPS _depthMode.nXRes = 640; _depthMode.nYRes = 480; _depthMode.nFPS = 30; // Depth map create. _status = _depth.Create(_context); CHECK_RC(_status, "AS3OpenNI :: Create depth generator"); _status = _depth.SetMapOutputMode(_depthMode); // Depth map create. _status = _image.Create(_context); CHECK_RC(_status, "AS3OpenNI :: Create image generator"); _status = _image.SetMapOutputMode(_depthMode); _status = _image.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); // Create the hands generator. _status = _hands.Create(_context); CHECK_RC(_status, "AS3OpenNI :: Create hands generator"); _hands.SetSmoothing(0.1); // Create the gesture generator. _status = _gesture.Create(_context); CHECK_RC(_status, "AS3OpenNI :: Create gesture generator"); // Create user generator. _status = _userGenerator.Create(_context); CHECK_RC(_status, "AS3OpenNI :: Find user generator"); // Create and initialize point tracker _sessionManager = new XnVSessionManager(); _status = _sessionManager->Initialize(&_context, "Wave", "RaiseHand"); if (_status != XN_STATUS_OK) { printf("AS3OpenNI :: Couldn't initialize the Session Manager: %s\n", xnGetStatusString(_status)); CleanupExit(); } _sessionManager->RegisterSession(NULL, &SessionStart, &SessionEnd, &SessionProgress); // Start catching signals for quit indications CatchSignals(&_quit); //---------------------------------------------------------------// //------------------------- SETUP FEATURES ---------------------// //--------------------------------------------------------------// // Define the Wave and SinglePoint detectors. _waveDetector = new XnVWaveDetector(); // SinglePoint detector. if(_featureSinglePoint) _waveDetector->RegisterPointUpdate(NULL, &OnPointUpdate); // Feature Gesture. if(_featureGesture) { // Wave detector. _waveDetector->RegisterWave(NULL, &OnWave); // Push detector. _pushDetector = new XnVPushDetector(); _pushDetector->RegisterPush(NULL, &onPush); // Swipe detector. _swipeDetector = new XnVSwipeDetector(); _swipeDetector->RegisterSwipeUp(NULL, &Swipe_SwipeUp); _swipeDetector->RegisterSwipeDown(NULL, &Swipe_SwipeDown); _swipeDetector->RegisterSwipeLeft(NULL, &Swipe_SwipeLeft); _swipeDetector->RegisterSwipeRight(NULL, &Swipe_SwipeRight); // Steady detector. _steadyDetector = new XnVSteadyDetector(); _steadyDetector->RegisterSteady(NULL, &Steady_OnSteady); } // Feature Circle. if(_featureCircle) { // Circle detector. _circleDetector = new XnVCircleDetector(); _circleDetector->RegisterCircle(NULL, &CircleCB); _circleDetector->RegisterNoCircle(NULL, &NoCircleCB); _circleDetector->RegisterPrimaryPointCreate(NULL, &Circle_PrimaryCreate); _circleDetector->RegisterPrimaryPointDestroy(NULL, &Circle_PrimaryDestroy); } // Feature Slider. if(_featureSlider) { // Left/Right slider. _leftRightSlider = new XnVSelectableSlider1D(3, 0, AXIS_X); _leftRightSlider->RegisterActivate(NULL, &LeftRightSlider_OnActivate); _leftRightSlider->RegisterDeactivate(NULL, &LeftRightSlider_OnDeactivate); _leftRightSlider->RegisterPrimaryPointCreate(NULL, &LeftRightSlider_OnPrimaryCreate); _leftRightSlider->RegisterPrimaryPointDestroy(NULL, &LeftRightSlider_OnPrimaryDestroy); _leftRightSlider->RegisterValueChange(NULL, &LeftRightSlider_OnValueChange); _leftRightSlider->SetValueChangeOnOffAxis(false); // Up/Down slider. _upDownSlider = new XnVSelectableSlider1D(3, 0, AXIS_Y); _upDownSlider->RegisterActivate(NULL, &UpDownSlider_OnActivate); _upDownSlider->RegisterDeactivate(NULL, &UpDownSlider_OnDeactivate); _upDownSlider->RegisterPrimaryPointCreate(NULL, &UpDownSlider_OnPrimaryCreate); _upDownSlider->RegisterPrimaryPointDestroy(NULL, &UpDownSlider_OnPrimaryDestroy); _upDownSlider->RegisterValueChange(NULL, &UpDownSlider_OnValueChange); _upDownSlider->SetValueChangeOnOffAxis(false); // In/Out slider. _inOutSlider = new XnVSelectableSlider1D(3, 0, AXIS_Z); _inOutSlider->RegisterActivate(NULL, &InOutSlider_OnActivate); _inOutSlider->RegisterDeactivate(NULL, &InOutSlider_OnDeactivate); _inOutSlider->RegisterPrimaryPointCreate(NULL, &InOutSlider_OnPrimaryCreate); _inOutSlider->RegisterPrimaryPointDestroy(NULL, &InOutSlider_OnPrimaryDestroy); _inOutSlider->RegisterValueChange(NULL, &InOutSlider_OnValueChange); _inOutSlider->SetValueChangeOnOffAxis(false); } // Feature TrackPad. if(_featureTrackPad) { // Track Pad. if(trackpad_columns > 0 && trackpad_rows > 0) { _trackPad = new XnVSelectableSlider2D(trackpad_columns, trackpad_rows); } else { _trackPad = new XnVSelectableSlider2D(4, 9); } _trackPad->RegisterItemHover(NULL, &TrackPad_ItemHover); _trackPad->RegisterItemSelect(NULL, &TrackPad_ItemSelect); _trackPad->RegisterPrimaryPointCreate(NULL, &TrackPad_PrimaryCreate); _trackPad->RegisterPrimaryPointDestroy(NULL, &TrackPad_PrimaryDestroy); } // Feature User Tracking. if(_featureUserTracking) { // Setup user generator callbacks. XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks; if (!_userGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("AS3OpenNI :: Supplied user generator doesn't support skeleton\n"); return 1; } _userGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); // Setup Skeleton detection. _userGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (_userGenerator.GetSkeletonCap().NeedPoseForCalibration()) { _needPose = true; if (!_userGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("AS3OpenNI :: Pose required, but not supported\n"); return 1; } _userGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); _userGenerator.GetSkeletonCap().GetCalibrationPose(_strPose); } _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); } // Create the broadcaster manager. _broadcaster = new XnVBroadcaster(); // Start generating all. _context.StartGeneratingAll(); // Set the frame rate. _status = xnFPSInit(&xnFPS, 180); CHECK_RC(_status, "AS3OpenNI :: FPS Init"); //----------------------------------------------------------------------// //------------------------- SETUP DISPLAY SUPPORT ---------------------// //--------------------------------------------------------------------// // Setup depth and image data. _depth.GetMetaData(_depthData); _image.GetMetaData(_imageData); // Hybrid mode isn't supported in this sample if (_imageData.FullXRes() != _depthData.FullXRes() || _imageData.FullYRes() != _depthData.FullYRes()) { printf ("AS3OpenNI :: The device depth and image resolution must be equal!\n"); return 1; } // RGB is the only image format supported. if (_imageData.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { printf("AS3OpenNI :: The device image format must be RGB24\n"); return 1; } // Setup the view points to match between the depth and image maps. if(_snapPixels) _depth.GetAlternativeViewPointCap().SetViewPoint(_image); //-------------------------------------------------------------// //------------------------- MAIN LOOP ------------------------// //-----------------------------------------------------------// // Setup the capture socket server for PC. #if (XN_PLATFORM == XN_PLATFORM_WIN32) if(_featureDepthMapCapture || _featureRGBCapture || _featureUserTracking) { if(_useSockets) { g_AS3Network = network(); g_AS3Network.init(setupServer); } } #endif // Main loop while ((!_kbhit()) && (!_quit)) { xnFPSMarkFrame(&xnFPS); _context.WaitAndUpdateAll(); _sessionManager->Update(&_context); if(_featureDepthMapCapture) captureDepthMap(g_ucDepthBuffer); if(_featureRGBCapture) captureRGB(g_ucImageBuffer); #if (XN_PLATFORM == XN_PLATFORM_WIN32) if(_featureUserTracking) getPlayers(); #else if(_featureUserTracking) renderSkeleton(); #endif } CleanupExit(); }
int main() { XnStatus nRetVal = XN_STATUS_OK; Context context; ScriptNode scriptNode; EnumerationErrors errors; XnUInt32 min_z1, min_z2, min_z3, maxGrad, distVal; 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 = context.InitFromXmlFile(fn, 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); } DepthGenerator depth; nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(nRetVal, "Find depth generator"); XnFPSData xnFPS; nRetVal = xnFPSInit(&xnFPS, 180); CHECK_RC(nRetVal, "FPS Init"); DepthMetaData depthMD; //Initialize WiringPi if(wiringPiSetup() == -1) exit(1); //Enable SoftPWM on pin 1,2 and 3 softPwmCreate(1, 0, RANGE); softPwmCreate(2, 0, RANGE); softPwmCreate(3, 0, RANGE); while (!xnOSWasKeyboardHit()) { nRetVal = context.WaitOneUpdateAll(depth); if (nRetVal != XN_STATUS_OK) { printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal)); continue; } xnFPSMarkFrame(&xnFPS); depth.GetMetaData(depthMD); const XnDepthPixel* pDepthMap = depthMD.Data(); int XRes = depthMD.XRes(); int YRes = depthMD.YRes(); //To find closest pixel value in Zone 1, Zone 2 and Zone 3 min_z1 = getClosestPixel( 0 , 0, (XRes / 2) , YRes, depthMD); min_z2 = getClosestPixel( (XRes / 4), 0, (3 * XRes / 4), YRes, depthMD); min_z3 = getClosestPixel( (XRes / 2), 0, XRes , YRes, depthMD); double in_low = 600; double in_high = 2000; double in_diff = in_high - in_low; double out_low = 51; double out_high = 973; double out_diff = out_high - out_low; distVal = min_z1; XnUInt32 pwm_val1 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low; distVal = min_z2; XnUInt32 pwm_val2 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low; distVal = min_z3; XnUInt32 pwm_val3 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low; // Zone 1 - Left side (pin ) if (pwm_val1 < out_low) pwm_val1 = 0; // if object too far, set DUTY CYCLE to 0 if (min_z1 == 9000.0) pwm_val1 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%) if (min_z1 < 600) pwm_val1 = out_high; // Zone 2 - Center (pin ) if (pwm_val2 < out_low) pwm_val2 = 0; // if object too far, set DUTY CYCLE to 0 if (min_z2 == 9000.0) pwm_val2 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%) if (min_z2 < 600) pwm_val2 = out_high; // Zone 3 - Right side (pin ) if (pwm_val3 < out_low) pwm_val3 = 0; // if object too far, set DUTY CYCLE to 0 if (min_z3 == 9000.0) pwm_val3 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%) if (min_z3 < 600) pwm_val3 = out_high; pwm_val1 = ((pwm_val1 - out_low) / (1.0 * out_diff)) * 100.0; pwm_val2 = ((pwm_val2 - out_low) / (1.0 * out_diff)) * 100.0; pwm_val3 = ((pwm_val3 - out_low) / (1.0 * out_diff)) * 100.0; softPwmWrite(1,(int)pwm_val1); softPwmWrite(2,(int)pwm_val2); softPwmWrite(3,(int)pwm_val3); if ( (depthMD.FrameID() % 30) == 0) { printf("Frame %d", depthMD.FrameID()); printf("\n"); printf("Zone 1 value is %u \t", pwm_val1); printf("Zone 2 value is %u \t", pwm_val2); printf("Zone 3 value is %u \n", pwm_val3); printf("Zone1 min_dis %u \t", min_z1); printf("Zone2 min_dis %u \t", min_z2); printf("Zone3 min_dis %u \n", min_z3); //To find a gradient value for the floor //maxGrad = getGradient( 5, 0, (YRes/2) + 1, depthMD.XRes(), depthMD.YRes(), depthMD); //printf("Frame %d max gradient for Floor is: %u. FPS: %f\n\n", depthMD.FrameID(), maxGrad, xnFPSCalc(&xnFPS)); } } softPwmWrite(1,0); softPwmWrite(2,0); softPwmWrite(3,0); //release the nodes depth.Release(); scriptNode.Release(); context.Release(); return 0; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { mxArray *tmpContext; mxArray *tmpIR, *tmpIRMD; mxArray *tmpDepth, *tmpDepthMD; mxArray *context_initialised_array; mxArray *has_ir_node_array; mxArray *has_depth_node_array; Context* context; IRGenerator* ir; IRMetaData* irMD; DepthGenerator* depth; DepthMetaData* depthMD; unsigned short* output_ir = 0; unsigned short* output_depth = 0; mwSize dims2_ir[2]; mwSize dims2_depth[2]; bool context_initialised = false; bool has_ir_node = false; bool has_depth_node = false; //--------------------------------- // Read input variables //--------------------------------- // ni_context_obj tmpContext = mxGetField(prhs[0], 0, "ni_context_obj"); memcpy((void*)&context, mxGetPr(tmpContext), sizeof(Context*)); // ir_obj tmpIR = mxGetField(prhs[0], 0, "ir_obj"); memcpy((void*)&ir, mxGetPr(tmpIR), sizeof(IRGenerator*)); // irMD_obj tmpIRMD = mxGetField(prhs[0], 0, "irMD_obj"); memcpy((void*)&irMD, mxGetPr(tmpIRMD), sizeof(IRMetaData*)); // depth_obj tmpDepth = mxGetField(prhs[0], 0, "depth_obj"); memcpy((void*)&depth, mxGetPr(tmpDepth), sizeof(DepthGenerator*)); // depthMD_obj tmpDepthMD = mxGetField(prhs[0], 0, "depthMD_obj"); memcpy((void*)&depthMD, mxGetPr(tmpDepthMD), sizeof(DepthMetaData*)); // context_initialised context_initialised_array = mxGetField(prhs[0], 0, "context_initialised"); context_initialised = mxGetScalar(context_initialised_array); // has_ir_node has_ir_node_array = mxGetField(prhs[0], 0, "has_ir_node"); has_ir_node = mxGetScalar(has_ir_node_array); // has_depth_node has_depth_node_array = mxGetField(prhs[0], 0, "has_depth_node"); has_depth_node = mxGetScalar(has_depth_node_array); //------------------------------- // Create output variables //------------------------------- bool initialised = context_initialised; int ir_width = 640; int ir_height = 480; int depth_width = 640; int depth_height = 480; if(initialised){ XnStatus rc; if(has_ir_node){ ir->GetMetaData(*irMD); ir_width = irMD->XRes(); ir_height = irMD->YRes(); } if(has_depth_node){ depth->GetMetaData(*depthMD); depth_width = depthMD->XRes(); depth_height = depthMD->YRes(); } } dims2_ir[0] = ir_height; dims2_ir[1] = ir_width; plhs[0] = mxCreateNumericArray(2, dims2_ir, mxUINT16_CLASS, mxREAL); output_ir = (unsigned short*)mxGetPr(plhs[0]); if(nlhs >= 2){ dims2_depth[0] = depth_height; dims2_depth[1] = depth_width; plhs[1] = mxCreateNumericArray(2, dims2_depth, mxUINT16_CLASS, mxREAL); output_depth = (unsigned short*)mxGetPr(plhs[1]); } if(!initialised) return; if(has_ir_node && output_ir != 0){ const XnIRPixel* pIR = irMD->Data(); int ir_image_size = ir_width*ir_height; for(int i=0;i<ir_height;i++){ for(int j = 0;j < ir_width;j++){ output_ir[j*ir_height+i] = pIR[i*ir_width+j]; } } } if(has_depth_node && output_depth != 0){ const XnDepthPixel* pDepth = depthMD->Data(); int depth_image_size = depth_width*depth_height; for(int i=0;i<depth_height;i++){ for(int j = 0;j < depth_width;j++){ output_depth[j*depth_height+i] = pDepth[i*depth_width+j]; } } } }
int main() { XnStatus nRetVal = XN_STATUS_OK; Context context; ScriptNode scriptNode; 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 = context.InitFromXmlFile(fn, 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); } DepthGenerator depth; nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(nRetVal, "Find depth generator"); XnFPSData xnFPS; nRetVal = xnFPSInit(&xnFPS, 180); CHECK_RC(nRetVal, "FPS Init"); DepthMetaData depthMD; while (!xnOSWasKeyboardHit()) { nRetVal = context.WaitOneUpdateAll(depth); if (nRetVal != XN_STATUS_OK) { printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal)); continue; } xnFPSMarkFrame(&xnFPS); depth.GetMetaData(depthMD); printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS)); } depth.Release(); scriptNode.Release(); context.Release(); return 0; }
int Init() { XnStatus rc; //Make sure our image types are the same as the OpenNI image types. assert(sizeof(XnRGB24Pixel) == sizeof(ColorPixel)); assert(sizeof(XnDepthPixel) == sizeof(DepthPixel)); assert(sizeof(XnStatus) == sizeof(int)); // Load OpenNI xml settings char filePath[255]; int length = Util::Helpers::GetExeDirectory(filePath, sizeof(filePath)); filePath[length] = '\\'; strcpy(&filePath[length+1], SAMPLE_XML_PATH); EnumerationErrors errors; rc = deviceContext.InitFromXmlFile(filePath, &errors); if (rc == XN_STATUS_NO_NODE_PRESENT) { //One reason would be if Microsoft SDK is installed beside PrimeSense. Device manager should say PrimeSense instead of Microsoft Kinect. //XnChar strError[1024]; //errors.ToString(strError, 1024); //LOGE("%s\n", strError); return -1; } else if (rc != XN_STATUS_OK) { fprintf(stderr, "%s\n", xnGetStatusString(rc)); /*LOGE("Open failed: %s\n", xnGetStatusString(rc));*/ return (rc); } // Retrieve colour and depth nodes rc = deviceContext.FindExistingNode(XN_NODE_TYPE_IMAGE, colorImageGenerator); rc = deviceContext.FindExistingNode(XN_NODE_TYPE_DEPTH, depthImageGenerator); // Set mirror mode to off SetMirrorMode(false); // Get a frame to perform checks on it ImageMetaData colorImageMetaData; DepthMetaData depthImageMetaData; depthImageGenerator.GetMetaData(depthImageMetaData); colorImageGenerator.GetMetaData(colorImageMetaData); // Hybrid mode isn't supported in this sample if (colorImageMetaData.FullXRes() != depthImageMetaData.FullXRes() || colorImageMetaData.FullYRes() != depthImageMetaData.FullYRes()) { /*LOGE("The device depth and image resolution must be equal!\n");*/ return 1; } // RGB is the only image format supported. if (colorImageMetaData.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { /*LOGE("The device image format must be RGB24\n");*/ return 1; } // Need to make sure the automatic alignment of colour and depth images is supported. XnBool isSupported = depthImageGenerator.IsCapabilitySupported("AlternativeViewPoint"); if(!isSupported) { /*LOGE("Cannot set AlternativeViewPoint!\n");*/ return 1; } // Set it to VGA maps at 30 FPS /*XnMapOutputMode mapMode; mapMode.nXRes = XN_VGA_X_RES; mapMode.nYRes = XN_VGA_Y_RES; mapMode.nFPS = 60; rc = g_depth.SetMapOutputMode(mapMode); if(rc) { LOGE("Failed to set depth map mode: %s\n", xnGetStatusString(rc)); return 1; } mapMode.nFPS = 30; rc = g_image.SetMapOutputMode(mapMode); if(rc) { LOGE("Failed to set image map mode: %s\n", xnGetStatusString(rc)); return 1; }*/ // Set automatic alignment of the colour and depth images. rc = depthImageGenerator.GetAlternativeViewPointCap().SetViewPoint(colorImageGenerator); if(rc) { /*LOGE("Failed to set depth map mode: %s\n", xnGetStatusString(rc));*/ return 1; } return XN_STATUS_OK; }