// デプスのヒストグラムを作成 depth_hist getDepthHistgram(const xn::DepthGenerator& depth, const xn::DepthMetaData& depthMD) { // デプスの傾向を計算する(アルゴリズムはNiSimpleViewer.cppを利用) const int MAX_DEPTH = depth.GetDeviceMaxDepth(); depth_hist depthHist(MAX_DEPTH); unsigned int points = 0; const XnDepthPixel* pDepth = depthMD.Data(); for (XnUInt y = 0; y < depthMD.YRes(); ++y) { for (XnUInt x = 0; x < depthMD.XRes(); ++x, ++pDepth) { if (*pDepth != 0) { depthHist[*pDepth]++; points++; } } } for (int i = 1; i < MAX_DEPTH; ++i) { depthHist[i] += depthHist[i-1]; } if ( points != 0) { for (int i = 1; i < MAX_DEPTH; ++i) { depthHist[i] = (unsigned int)(256 * (1.0f - (depthHist[i] / points))); } } return depthHist; }
int GenerateMinecraftCharacter(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, const XnRGB24Pixel* image) { int ret = 0; int xRes = dmd.XRes(); int yRes = dmd.YRes(); cv::Mat inputImage = cv::Mat(yRes, xRes, CV_8UC3); cv::Mat skin = cv::Mat::zeros(cv::Size(64,32), CV_8UC3); XnToCV(image,&inputImage); cv::cvtColor(inputImage,inputImage,CV_RGB2BGR); XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); int i = 0; for (i = 0; i < nUsers; ++i) { if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) break; } // No users being tracked if (i == nUsers) return -1; ret = GenerateSkin(aUsers[i], &inputImage, &skin); printf("GenerateSkin returned %d on user %d\n",ret,(int)aUsers[i]); cv::imwrite("skin.png",skin); SegmentUser(aUsers[i], &inputImage, smd); DrawDebugPoints(aUsers[i], &inputImage); cv::imwrite("blah.png",inputImage); sync(); system("convert skin.png -transparent black skin.png && composite -geometry +32+0 hardhat.png skin.png skin.png"); sync(); return ret; }
void getDepthImage(xn::DepthMetaData& depthMD, cv::Mat& depth_image) { int rows = depthMD.GetUnderlying()->pMap->Res.Y; int cols = depthMD.GetUnderlying()->pMap->Res.X; // Data is 16-bit unsigned depths in mm cv::Mat tempmat(rows, cols, CV_16U); tempmat.data = (uchar*) depthMD.GetUnderlying()->pData; // Convert to floating point meters tempmat.convertTo(depth_image, CV_32F); depth_image /= 1000; }
virtual bool grab( xn::DepthMetaData& depthMetaData, xn::ImageMetaData& imageMetaData ) { while(1) { if( !isDepthFilled ) isDepthFilled = popDepthMetaData(depth); if( !isImageFilled ) isImageFilled = popImageMetaData(image); if( !isDepthFilled || !isImageFilled ) break; double timeDiff = 1e-3 * std::abs(static_cast<double>(depth.Timestamp()) - static_cast<double>(image.Timestamp())); if( timeDiff <= approxSyncGrabber.maxTimeDuration ) { depthMetaData.InitFrom(depth); imageMetaData.InitFrom(image); isDepthFilled = isImageFilled = false; return true; } else { if( depth.Timestamp() < image.Timestamp() ) isDepthFilled = false; else isImageFilled = false; } } return false; }
IplImage* CvCapture_OpenNI::retrievePointCloudMap() { if( !depthMetaData.Data() ) return 0; cv::Mat depth; getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue ); const int badPoint = INVALID_PIXEL_VAL; const float badCoord = INVALID_COORDINATE_VAL; int cols = depthMetaData.XRes(), rows = depthMetaData.YRes(); cv::Mat pointCloud_XYZ( rows, cols, CV_32FC3, cv::Scalar::all(badPoint) ); cv::Ptr<XnPoint3D> proj = new XnPoint3D[cols*rows]; cv::Ptr<XnPoint3D> real = new XnPoint3D[cols*rows]; for( int y = 0; y < rows; y++ ) { for( int x = 0; x < cols; x++ ) { int ind = y*cols+x; proj[ind].X = (float)x; proj[ind].Y = (float)y; proj[ind].Z = depth.at<unsigned short>(y, x); } } depthGenerator.ConvertProjectiveToRealWorld(cols*rows, proj, real); for( int y = 0; y < rows; y++ ) { for( int x = 0; x < cols; x++ ) { // Check for invalid measurements if( depth.at<unsigned short>(y, x) == badPoint ) // not valid pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( badCoord, badCoord, badCoord ); else { int ind = y*cols+x; pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( real[ind].X*0.001f, real[ind].Y*0.001f, real[ind].Z*0.001f); // from mm to meters } } } outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].mat = pointCloud_XYZ; return outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].getIplImagePtr(); }
virtual inline bool popDepthMetaData( xn::DepthMetaData& depthMetaData ) { cv::Ptr<xn::DepthMetaData> depthPtr; bool isPop = depthQueue.try_pop(depthPtr); if( isPop ) depthMetaData.CopyFrom(*depthPtr); return isPop; }
inline void getDepthMapFromMetaData( const xn::DepthMetaData& depthMetaData, cv::Mat& depthMap, XnUInt64 noSampleValue, XnUInt64 shadowValue ) { int cols = depthMetaData.XRes(); int rows = depthMetaData.YRes(); depthMap.create( rows, cols, CV_16UC1 ); const XnDepthPixel* pDepthMap = depthMetaData.Data(); // CV_Assert( sizeof(unsigned short) == sizeof(XnDepthPixel) ); memcpy( depthMap.data, pDepthMap, cols*rows*sizeof(XnDepthPixel) ); cv::Mat badMask = (depthMap == (double)noSampleValue) | (depthMap == (double)shadowValue) | (depthMap == 0); // mask the pixels with invalid depth depthMap.setTo( cv::Scalar::all( CvCapture_OpenNI::INVALID_PIXEL_VAL ), badMask ); }
IplImage* CvCapture_OpenNI::retrieveDepthMap() { if( !depthMetaData.Data() ) return 0; getDepthMapFromMetaData( depthMetaData, outputMaps[CV_CAP_OPENNI_DEPTH_MAP].mat, noSampleValue, shadowValue ); return outputMaps[CV_CAP_OPENNI_DEPTH_MAP].getIplImagePtr(); }
virtual inline bool popDepthMetaData( xn::DepthMetaData& depthMetaData ) { if( depthQueue.empty() ) return false; depthMetaData.CopyFrom(*depthQueue.front()); depthQueue.pop(); return true; }
/** * @brief Gets a copy of the Color cv::Mat. * @details Gets a copy of the Color cv::Mat, if the Image Generator * (_image_generator) is active. * * @param[out] depth * Copy of the Image cv::Mat. * * @retval true if the cv::Mat was successfully copied. * @retval false if the generator is not active or some other error occurred. */ bool NIKinect::get_depth_meta_data(xn::DepthMetaData& depth){ if(this->_flags[NIKinect::DEPTH_G]){ depth.CopyFrom(this->_depth_md); return true; } else{ return false; } }
IplImage* CvCapture_OpenNI::retrieveDisparityMap_32F() { if( !depthMetaData.Data() ) return 0; computeDisparity_32F( depthMetaData, outputMaps[CV_CAP_OPENNI_DISPARITY_MAP_32F].mat, baseline, depthFocalLength_VGA, noSampleValue, shadowValue ); return outputMaps[CV_CAP_OPENNI_DISPARITY_MAP_32F].getIplImagePtr(); }
std::string getDepthMap(bool br) { { ReadLock r_lock(myLock); stringstream emitter; emitter << "{DepthMap," << g_depthMD.XRes() << ","<< g_depthMD.YRes() << "}["; if(g_depthMD.XRes() != 0){ for(int i = 0; i < g_depthMD.XRes(); i++) { if(i>0 && br == true) emitter << "<BR>"; for(int j = 0; j < g_depthMD.YRes(); j++) { emitter << g_depthMD(i, j); emitter << ","; } } } emitter << "]"; return emitter.str(); } }
IplImage* CvCapture_OpenNI::retrieveDisparityMap() { if( !depthMetaData.Data() ) return 0; cv::Mat disp32; computeDisparity_32F( depthMetaData, disp32, baseline, depthFocalLength_VGA, noSampleValue, shadowValue ); disp32.convertTo( outputMaps[CV_CAP_OPENNI_DISPARITY_MAP].mat, CV_8UC1 ); return outputMaps[CV_CAP_OPENNI_DISPARITY_MAP].getIplImagePtr(); }
IplImage* CvCapture_OpenNI::retrieveValidDepthMask() { if( !depthMetaData.Data() ) return 0; cv::Mat depth; getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue ); outputMaps[CV_CAP_OPENNI_VALID_DEPTH_MASK].mat = depth != CvCapture_OpenNI::INVALID_PIXEL_VAL; return outputMaps[CV_CAP_OPENNI_VALID_DEPTH_MASK].getIplImagePtr(); }
//- - - - - - - - - - - - - - - - - - // void C_TrackWindow::initTexs( const xn::DepthMetaData& depthMD ) { struct getClosestPowerOfTwo { inline unsigned int operator()( unsigned int n) { unsigned int m = 2; while(m < n) m<<=1; return m; }; }; GLuint texID = 0; glGenTextures( 1, &depthTexID_ ); texWidth_ = getClosestPowerOfTwo()( depthMD.XRes() ); texHeight_ = getClosestPowerOfTwo()( depthMD.YRes() ); pDepthTexBuf_ = new unsigned char[ texWidth_ * texHeight_ * 4 ]; glBindTexture( GL_TEXTURE_2D, depthTexID_ ); glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR ); glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR ); }
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; }
bool DataCapture::captureOne() { XnStatus rc = context_.WaitAndUpdateAll(); // want this to be WaitOneUpdateAll(RGB image) if( rc != XN_STATUS_OK ) { std::cout << "WaitAndUpdateAll: " << xnGetStatusString(rc) << std::endl; return false; } // grab image imageGen_.GetMetaData(imageMd_); const XnRGB24Pixel* rgbData = imageMd_.RGB24Data(); for( unsigned int i = 0; i < 640 * 480; ++i ) { pRgbData_[3*i] = rgbData->nRed; pRgbData_[3*i + 1] = rgbData->nGreen; pRgbData_[3*i + 2] = rgbData->nBlue; ++rgbData; } // grab depth image depthGen_.GetMetaData(depthMd_); const uint16_t* pDepthDataU16 = depthMd_.Data(); for( int i = 0; i < 640 * 480; ++i) { uint16_t d = pDepthDataU16[i]; if( d != 0 ) { pDepthData_[i] = (d * 255)/2048; } else { pDepthData_[i] = 0; // should be NAN } } return true; }
//---------------------------------------------------- // ヒストグラム作成関数 //---------------------------------------------------- void setDepthHistgram(const xn::DepthGenerator& depth, const xn::DepthMetaData& depthMD, float _pDepthHist[]){ xnOSMemSet(_pDepthHist, 0, KINECT_MAX_DEPTH * sizeof(float)); // g_pDepthHistの全てに0を代入 unsigned int points = 0; const XnDepthPixel* pDepth = depthMD.Data(); for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; ++ y) { for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; ++ x, ++ pDepth) { if (*pDepth != 0) { _pDepthHist[*pDepth] ++; points ++; } } } for (int i = 1; i < KINECT_MAX_DEPTH; ++ i) { _pDepthHist[i] += _pDepthHist[i - 1]; } if ( points != 0) { for (int i = 1; i < KINECT_MAX_DEPTH; ++ i) { _pDepthHist[i] = (unsigned int)(256 * (1.0f - (_pDepthHist[i] / points))); } } }
/*********************************************************************************** * This routine will draw the scene. This is the OpenGL display function / idle function callback routine. There have been additions to this routine (from the depth map sample). a. Along with updating the context, we also update the session manager. b. We draw the hand point as well. The hand point is represented using a white square. ***********************************************************************************/ void draw(){ glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //clear the gl buffers rc = cxt.WaitAnyUpdateAll(); //first update the context - refresh the depth/image data coming from the sensor if(rc != XN_STATUS_OK){ //if the update failed, i.e. couldn't be read printf("\nERROR:Read failed... Quitting!\n"); //print error message exit(0); //exit the program } else if(inSession){ sessionMgr->Update(&cxt); //NEW ADDITION - update the session manager depthGen.GetMetaData(depthMapMetaData); //grab the depth map meta data long xSize = depthMapMetaData.XRes(); //this meta data will give us the true width long ySize = depthMapMetaData.YRes(); //and height of the scene long totalSize = xSize * ySize; //total size of the depth map is gotten this way const XnDepthPixel* depthMapData; //this array will contain the value of depth of each pixel in depth map depthMapData = depthMapMetaData.Data(); //this will grab the depth values and store it into the array int i, j,colorToSet; //color to set will be calculated for the pixel based on its depth int depth; //to store the depth at a point glLoadIdentity(); //set the matrix to an identity matrix glOrtho(0, xSize, ySize, 0, -1, 1); //set the orthogonal view. -1 and 1 define the near and far clip off points along z axis /************************************** DRAWS THE DEPTH MAP *************************************/ glBegin(GL_POINTS); //start the drawing of points /* Note: the depthMapData is a pointer to an array that is linear. It contains integer values of depth in millimeters. This linear array must be imagined as a matrix of dimensions --> xSize,ySize */ for(i = 0;i<xSize;i++) //this loop will run through width of the depth map { for(j=0;j<ySize;j++) //this will run through the height { depth = depthMapMetaData(i,j); //get the depth info from the depthMapData. lvalue is (i*Xmax + j) in order to translate matrix coords to linear ones colorToSet = MAX_COLOR - depth/COLOR_ZONES; if((depth < DEPTH_THRESH_FAR) && (depth > DEPTH_THRESH_NEAR)) { //if the current pixel depth is within the threshold limits (acceptable range in millimeters) glColor3ub(0,colorToSet,0); //then paint this pixel glVertex3f(i,j,0); } } } glEnd(); //end drawing sequence glBegin(GL_POINTS); pixelmap[4][4].g=255; for(i = 0;i<xSize;i++) //this loop will run through width of the depth map { for(j=0;j<ySize;j++) //this will run through the height { if (pixelmap[i][j].r > 0) { glColor3ub(pixelmap[i][j].r,pixelmap[i][j].g, pixelmap[i][j].b); //then paint this pixel glVertex3f(i,j,0); } } } glEnd(); /********************************* DRAWS THE HAND POINT ***************************************/ if(isHandPointNull() == false){ glColor3f(255,255,255); //set the color to white glBegin(GL_POLYGON); //start drawing the polygon int xCo = handPointCoords.X; //xCoord of the hand point int yCo = handPointCoords.Y; //yCoord of the hand point int zCo = handPointCoords.Z; int size = 4; //size of this square glVertex3f(xCo-size,yCo-size,0); //plot the top left corner glVertex3f(xCo+size,yCo-size,0); //plot the top right corner glVertex3f(xCo+size,yCo+size,0); //plot the bot right corner glVertex3f(xCo-size,yCo+size,0); //plot the bot left corner glEnd(); //stop drawing the polygon //draws a bounding box around the hand using lines int boxSize = 200; int xBox = handPointCoords.X - boxSize/2; int yBox = handPointCoords.Y - boxSize/2; //pixelmap[-1][-1].g=233; if (zCo < g_dist) { glColor3f(255,0,0); for (int i = -5; i <= 5; i++) for (int j = -5; j <= 5; j++) pixelmap[(xCo + i)%640][(yCo + j)%480].r = 255; } glBegin(GL_LINES); glVertex3f(xBox,yBox,0); //top edge glVertex3f(xBox+boxSize,yBox,0); glVertex3f(xBox,yBox,0); //left edge glVertex3f(xBox,yBox+boxSize,0); glVertex3f(xBox+boxSize,yBox,0); //right edge glVertex3f(xBox+boxSize,yBox+boxSize,0); glVertex3f(xBox,yBox+boxSize,0); //bottom edge glVertex3f(xBox+boxSize,yBox+boxSize,0); glEnd(); } /**********************************************************************************************/ } glutSwapBuffers(); //for double buffering }
void DrawDepthMap(const xn::DepthMetaData& dm) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; if(!bInitialized) { XnUInt16 nXRes = dm.XRes(); XnUInt16 nYRes = dm.YRes(); texWidth = getClosestPowerOfTwo(nXRes); texHeight = getClosestPowerOfTwo(nYRes); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; bInitialized = true; topLeftX = nXRes; topLeftY = 0; bottomRightY = nYRes; bottomRightX = 0; texXpos =(float)nXRes/texWidth; texYpos =(float)nYRes/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dm.XRes(); XnUInt16 g_nYRes = dm.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnUInt16* pDepth = dm.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 = dm.Data(); { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { nValue = *pDepth; if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; pDestImage[0] = nHistValue; pDestImage[1] = nHistValue; pDestImage[2] = nHistValue; } else { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; } pDepth++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.5,0.5,0.5,1); glEnable(GL_TEXTURE_2D); DrawTexture(dm.XRes(),dm.YRes(),0,0); glDisable(GL_TEXTURE_2D); }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; if(!bInitialized) { texWidth = getClosestPowerOfTwo(dmd.XRes()); texHeight = getClosestPowerOfTwo(dmd.YRes()); // printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; // printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight); bInitialized = true; topLeftX = dmd.XRes(); topLeftY = 0; bottomRightY = dmd.YRes(); bottomRightX = 0; texXpos =(float)dmd.XRes()/texWidth; texYpos =(float)dmd.YRes()/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // Calculate the accumulative histogram memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float)); for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { g_pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); if (g_bDrawPixels) { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (g_bDrawBackground || *pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; pDestImage[0] = nHistValue * Colors[nColorID][0]; pDestImage[1] = nHistValue * Colors[nColorID][1]; pDestImage[2] = nHistValue * Colors[nColorID][2]; } } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } else { xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes); } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.75,0.75,0.75,1); glEnable(GL_TEXTURE_2D); DrawTexture(dmd.XRes(),dmd.YRes(),0,0); glDisable(GL_TEXTURE_2D); char strLabel[50] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if (g_bPrintID) { XnPoint3D com; g_UserGenerator.GetCoM(aUsers[i], com); g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com); xnOSMemSet(strLabel, 0, sizeof(strLabel)); if (!g_bPrintState) { // Tracking sprintf(strLabel, "%d", aUsers[i]); } else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // Tracking sprintf(strLabel, "%d - Tracking", aUsers[i]); } else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i])) { // Calibrating sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first)); } else { // Nothing sprintf(strLabel, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second)); } glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(com.X, com.Y); glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } //Draw skeleton if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { glBegin(GL_LINES); glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1); DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK); DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER); DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW); DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND); DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER); DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW); DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND); DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO); DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO); DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP); DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE); DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT); DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP); DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE); DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT); DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP); glEnd(); } } }
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 ); }
bool DataCapture::initialise() { context_.Shutdown(); XnStatus rc = context_.Init(); if( rc != XN_STATUS_OK ) { std::cout << "Init: " << xnGetStatusString(rc) << std::endl; return false; } rc = depthGen_.Create(context_); if( rc != XN_STATUS_OK ) { std::cout << "depthGen.Create: " << xnGetStatusString(rc) << std::endl; return false; } rc = imageGen_.Create(context_); if( rc != XN_STATUS_OK ) { std::cout << "imageGen.Create: " << xnGetStatusString(rc) << std::endl; return false; } rc = imageGen_.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); if( rc != XN_STATUS_OK ) { std::cout << "SetPixelFormat: " << xnGetStatusString(rc) << std::endl; return false; } XnMapOutputMode imgMode; imgMode.nXRes = 640; // XN_VGA_X_RES imgMode.nYRes = 480; // XN_VGA_Y_RES imgMode.nFPS = 30; rc = imageGen_.SetMapOutputMode(imgMode); if( rc != XN_STATUS_OK ) { std::cout << "image SetMapOutputMode: " << xnGetStatusString(rc) << std::endl; return false; } rc = depthGen_.SetMapOutputMode(imgMode); if( rc != XN_STATUS_OK ) { std::cout << "depth SetMapOutputMode: " << xnGetStatusString(rc) << std::endl; return false; } depthGen_.GetMetaData(depthMd_); std::cout << "Depth offset " << depthMd_.XOffset() << " " << depthMd_.YOffset() << std::endl; // set the depth image viewpoint depthGen_.GetAlternativeViewPointCap().SetViewPoint(imageGen_); // read off the depth camera field of view. This is the FOV corresponding to // the IR camera viewpoint, regardless of the alternative viewpoint settings. XnFieldOfView fov; rc = depthGen_.GetFieldOfView(fov); std::cout << "Fov: " << fov.fHFOV << " " << fov.fVFOV << std::endl; pDepthData_ = new char [640 * 480]; pRgbData_ = new char [640 * 480 * 3]; return true; }
// 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); } }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; if(!bInitialized) { texWidth = getClosestPowerOfTwo(dmd.XRes()); texHeight = getClosestPowerOfTwo(dmd.YRes()); // printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; // printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight); bInitialized = true; topLeftX = dmd.XRes(); topLeftY = 0; bottomRightY = dmd.YRes(); bottomRightX = 0; texXpos =(float)dmd.XRes()/texWidth; texYpos =(float)dmd.YRes()/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // Calculate the accumulative histogram memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float)); for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { g_pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; pDestImage[0] = nHistValue * Colors[nColorID][0]; pDestImage[1] = nHistValue * Colors[nColorID][1]; pDestImage[2] = nHistValue * Colors[nColorID][2]; } else { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.75,0.75,0.75,1); glEnable(GL_TEXTURE_2D); DrawTexture(dmd.XRes(),dmd.YRes(),0,0); glDisable(GL_TEXTURE_2D); char strLabel[20] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { XnPoint3D com; g_UserGenerator.GetCoM(aUsers[i], com); g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com); if (aUsers[i] == player) sprintf(strLabel, "%d (Player)", aUsers[i]); else sprintf(strLabel, "%d", aUsers[i]); glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(com.X, com.Y); glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } // Draw skeleton of user if (player != 0) { glBegin(GL_LINES); glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1); // gesture static int gesture = 0; static XnPoint3D previousLeftHandPt; XnPoint3D newLeftHandPt; newLeftHandPt = getJointPoint(player, XN_SKEL_LEFT_HAND); if(previousLeftHandPt.X > 0 && previousLeftHandPt.X < 640) if(previousLeftHandPt.X - newLeftHandPt.X > 60) gesture = 1; else if(previousLeftHandPt.X - newLeftHandPt.X < -60) gesture = 2; else if(previousLeftHandPt.Y - newLeftHandPt.Y > 60) gesture = 3; else if(previousLeftHandPt.Y - newLeftHandPt.Y < -60) gesture = 4; else gesture = 0; if(gesture != 0) printf("gesture: %d\n", gesture); previousLeftHandPt = newLeftHandPt; // head XnPoint3D pt = getJointPoint(player, XN_SKEL_HEAD); // save saveLocation(pt, newLeftHandPt, gesture); DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK); DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER); DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW); DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND); DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER); DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW); DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND); DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO); DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO); DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP); DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE); DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT); DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP); DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE); DrawLimb(player, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT); glEnd(); } }
int main ( int argc, char * argv[] ) { // // Initialize OpenNI Settings // XnStatus nRetVal = XN_STATUS_OK; xn::ScriptNode scriptNode; xn::EnumerationErrors errors; // // Initialize Context Object // nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, scriptNode, &errors ); if ( nRetVal == XN_STATUS_NO_NODE_PRESENT ) { XnChar strError[1024]; errors.ToString(strError, 1024); printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError ); system ( "pause" ); return ( nRetVal ); } else if ( nRetVal != XN_STATUS_OK ) { printf ( "Open failed: %s\n", xnGetStatusString(nRetVal) ); system ( "pause" ); return ( nRetVal ); } // // Handle Image & Depth Generator Node // bool colorFlag = true; bool depthFlag = true; nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen ); if ( nRetVal != XN_STATUS_OK ) { printf("No depth node exists!\n"); depthFlag = false; } nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen ); if ( nRetVal != XN_STATUS_OK ) { printf("No image node exists!\n"); colorFlag = false; } // g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen ); if ( depthFlag ) { g_DepthGen.GetMetaData ( g_DepthMD ); assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT ); } if ( colorFlag ) { g_ImageGen.GetMetaData ( g_ImageMD ); assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 ); } g_DepthImgShow = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC1 ); g_DepthImgMat = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 ); g_ColorImgMat = cv::Mat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 ); // // Start to Loop // bool flipColor = true; int ctlWndKey = -1; g_StartTickCount = GetTickCount(); g_HeadTrackingFrameCount = 0; while ( ctlWndKey != ESC_KEY_VALUE ) { nRetVal = g_Context.WaitOneUpdateAll ( g_DepthGen ); // nRetVal = g_Context.WaitAnyUpdateAll(); #ifdef HANDLING_IMAGE_DATA if ( colorFlag ) { g_ImageGen.GetMetaData ( g_ImageMD ); assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() ); assert ( g_ImageMD.FullYRes() == g_ImageMD.YRes() ); GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), g_ColorImgMat ); if ( ctlWndKey == 's' || ctlWndKey == 'S' ) { // Switch flipColor = !flipColor; } if ( flipColor ) { cv::cvtColor ( g_ColorImgMat, g_ColorImgMat, CV_RGB2BGR ); } cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE ); cv::imshow ( IMAGE_WIN_NAME, g_ColorImgMat ); } #endif #ifdef HANDLING_DEPTH_DATA if ( depthFlag ) { g_DepthGen.GetMetaData(g_DepthMD); // assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() ); // assert ( g_DepthMD.FullYRes() == g_DepthMD.YRes() ); GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), g_DepthImgMat ); GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( g_DepthImgMat, g_DepthImgShow ); /* cv::putText(colorImgMat, GlobalUtility::DoubleToString(g_ImageMD.FPS()) + " FPS", cv::Point(10, 450), cv::FONT_ITALIC, 0.7, cv::Scalar(255, 255, 255, 0), 2, 8, false); */ cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE ); cv::imshow ( DEPTH_WIN_NAME, g_DepthImgShow ); } #endif XnFieldOfView fov; g_DepthGen.GetFieldOfView( fov ); std::cout << "HFov = " << fov.fHFOV << std::endl << "VFov = " << fov.fVFOV << std::endl; ctlWndKey = cvWaitKey ( 5 ); g_HeadTrackingFrameCount++; g_CurrTickCount = GetTickCount(); std::cout << "FPS = " << 1000 / ( ( double )( g_CurrTickCount - g_StartTickCount ) / ( double )( g_HeadTrackingFrameCount ) ) << std::endl; } g_Context.Release (); exit ( EXIT_SUCCESS ); }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; /* if not initialized set parameters and reserve memory space */ if(!bInitialized) { texWidth = getClosestPowerOfTwo(dmd.XRes()); texHeight = getClosestPowerOfTwo(dmd.YRes()); // printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; // printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight); bInitialized = true; topLeftX = dmd.XRes(); topLeftY = 0; bottomRightY = dmd.YRes(); bottomRightX = 0; texXpos =(float)dmd.XRes()/texWidth; texYpos =(float)dmd.YRes()/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // get the depth resolution static unsigned int nZRes = dmd.ZRes(); static float* pDepthHist = (float*)malloc(nZRes* sizeof(float)); // Calculate the accumulative histogram memset(pDepthHist, 0, nZRes*sizeof(float)); // count the number of pixels of every possible depth value for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<nZRes; nIndex++) { pDepthHist[nIndex] += pDepthHist[nIndex-1]; } // calculate percentage for every depth value // the larger the value is, the darker the pixel should be if (nNumberOfPoints) { for (nIndex=1; nIndex<nZRes; nIndex++) { pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); if (g_bDrawPixels) { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (g_bDrawBackground || *pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = pDepthHist[nValue]; pDestImage[0] = nHistValue * Colors[nColorID][0]; pDestImage[1] = nHistValue * Colors[nColorID][1]; pDestImage[2] = nHistValue * Colors[nColorID][2]; } } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } else { xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes); } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.75,0.75,0.75,1); glEnable(GL_TEXTURE_2D); DrawTexture(dmd.XRes(),dmd.YRes(),0,0); glDisable(GL_TEXTURE_2D); char strLabel[50] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { #ifndef USE_GLES if (g_bPrintID) { XnPoint3D com; g_UserGenerator.GetCoM(aUsers[i], com); g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com); XnUInt32 nDummy = 0; xnOSMemSet(strLabel, 0, sizeof(strLabel)); if (!g_bPrintState) { // Tracking xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d", aUsers[i]); } else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // Tracking xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Tracking", aUsers[i]); } else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i])) { // Calibrating xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first)); } else { // Nothing xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second)); } glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(com.X, com.Y); glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } #endif if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1); // Draw Joints if (g_bMarkJoints) { // Try to draw all joints DrawJoint(aUsers[i], XN_SKEL_HEAD); DrawJoint(aUsers[i], XN_SKEL_NECK); DrawJoint(aUsers[i], XN_SKEL_TORSO); DrawJoint(aUsers[i], XN_SKEL_WAIST); DrawJoint(aUsers[i], XN_SKEL_LEFT_COLLAR); DrawJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER); DrawJoint(aUsers[i], XN_SKEL_LEFT_ELBOW); DrawJoint(aUsers[i], XN_SKEL_LEFT_WRIST); DrawJoint(aUsers[i], XN_SKEL_LEFT_HAND); DrawJoint(aUsers[i], XN_SKEL_LEFT_FINGERTIP); DrawJoint(aUsers[i], XN_SKEL_RIGHT_COLLAR); DrawJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER); DrawJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW); DrawJoint(aUsers[i], XN_SKEL_RIGHT_WRIST); DrawJoint(aUsers[i], XN_SKEL_RIGHT_HAND); DrawJoint(aUsers[i], XN_SKEL_RIGHT_FINGERTIP); DrawJoint(aUsers[i], XN_SKEL_LEFT_HIP); DrawJoint(aUsers[i], XN_SKEL_LEFT_KNEE); DrawJoint(aUsers[i], XN_SKEL_LEFT_ANKLE); DrawJoint(aUsers[i], XN_SKEL_LEFT_FOOT); DrawJoint(aUsers[i], XN_SKEL_RIGHT_HIP); DrawJoint(aUsers[i], XN_SKEL_RIGHT_KNEE); DrawJoint(aUsers[i], XN_SKEL_RIGHT_ANKLE); DrawJoint(aUsers[i], XN_SKEL_RIGHT_FOOT); } #ifndef USE_GLES glBegin(GL_LINES); #endif // Draw Limbs DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK); DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER); DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW); if (!DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_WRIST)) { DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND); } else { DrawLimb(aUsers[i], XN_SKEL_LEFT_WRIST, XN_SKEL_LEFT_HAND); DrawLimb(aUsers[i], XN_SKEL_LEFT_HAND, XN_SKEL_LEFT_FINGERTIP); } DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER); DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW); if (!DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_WRIST)) { DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND); } else { DrawLimb(aUsers[i], XN_SKEL_RIGHT_WRIST, XN_SKEL_RIGHT_HAND); DrawLimb(aUsers[i], XN_SKEL_RIGHT_HAND, XN_SKEL_RIGHT_FINGERTIP); } DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO); DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO); DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP); DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE); DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT); DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP); DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE); DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT); DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP); #ifndef USE_GLES glEnd(); #endif } } if (g_bPrintFrameID) { static XnChar strFrameID[80]; xnOSMemSet(strFrameID, 0, 80); XnUInt32 nDummy = 0; xnOSStrFormat(strFrameID, sizeof(strFrameID), &nDummy, "%d", dmd.FrameID()); glColor4f(1, 0, 0, 1); glRasterPos2i(10, 10); glPrintString(GLUT_BITMAP_HELVETICA_18, strFrameID); } }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, ros::Publisher pub_body, edwin::SceneAnalysis scene, edwin::People person) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; if(!bInitialized) { texWidth = getClosestPowerOfTwo(dmd.XRes()); texHeight = getClosestPowerOfTwo(dmd.YRes()); // printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; // printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight); bInitialized = true; topLeftX = dmd.XRes(); topLeftY = 0; bottomRightY = dmd.YRes(); bottomRightX = 0; texXpos =(float)dmd.XRes()/texWidth; texYpos =(float)dmd.YRes()/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // Calculate the accumulative histogram memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float)); for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { g_pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints))); } } XnPoint3D coms[20]; XnUInt32 labels[20] = {0}; for (int i = 0; i < 20; ++i) { coms[i] = xnCreatePoint3D(0,0,0); } pDepth = dmd.Data(); { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; pDestImage[0] = nHistValue * Colors[nColorID][0]; pDestImage[1] = nHistValue * Colors[nColorID][1]; pDestImage[2] = nHistValue * Colors[nColorID][2]; if (label < 20 && label > 0) { coms[label].X += nX; coms[label].Y += nY; coms[label].Z += *pDepth; labels[label]++; } } else { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.75,0.75,0.75,1); glEnable(GL_TEXTURE_2D); DrawTexture(dmd.XRes(),dmd.YRes(),0,0); glDisable(GL_TEXTURE_2D); char strLabel[3] = ""; //Initialize the scene array // scene.crowd = []; for (int i = 0; i < 20; ++i) { if (labels[i] == 0) continue; coms[i].X /= labels[i]; coms[i].Y /= labels[i]; coms[i].Z /= labels[i]; sprintf(strLabel, "%d", i); glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(coms[i].X, coms[i].Y); // glRasterPos2i(320, 240); //testing for midpoint of Kinect person.ID = i; person.xpos = coms[i].X; person.ypos = coms[i].Y; person.zpos = coms[i].Z; scene.crowd[i] = person; glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } pub_body.publish(scene); for(int i = 0; i < 20; ++i) { scene.crowd[i].ID = 0; } }
void PublishPeopleImage(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, image_transport::Publisher& pub) { const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); unsigned int nValue = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); cv::Mat peopleSegmentation(g_nYRes, g_nXRes, CV_8UC3); // Prepare the texture map for (unsigned int nY=0; nY<g_nYRes; nY++) { uchar* pDestImage = peopleSegmentation.ptr(nY); for (unsigned int nX=0; nX < g_nXRes; nX++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (*pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (nValue != 0) { pDestImage[0] = 255 * Colors[nColorID][0]; pDestImage[1] = 255 * Colors[nColorID][1]; pDestImage[2] = 255 * Colors[nColorID][2]; } } pDepth++; pLabels++; pDestImage+=3; } } // todo: stop and start with respect to odometry: segmentation works best if robot is standing still // publish try { IplImage img = (IplImage)peopleSegmentation; sensor_msgs::ImagePtr msg = (sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8")); msg->header.stamp = ros::Time::now(); pub.publish(msg); } catch (sensor_msgs::CvBridgeException error) { ROS_ERROR("[openni_tracker] Could not convert IplImage to ROS message"); } // cv_bridge::CvImage bridgeImage; did not work // bridgeImage.image = peopleSegmentation.clone(); // sensor_msgs::ImagePtr msg = bridgeImage.toImageMsg(); // pub.publish(msg); // display for checking the output // cv::namedWindow("Test"); // imshow("Test", peopleSegmentation); // uchar key = cv::waitKey(10); // if (key == 'r') // { // g_UserGenerator.StopGenerating(); // std::cout << "stop\n"; // } // if (key == 'c') // { // g_UserGenerator.StartGenerating(); // std::cout << "start\n"; // } }
void SceneDrawer::updateScene(const xn::DepthMetaData &dmd, const xn::SceneMetaData &smd) { if (!initialized) init(dmd.XRes(), dmd.YRes(), dmd.ZRes()); unsigned int val = 0, numOfPoints = 0; unsigned int xRes = dmd.XRes(), yRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // calculate the accumulative depth histogram memset(pDepthHist, 0, zRes*sizeof(float)); int numOfIterations = xRes * yRes; for (int i = 0; i < numOfIterations; ++i, ++pDepth){ val = *pDepth; if (val != 0){ pDepthHist[val]++; numOfPoints++; } } for (int i = 1; i < zRes; ++i) pDepthHist[i] += pDepthHist[i-1]; if (numOfPoints > 0){ for (int i = 0; i < zRes; ++i) pDepthHist[i] = floor(256.0f*(1.0f-pDepthHist[i]/(float)numOfPoints)); } // turn depth map to a colored texture image pDepth = dmd.Data(); XnUInt32 ci; XnLabel label; unsigned int histVal; for (int i = 0; i < numOfIterations; ++i, ++pDepth, ++pLabels, pDestImage += 3){ val = *pDepth; label = *pLabels; if (label != 0) ci = label % nColors; else ci = nColors; if (val != 0){ histVal = pDepthHist[val]; pDestImage[0] = histVal * colors[ci][0]; pDestImage[1] = histVal * colors[ci][1]; pDestImage[2] = histVal * colors[ci][2]; } else pDestImage[0] = pDestImage[1] = pDestImage[2] = 0; } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); }