bool SetupDepth(xn::Context& g_context) { XnStatus nRetVal = XN_STATUS_OK; fprintf(stderr,"Setting up the depth generator\n"); if ((nRetVal = g_depth.Create(g_context))!= XN_STATUS_OK) { printf("Could not create depth generator: %s\n", xnGetStatusString(nRetVal)); return FALSE; } /* if ((nRetVal = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth)) != XN_STATUS_OK) { fprintf(stderr,"Could not find depth sensor: %s\n", xnGetStatusString(nRetVal)); return FALSE; } */ XnMapOutputMode mapMode; mapMode.nXRes = XN_VGA_X_RES; mapMode.nYRes = XN_VGA_Y_RES; mapMode.nFPS = 30; if ((nRetVal = g_depth.SetMapOutputMode(mapMode)) != XN_STATUS_OK) { fprintf(stderr,"Could not set depth mode: %s\n", xnGetStatusString(nRetVal)); return FALSE; } g_depth.GetMetaData(g_depthMD); g_depthWidth = g_depthMD.FullXRes(); g_depthHeight = g_depthMD.FullYRes(); return TRUE; }
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 ); }
// Save new data from OpenNI void MovieMng::Update(const xn::DepthMetaData& dmd, const xn::ImageMetaData& imd) { // バッファ空き確認 if((m_nBufferSizeFile == 0) || (m_nBufferSizeSend == 0)) { printf("バッファ空きなし!! ForFile:%d ForSend:%d\n", m_nBufferSizeFile, m_nBufferSizeSend); return; } SingleFrame* pFrames = m_pFrames + m_nNextWrite; if(m_nNextWrite == 0) { m_nImageHeight = imd.FullYRes(); m_nImageWidth = imd.FullXRes(); m_nDepthHeight = dmd.FullYRes(); m_nDepthWidth = dmd.FullXRes(); printf("Image Height:%d Width:%d Depth Height:%d Width:%d\n", m_nImageHeight, m_nImageWidth, m_nDepthHeight, m_nDepthWidth); } #ifdef LOG_WRITE_ENABLE fprintf(mmng_fp, "[%s] update. m_nNextWrite:%d\n", __FUNCTION__, m_nNextWrite); #endif if (m_bDepth) { pFrames->depthFrame.CopyFrom(dmd); } if (m_bImage) { pFrames->imageFrame.CopyFrom(imd); } // バッファ管理情報書き換え if (m_bFileWrite) { EnterCriticalSection(&csFileWriter); m_nDataCountFile++; m_nBufferSizeFile--; } if (m_bSendDepth) { EnterCriticalSection(&csDepthSender); m_nDataCountSend++; m_nBufferSizeSend--; } m_nNextWrite++; if (m_nBufferSize == m_nNextWrite) { m_nNextWrite = 0; } if (m_bFileWrite) { LeaveCriticalSection(&csFileWriter); } if (m_bSendDepth) { LeaveCriticalSection(&csDepthSender); } }
bool OpenNIVideo::update(osg::NodeVisitor* nv) { //this is the main function of your video plugin //you can either retrieve images from your video stream/camera/file //or communicate with a thread to synchronize and get the data out //the most important is to synchronize your data //and copy the result to the VideoImageSteam used in this plugin // //0. you can collect some stats, for that you can use a timer osg::Timer t; { //1. mutex lock access to the image video stream OpenThreads::ScopedLock<OpenThreads::Mutex> _lock(this->getMutex()); osg::notify(osg::DEBUG_INFO)<<"osgART::OpenNIVideo::update() get new image.."<<std::endl; XnStatus nRetVal = XN_STATUS_OK; nRetVal=context.WaitAndUpdateAll(); CHECK_RC(nRetVal, "Update Data"); xnFPSMarkFrame(&xnFPS); depth_generator.GetMetaData(depthMD); const XnDepthPixel* pDepthMap = depthMD.Data(); //depth pixel floating point depth map. image_generator.GetMetaData(imageMD); const XnUInt8* pImageMap = imageMD.Data(); // Hybrid mode isn't supported in this sample if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes()) { std::cerr<<"The device depth and image resolution must be equal!"<<std::endl; exit(1); } // RGB is the only image format supported. if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { std::cerr<<"The device image format must be RGB24"<<std::endl; exit(1); } const XnDepthPixel* pDepth=pDepthMap; const XnUInt8* pImage=pImageMap; XnDepthPixel zMax = depthMD.ZRes(); //convert float buffer to unsigned short for ( unsigned int i=0; i<(depthMD.XRes() * depthMD.YRes()); ++i ) { *(_depthBufferByte + i) = 255 * (float(*(pDepth + i)) / float(zMax)); } memcpy(_videoStreamList[0]->data(),pImage, _videoStreamList[0]->getImageSizeInBytes()); memcpy(_videoStreamList[1]->data(),_depthBufferByte, _videoStreamList[1]->getImageSizeInBytes()); //3. don't forget to call this to notify the rest of the application //that you have a new video image _videoStreamList[0]->dirty(); _videoStreamList[1]->dirty(); } //4. hopefully report some interesting data if (nv) { const osg::FrameStamp *framestamp = nv->getFrameStamp(); if (framestamp && _stats.valid()) { _stats->setAttribute(framestamp->getFrameNumber(), "Capture time taken", t.time_m()); } } // Increase modified count every X ms to ensure tracker updates if (updateTimer.time_m() > 50) { _videoStreamList[0]->dirty(); _videoStreamList[1]->dirty(); updateTimer.setStartTick(); } return true; }
bool SetupPrimesense(void) { XnStatus nRetVal = XN_STATUS_OK; if ((nRetVal = g_context.Init()) != XN_STATUS_OK) { fprintf(stderr,"Could not init OpenNI context: %s\n", xnGetStatusString(nRetVal)); return FALSE; } if ((nRetVal = g_depth.Create(g_context))!= XN_STATUS_OK) { fprintf(stderr,"Could not create depth generator: %s\n", xnGetStatusString(nRetVal)); g_haveDepth = FALSE; } else if ((nRetVal = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth)) != XN_STATUS_OK) { fprintf(stderr,"Could not find depth sensor: %s\n", xnGetStatusString(nRetVal)); g_haveDepth = FALSE; } if ((nRetVal = g_image.Create(g_context))!= XN_STATUS_OK) { fprintf(stderr,"Could not create image generator: %s\n", xnGetStatusString(nRetVal)); g_haveImage = FALSE; } else if ((nRetVal = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image)) != XN_STATUS_OK) { fprintf(stderr,"Could not find image sensor: %s\n", xnGetStatusString(nRetVal)); g_haveImage = FALSE; } if (!g_haveImage && !g_haveDepth) { fprintf(stderr,"Could not find either depth or image sources.\n"); return FALSE; } XnMapOutputMode mapMode; mapMode.nXRes = XN_VGA_X_RES; mapMode.nYRes = XN_VGA_Y_RES; mapMode.nFPS = 30; if (g_haveDepth && ( (nRetVal = g_depth.SetMapOutputMode(mapMode)) != XN_STATUS_OK)) { fprintf(stderr,"Could not set depth mode: %s\n", xnGetStatusString(nRetVal)); return FALSE; } if (g_haveDepth) { g_depth.GetMetaData(g_depthMD); g_depthWidth = g_depthMD.FullXRes(); g_depthHeight = g_depthMD.FullYRes(); } if (g_haveImage && (nRetVal = g_image.SetMapOutputMode(mapMode)) != XN_STATUS_OK) { fprintf(stderr,"Could not set image: %s\n", xnGetStatusString(nRetVal)); return FALSE; } if ((nRetVal = g_context.StartGeneratingAll()) != XN_STATUS_OK) { fprintf(stderr,"Could not start: %s\n", xnGetStatusString(nRetVal)); return FALSE; } return TRUE; }