BayesianClass::BayesianClass(string name,string path):className(name),fileCount(0) { DIR *dir; struct dirent *ent; cout<<"Calculating "<<className<<endl; if ((dir = opendir (path.c_str())) != NULL) { /* print all the files and directories within directory */ while ((ent = readdir (dir)) != NULL) { struct _stat buf; string directory_name(ent->d_name); if((directory_name==".")||(directory_name=="..")) continue; string class_path = path + "\\" + directory_name; int status = _stat(class_path.c_str(),&buf); if(status!=0) continue; if (!(buf.st_mode&S_IFREG)) continue; calculateHistogram(class_path); } closedir (dir); } else { /* could not open directory */ perror (""); exit(errno); } }
void RobotIdentifier::loadShirtImages() { std::string path; ros::param::get("/vision/tracker/team", team_color_); path = ros::package::getPath("unball"); path += "/data/camisas/"; // Loads the shirt images if (team_color_ == "Blue") { shirt_images_[0] = cv::imread(path + "red/blue.png", CV_LOAD_IMAGE_COLOR); shirt_images_[1] = cv::imread(path + "green/blue.png", CV_LOAD_IMAGE_COLOR); shirt_images_[2] = cv::imread(path + "purple/blue.png", CV_LOAD_IMAGE_COLOR); } else if (team_color_ == "Yellow") { shirt_images_[0] = cv::imread(path + "red/yellow.png", CV_LOAD_IMAGE_COLOR); shirt_images_[1] = cv::imread(path + "green/yellow.png", CV_LOAD_IMAGE_COLOR); shirt_images_[2] = cv::imread(path + "purple/yellow.png", CV_LOAD_IMAGE_COLOR); } else { ROS_ERROR("Unknown team identification"); } // Calculates shirt histograms for (int i = 0; i < 3; ++i) shirt_histograms_[i] = calculateHistogram(shirt_images_[i]); }
void ColorMatcher::addTrainingInstance(const ed::Entity& e, const std::string& property, const std::string& value) { if (property != "type") return; ColorHistogram color_histogram; calculateHistogram(e, color_histogram); std::map<std::string, ColorModel>::iterator it = models_.find(value); if (it == models_.end()) { // No other training instances for this type ColorModel& model = models_[value]; model.min = color_histogram; model.max = color_histogram; } else { ColorModel& model = it->second; // Determine maximum for each histogram bin (i.e., determine maximum for each color) for(unsigned int i = 0; i < ColorNameTable::NUM_COLORS; ++i) { model.min[i] = std::min(model.min[i], color_histogram[i]); model.max[i] = std::max(model.max[i], color_histogram[i]); } } }
// Display the count images in bgr and compute their HSV histograms. // Then compare each histogram to the first one and report results. // static void showHistogramComparisons(int count, const char *name[], const cv::Mat bgr[]) { std::vector<cv::Mat> hsv(count); std::vector<cv::Mat> histogram(count); for (int i = 0; i < count; ++i) { makeWindow(name[i], bgr[i]); cv::cvtColor(bgr[i], hsv[i], cv::COLOR_BGR2HSV); } for (int i = 0; i < histogram.size(); ++i) { histogram[i] = calculateHistogram(hsv[i]); } compareHistograms(histogram, name); std::cout << "Press a key to quit." << std::endl; cv::waitKey(0); }
void ColorMatcher::classify(const ed::Entity& e, const std::string& property, const ed::perception::CategoricalDistribution& prior, ed::perception::ClassificationOutput& output) const { tue::Configuration& result = output.data; ColorHistogram color_histogram; calculateHistogram(e, color_histogram); if (property == "color") { for(unsigned int i = 0; i < ColorNameTable::NUM_COLORS; ++i) output.likelihood.setScore(color_table_.intToColorName(i), color_histogram[i]); output.likelihood.setUnknownScore(0); return; } for(std::map<std::string, ColorModel>::const_iterator it = models_.begin(); it != models_.end(); ++it) { const std::string& model_name = it->first; const ColorModel& model = it->second; output.likelihood.setScore(model_name, 1); for(unsigned int i = 0; i < ColorNameTable::NUM_COLORS; ++i) { float v = color_histogram[i]; if (v < model.min[i] - color_margin_ || v > model.max[i] + color_margin_) output.likelihood.setScore(model_name, 0); } } // Represent data (for debugging) result.writeArray("colors"); for(unsigned int i = 0; i < ColorNameTable::NUM_COLORS; ++i) { result.addArrayItem(); result.setValue("color", ColorNameTable::intToColorName(i)); result.setValue("value", color_histogram[i]); result.endArrayItem(); } result.endArray(); }
void drawFrame() { // calculate locations g_DrawConfig.DepthLocation.uBottom = 0; g_DrawConfig.DepthLocation.uTop = WIN_SIZE_Y - 1; g_DrawConfig.DepthLocation.uLeft = 0; g_DrawConfig.DepthLocation.uRight = WIN_SIZE_X - 1; g_DrawConfig.ColorLocation.uBottom = 0; g_DrawConfig.ColorLocation.uTop = WIN_SIZE_Y - 1; g_DrawConfig.ColorLocation.uLeft = 0; g_DrawConfig.ColorLocation.uRight = WIN_SIZE_X - 1; if (g_DrawConfig.Streams.ScreenArrangement == SIDE_BY_SIDE) { g_DrawConfig.DepthLocation.uTop = WIN_SIZE_Y / 2 - 1; g_DrawConfig.DepthLocation.uRight = WIN_SIZE_X / 2 - 1; g_DrawConfig.ColorLocation.uTop = WIN_SIZE_Y / 2 - 1; g_DrawConfig.ColorLocation.uLeft = WIN_SIZE_X / 2; } // Texture map init openni::VideoFrameRef* pDepthMD = &getDepthFrame(); if (isDepthOn() && pDepthMD->isValid()) { int maxDepth = 0; maxDepth = getDepthStream().getMaxPixelValue(); g_fMaxDepth = maxDepth; TextureMapInit(&g_texDepth, pDepthMD->getVideoMode().getResolutionX(), pDepthMD->getVideoMode().getResolutionY(), 4, pDepthMD->getWidth(), pDepthMD->getHeight()); fixLocation(&g_DrawConfig.DepthLocation, pDepthMD->getVideoMode().getResolutionX(), pDepthMD->getVideoMode().getResolutionY()); } openni::VideoFrameRef* pImageMD = NULL; if (isColorOn()) { pImageMD = &getColorFrame(); } else if (isIROn()) { pImageMD = &getIRFrame(); } if (pImageMD != NULL && pImageMD->isValid()) { TextureMapInit(&g_texColor, pImageMD->getVideoMode().getResolutionX(), pImageMD->getVideoMode().getResolutionY(), 4, pImageMD->getWidth(), pImageMD->getHeight()); fixLocation(&g_DrawConfig.ColorLocation, pImageMD->getVideoMode().getResolutionX(), pImageMD->getVideoMode().getResolutionY()); } // check if pointer is over a map bool bOverDepth = (pDepthMD != NULL && pDepthMD->isValid()) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.DepthLocation); bool bOverImage = (pImageMD != NULL && pImageMD->isValid()) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.ColorLocation); bool bDrawDepthPointer = false; bool bDrawImagePointer = false; int imagePointerRed = 255; int imagePointerGreen = 0; int imagePointerBlue = 0; IntPair pointerInDepth = {0,0}; IntPair pointerInColor = {0,0}; if (bOverImage) { pointerInColor.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.ColorLocation.uLeft) / (g_DrawConfig.ColorLocation.uRight - g_DrawConfig.ColorLocation.uLeft + 1) * pImageMD->getVideoMode().getResolutionX(); pointerInColor.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.ColorLocation.uBottom) / (g_DrawConfig.ColorLocation.uTop - g_DrawConfig.ColorLocation.uBottom + 1) * pImageMD->getVideoMode().getResolutionY(); bDrawImagePointer = true; } if (bOverDepth) { pointerInDepth.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.DepthLocation.uLeft) / (g_DrawConfig.DepthLocation.uRight - g_DrawConfig.DepthLocation.uLeft + 1) * pDepthMD->getVideoMode().getResolutionX(); pointerInDepth.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.DepthLocation.uBottom) / (g_DrawConfig.DepthLocation.uTop - g_DrawConfig.DepthLocation.uBottom + 1) * pDepthMD->getVideoMode().getResolutionY(); bDrawDepthPointer = true; if (!bOverImage && g_DrawConfig.bShowPointer && pointerInDepth.X >= pDepthMD->getCropOriginX() && pointerInDepth.X < (pDepthMD->getCropOriginX() + pDepthMD->getWidth()) && pointerInDepth.Y >= pDepthMD->getCropOriginY() && pointerInDepth.Y < (pDepthMD->getCropOriginY() + pDepthMD->getHeight())) { // try to translate depth pixel to image openni::DepthPixel* pDepthPixels = (openni::DepthPixel*)pDepthMD->getData(); openni::DepthPixel pointerDepth = pDepthPixels[(pointerInDepth.Y - pDepthMD->getCropOriginY()) * pDepthMD->getWidth() + (pointerInDepth.X - pDepthMD->getCropOriginX())]; if (convertDepthPointToColor(pointerInDepth.X, pointerInDepth.Y, pointerDepth, &pointerInColor.X, &pointerInColor.Y)) { bDrawImagePointer = true; imagePointerRed = 0; imagePointerGreen = 0; imagePointerBlue = 255; } } } glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); // Setup the opengl env for fixed location view glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0,WIN_SIZE_X,WIN_SIZE_Y,0,-1.0,1.0); glDisable(GL_DEPTH_TEST); if (g_DrawConfig.Streams.Depth.Coloring == CYCLIC_RAINBOW_HISTOGRAM || g_DrawConfig.Streams.Depth.Coloring == LINEAR_HISTOGRAM || g_DrawConfig.bShowPointer) calculateHistogram(); drawColor(&g_DrawConfig.ColorLocation, bDrawImagePointer ? &pointerInColor : NULL, imagePointerRed, imagePointerGreen, imagePointerBlue); drawDepth(&g_DrawConfig.DepthLocation, bDrawDepthPointer ? &pointerInDepth : NULL); printRecordingInfo(); if (g_DrawConfig.bShowPointer) drawPointerMode(bOverDepth ? &pointerInDepth : NULL); drawUserInput(!bOverDepth && !bOverImage); drawUserMessage(); drawPlaybackSpeed(); if (g_DrawConfig.strErrorState[0] != '\0') drawErrorState(); if (g_DrawConfig.bHelp) drawHelpScreen(); glutSwapBuffers(); }
void drawFrame() { // calculate locations g_DrawConfig.DepthLocation.uBottom = 0; g_DrawConfig.DepthLocation.uTop = WIN_SIZE_Y - 1; g_DrawConfig.DepthLocation.uLeft = 0; g_DrawConfig.DepthLocation.uRight = WIN_SIZE_X - 1; g_DrawConfig.ImageLocation.uBottom = 0; g_DrawConfig.ImageLocation.uTop = WIN_SIZE_Y - 1; g_DrawConfig.ImageLocation.uLeft = 0; g_DrawConfig.ImageLocation.uRight = WIN_SIZE_X - 1; if (g_DrawConfig.Streams.ScreenArrangement == SIDE_BY_SIDE) { g_DrawConfig.DepthLocation.uTop = WIN_SIZE_Y / 2 - 1; g_DrawConfig.DepthLocation.uRight = WIN_SIZE_X / 2 - 1; g_DrawConfig.ImageLocation.uTop = WIN_SIZE_Y / 2 - 1; g_DrawConfig.ImageLocation.uLeft = WIN_SIZE_X / 2; } // Texture map init const DepthMetaData* pDepthMD = getDepthMetaData(); if (isDepthOn()) { g_nMaxDepth = getDepthGenerator()->GetDeviceMaxDepth(); TextureMapInit(&g_texDepth, pDepthMD->FullXRes(), pDepthMD->FullYRes(), 4, pDepthMD->XRes(), pDepthMD->YRes()); fixLocation(&g_DrawConfig.DepthLocation, pDepthMD->FullXRes(), pDepthMD->FullYRes()); } const MapMetaData* pImageMD = NULL; if (isImageOn()) { pImageMD = getImageMetaData(); } else if (isIROn()) { pImageMD = getIRMetaData(); } if (pImageMD != NULL) { TextureMapInit(&g_texImage, pImageMD->FullXRes(), pImageMD->FullYRes(), 4, pImageMD->XRes(), pImageMD->YRes()); fixLocation(&g_DrawConfig.ImageLocation, pImageMD->FullXRes(), pImageMD->FullYRes()); } // check if pointer is over a map bool bOverDepth = (pDepthMD != NULL) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.DepthLocation); bool bOverImage = (pImageMD != NULL) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.ImageLocation); bool bDrawDepthPointer = false; bool bDrawImagePointer = false; int imagePointerRed = 255; int imagePointerGreen = 0; int imagePointerBlue = 0; IntPair pointerInDepth; IntPair pointerInImage; if (bOverImage) { pointerInImage.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.ImageLocation.uLeft) / (g_DrawConfig.ImageLocation.uRight - g_DrawConfig.ImageLocation.uLeft + 1) * pImageMD->FullXRes(); pointerInImage.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.ImageLocation.uBottom) / (g_DrawConfig.ImageLocation.uTop - g_DrawConfig.ImageLocation.uBottom + 1) * pImageMD->FullYRes(); bDrawImagePointer = true; } if (bOverDepth) { pointerInDepth.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.DepthLocation.uLeft) / (g_DrawConfig.DepthLocation.uRight - g_DrawConfig.DepthLocation.uLeft + 1) * pDepthMD->FullXRes(); pointerInDepth.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.DepthLocation.uBottom) / (g_DrawConfig.DepthLocation.uTop - g_DrawConfig.DepthLocation.uBottom + 1) * pDepthMD->FullYRes(); // make sure we're in cropped area if (pointerInDepth.X >= pDepthMD->XOffset() && pointerInDepth.X < (pDepthMD->XOffset() + pDepthMD->XRes()) && pointerInDepth.Y >= pDepthMD->YOffset() && pointerInDepth.Y < (pDepthMD->YOffset() + pDepthMD->YRes())) { bDrawDepthPointer = true; if (!bOverImage && g_DrawConfig.bShowPointer) { // try to translate depth pixel to image if (getImageCoordinatesForDepthPixel(pointerInDepth.X, pointerInDepth.Y, pointerInImage.X, pointerInImage.Y)) { bDrawImagePointer = true; imagePointerRed = 0; imagePointerGreen = 0; imagePointerBlue = 255; } } } } glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); // Setup the opengl env for fixed location view glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0,WIN_SIZE_X,WIN_SIZE_Y,0,-1.0,1.0); glDisable(GL_DEPTH_TEST); if (g_DrawConfig.Streams.Depth.Coloring == CYCLIC_RAINBOW_HISTOGRAM || g_DrawConfig.Streams.Depth.Coloring == LINEAR_HISTOGRAM || g_DrawConfig.bShowPointer) calculateHistogram(); drawColorImage(&g_DrawConfig.ImageLocation, bDrawImagePointer ? &pointerInImage : NULL, imagePointerRed, imagePointerGreen, imagePointerBlue); drawDepth(&g_DrawConfig.DepthLocation, bDrawDepthPointer ? &pointerInDepth : NULL); printStatisticsInfo(); printRecordingInfo(); if (g_DrawConfig.bShowPointer) drawPointerMode(bDrawDepthPointer ? &pointerInDepth : NULL); drawUserInput(!bOverDepth && !bOverImage); drawUserMessage(); drawPlaybackSpeed(); if (g_DrawConfig.strErrorState != NULL) drawErrorState(); if (g_DrawConfig.bHelp) drawHelpScreen(); glutSwapBuffers(); }
void SampleViewer::display() { if (!m_pClosestPointListener->isAvailable()) { return; } openni::VideoFrameRef depthFrame = m_pClosestPointListener->getFrame(); const closest_point::IntPoint3D& closest = m_pClosestPointListener->getClosestPoint(); m_pClosestPointListener->setUnavailable(); if (m_pTexMap == NULL) { // Texture map init m_nTexMapX = MIN_CHUNKS_SIZE(depthFrame.getWidth(), TEXTURE_SIZE); m_nTexMapY = MIN_CHUNKS_SIZE(depthFrame.getHeight(), TEXTURE_SIZE); m_pTexMap = new openni::RGB888Pixel[m_nTexMapX * m_nTexMapY]; } glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0); if (depthFrame.isValid()) { calculateHistogram(m_pDepthHist, MAX_DEPTH, depthFrame); } memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel)); float factor[3] = {1, 1, 1}; // check if we need to draw depth frame to texture if (depthFrame.isValid()) { const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)depthFrame.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + depthFrame.getCropOriginY() * m_nTexMapX; int rowSize = depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel); int width = depthFrame.getWidth(); int height = depthFrame.getHeight(); for (int y = 0; y < height; ++y) { const openni::DepthPixel* pDepth = pDepthRow; openni::RGB888Pixel* pTex = pTexRow + depthFrame.getCropOriginX(); for (int x = 0; x < width; ++x, ++pDepth, ++pTex) { if (*pDepth != 0) { if (*pDepth == closest.Z) { factor[0] = factor[1] = 0; } // // Add debug lines - every 10cm // else if ((*pDepth / 10) % 10 == 0) // { // factor[0] = factor[2] = 0; // } int nHistValue = m_pDepthHist[*pDepth]; pTex->r = nHistValue*factor[0]; pTex->g = nHistValue*factor[1]; pTex->b = nHistValue*factor[2]; factor[0] = factor[1] = factor[2] = 1; } } pDepthRow += rowSize; pTexRow += m_nTexMapX; } } 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, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glEnable(GL_TEXTURE_2D); glBegin(GL_QUADS); int nXRes = depthFrame.getWidth(); int nYRes = depthFrame.getHeight(); // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)nXRes/(float)m_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)nXRes/(float)m_nTexMapX, (float)nYRes/(float)m_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)nYRes/(float)m_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); glDisable(GL_TEXTURE_2D); float closestCoordinates[3] = {closest.X*GL_WIN_SIZE_X/float(depthFrame.getWidth()), closest.Y*GL_WIN_SIZE_Y/float(depthFrame.getHeight()), 0}; glVertexPointer(3, GL_FLOAT, 0, closestCoordinates); glColor3f(1.f, 0.f, 0.f); glPointSize(10); glDrawArrays(GL_POINTS, 0, 1); glFlush(); // Swap the OpenGL display buffers glutSwapBuffers(); }
bool CHistogram2D::compute() throw() { typedef boost::shared_ptr<ImageType> ImagePtr; typedef typename ImageType::TDataType ValueType; ImagePtr imagePtr = static_pointer_cast<ImageType>( getInput() ); ImagePtr classPtr = static_pointer_cast<ImageType>( getInput(1) ); ImagePtr gradientPtr = static_pointer_cast<ImageType>( getInput(2) ); if ( !checkInput<ImageType>( imagePtr, 2, 3 ) || !checkInput<ImageType>( classPtr, 2, 3 ) || !checkInput<ImageType>( gradientPtr, 2, 3 ) ) { return false; } bModuleReady = true; deleteOldOutput(); double dClassOneMean = 0.0; double dClassOneVariance = 0.0; vector<ValueType> classOneValuesVec; double dClassTwoMean = 0.0; double dClassTwoVariance = 0.0; vector<ValueType> classTwoValuesVec; uint uiClassOne = parameters.getUnsignedLong( "Class1" ); uint uiClassTwo = parameters.getUnsignedLong( "Class2" ); cerr << uiClassOne << " " << uiClassTwo << endl; // Calculate intensity mean and variance of the selected classes typename ImageType::iterator classIt = classPtr->begin(); typename ImageType::iterator imageIt = imagePtr->begin(); while( classIt != classPtr->end() ) { if ( *classIt == static_cast<ValueType>( uiClassOne ) ) classOneValuesVec.push_back( *imageIt ); else if ( *classIt == static_cast<ValueType>( uiClassTwo ) ) classTwoValuesVec.push_back( *imageIt ); ++classIt; ++imageIt; } for_each( classOneValuesVec.begin(), classOneValuesVec.end(), dClassOneMean += _1 ); dClassOneMean /= static_cast<double>( classOneValuesVec.size() ); for_each( classOneValuesVec.begin(), classOneValuesVec.end(), dClassOneVariance += ( _1 - dClassOneMean ) * ( _1 - dClassOneMean ) ); dClassOneVariance /= static_cast<double>( classOneValuesVec.size() ); dClassOneVariance = sqrt( dClassOneVariance ); for_each( classTwoValuesVec.begin(), classTwoValuesVec.end(), dClassTwoMean += _1 ); dClassTwoMean /= static_cast<double>( classTwoValuesVec.size() ); for_each( classTwoValuesVec.begin(), classTwoValuesVec.end(), dClassTwoVariance += ( _1 - dClassTwoMean ) * ( _1 - dClassTwoMean ) ); dClassTwoVariance /= static_cast<double>( classTwoValuesVec.size() ); dClassTwoVariance = sqrt( dClassTwoVariance ); cerr << " C1 (" << uiClassOne << ") : Mean " << dClassOneMean << " SD " << dClassOneVariance << endl << " C2 (" << uiClassTwo << ") : Mean " << dClassTwoMean << " SD " << dClassTwoVariance << endl; int rangeMin, rangeMax; rangeMin = static_cast<int>( std::min( dClassOneMean - dClassOneVariance, dClassTwoMean - dClassTwoVariance ) ); rangeMax = static_cast<int>( std::max( dClassOneMean - dClassOneVariance, dClassTwoMean - dClassTwoVariance ) ); ImagePtr tmp( new ImageType( *gradientPtr ) ); classIt = imagePtr->begin(); imageIt = tmp->begin(); while( classIt != imagePtr->end() ) { if ( *classIt < rangeMin || *classIt > rangeMax ) *imageIt = 0; ++classIt; ++imageIt; } calculateHistogram( tmp ); // Do a gaussian smoothing on the histogram // Compute gaussian mask double dSigma = parameters.getDouble( "HistoSmooth" ); cerr << "HistoSmooth " << dSigma << endl; int iMaskSize = static_cast<uint>( floor( 6.0 * dSigma + 0.5 ) ); vector<double> dGaussMask; for( int i = -( iMaskSize / 2 ); i <= ( iMaskSize / 2 ); ++i ) { double dExponential = ( 1.0 / sqrt( dSigma * dSigma * 2.0 * M_PI ) ) * exp( static_cast<double>( -i * i ) / ( 2.0 * ( dSigma * dSigma ) ) ); dGaussMask.push_back( dExponential ); } cerr << "Mask size " << iMaskSize << endl; // Shift histogram to attend smoothing vector<double> shiftedHisto; for( int i = 0; i < iMaskSize/2; ++i ) shiftedHisto.push_back(0.0); for( uint i = 0; i < theHistogramVec[0].size(); ++i ) { shiftedHisto.push_back( theHistogramVec[0][i] ); } for( int i = 0; i <= iMaskSize/2; ++i ) shiftedHisto.push_back(0.0); const uint uiShift = iMaskSize/2; vector<double> smoothedHisto( shiftedHisto.size() + uiShift); for( uint i = uiShift+1; i < shiftedHisto.size() - uiShift; ++i ) { double smoothed = 0.0; for( int j = -(uiShift+1); j <= uiShift; ++j ) { smoothed += shiftedHisto[i+j]*dGaussMask[j+uiShift+1]; } smoothedHisto[i] = smoothed; } smoothedHisto = shiftedHisto; // Smooth // Reshift histogram // Determine the global maximum g1 uint g1 = 0; double gv1 = 0.0; for( uint i = uiShift+1; i < smoothedHisto.size(); ++i ) { if ( smoothedHisto[i] >= smoothedHisto[i-1] && smoothedHisto[i] >= gv1 ) { gv1 = smoothedHisto[i]; g1 = i; } else if ( smoothedHisto[i] < smoothedHisto[i-1] ) break; } // Determine next global maximum g2 /* if ( g1 == 0 ) g1 = parameters.getUnsignedLong("Shift");*/ uint t = g1 + parameters.getUnsignedLong("Shift"); while( smoothedHisto[t] <= smoothedHisto[t-1] ) ++t; uint g2 = t+1; double gv2 = 0; for( uint i = t+1; i < smoothedHisto.size(); ++i ) { if ( smoothedHisto[i] > smoothedHisto[i-1] && smoothedHisto[i] > gv2 ) { gv2 = smoothedHisto[i]; g2 = i; } else if ( smoothedHisto[i] < smoothedHisto[i-1] ) break; } if ( g1 == 0 ) { g1 = g2; t = g1+parameters.getUnsignedLong("Shift"); while( smoothedHisto[t] <= smoothedHisto[t-1] ) ++t; g2 = t+1; gv2 = 0; for( uint i = t+1; i < smoothedHisto.size(); ++i ) { if ( smoothedHisto[i] > smoothedHisto[i-1] && smoothedHisto[i] > gv2 ) { gv2 = smoothedHisto[i]; g2 = i; } else if ( smoothedHisto[i] < smoothedHisto[i-1] ) break; } } if ( g2-g1 > (2 * parameters.getUnsignedLong("Shift")) ) g2 = g1 + 2 * parameters.getUnsignedLong("Shift"); g1--; g2--; cerr << "Threshold values are " << g1 << " and " << g2; parameters.setUnsignedLong("T1", static_cast<ulong>( g1 * 0.33)-1 ); parameters.setUnsignedLong("T2", static_cast<ulong>( std::max( g1 * 0.66, g1 + parameters.getDouble("Shift") + 1) ) - 1 ); ofstream file( "/home/hendrik/orig_histogram.data" ); for( uint i = 0; i <= theHistogramVec[0].size(); ++i ) { file << i << " " << theHistogramVec[0][i] << endl; } file.close(); ofstream file2( "/home/hendrik/shifted_histogram.data" ); for( uint i = 0; i <= shiftedHisto.size(); ++i ) { file2 << i << " " << shiftedHisto[i] << endl; } file2.close(); ofstream file3( "/home/hendrik/smoothed_histogram.data" ); for( uint i = 0; i <= smoothedHisto.size(); ++i ) { file3 << i << " " << smoothedHisto[i] << endl; } file3.close(); return true; }
void SampleViewer::display() { int changedIndex; openni::Status rc = openni::OpenNI::waitForAnyStream(m_streams, 2, &changedIndex); if (rc != openni::STATUS_OK) { printf("Wait failed\n"); return; } switch (changedIndex) { case 0: m_depth1.readFrame(&m_depth1Frame); break; case 1: m_depth2.readFrame(&m_depth2Frame); break; default: printf("Error in wait\n"); } glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0); if (m_depth1Frame.isValid() && m_eViewState != DISPLAY_MODE_DEPTH2) calculateHistogram(m_pDepthHist, MAX_DEPTH, m_depth1Frame); else calculateHistogram(m_pDepthHist, MAX_DEPTH, m_depth2Frame); memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel)); // check if we need to draw image frame to texture switch (m_eViewState) { case DISPLAY_MODE_DEPTH1: displayFrame(m_depth1Frame); break; case DISPLAY_MODE_DEPTH2: displayFrame(m_depth2Frame); break; default: displayBothFrames(); } 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, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glBegin(GL_QUADS); int nXRes = m_width; int nYRes = m_height; // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)nXRes/(float)m_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)nXRes/(float)m_nTexMapX, (float)nYRes/(float)m_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)nYRes/(float)m_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); // Swap the OpenGL display buffers glutSwapBuffers(); }
void SampleViewer::Display() { // namespace bg = boost::geometry; nite::UserTrackerFrameRef userTrackerFrame; openni::VideoFrameRef depthFrame; nite::Status rc = m_pUserTracker->readFrame(&userTrackerFrame); if (rc != nite::STATUS_OK) { printf("GetNextData failed\n"); return; } depthFrame = userTrackerFrame.getDepthFrame(); if (m_pTexMap == NULL) { // Texture map init m_nTexMapX = MIN_CHUNKS_SIZE(depthFrame.getVideoMode().getResolutionX(), TEXTURE_SIZE); m_nTexMapY = MIN_CHUNKS_SIZE(depthFrame.getVideoMode().getResolutionY(), TEXTURE_SIZE); m_pTexMap = new openni::RGB888Pixel[m_nTexMapX * m_nTexMapY]; } const nite::UserMap& userLabels = userTrackerFrame.getUserMap(); glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0); if (depthFrame.isValid() && g_drawDepth) { calculateHistogram(m_pDepthHist, MAX_DEPTH, depthFrame); } memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel)); float factor[3] = {1, 1, 1}; // check if we need to draw depth frame to texture if (depthFrame.isValid() && g_drawDepth) { const nite::UserId* pLabels = userLabels.getPixels(); const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)depthFrame.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + depthFrame.getCropOriginY() * m_nTexMapX; int rowSize = depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel); for (int y = 0; y < depthFrame.getHeight(); ++y) { const openni::DepthPixel* pDepth = pDepthRow; openni::RGB888Pixel* pTex = pTexRow + depthFrame.getCropOriginX(); for (int x = 0; x < depthFrame.getWidth(); ++x, ++pDepth, ++pTex, ++pLabels) { if (*pDepth != 0) { if (*pLabels == 0) { if (!g_drawBackground) { factor[0] = factor[1] = factor[2] = 0; } else { factor[0] = Colors[colorCount][0]; factor[1] = Colors[colorCount][1]; factor[2] = Colors[colorCount][2]; } } else { factor[0] = Colors[*pLabels % colorCount][0]; factor[1] = Colors[*pLabels % colorCount][1]; factor[2] = Colors[*pLabels % colorCount][2]; } // // Add debug lines - every 10cm // else if ((*pDepth / 10) % 10 == 0) // { // factor[0] = factor[2] = 0; // } int nHistValue = m_pDepthHist[*pDepth]; pTex->r = nHistValue*factor[0]; pTex->g = nHistValue*factor[1]; pTex->b = nHistValue*factor[2]; factor[0] = factor[1] = factor[2] = 1; } } pDepthRow += rowSize; pTexRow += m_nTexMapX; } } 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, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glEnable(GL_TEXTURE_2D); glBegin(GL_QUADS); g_nXRes = depthFrame.getVideoMode().getResolutionX(); g_nYRes = depthFrame.getVideoMode().getResolutionY(); // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, (float)g_nYRes/(float)m_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)g_nYRes/(float)m_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); glDisable(GL_TEXTURE_2D); const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers(); // 11.16.15 remove for loop for tracking only one user // for (int i = 0; i < users.getSize(); ++i) // { //const nite::UserData& user = users[i]; if (users.getSize() > 0) { const nite::UserData& user = users[0]; updateUserState(user, userTrackerFrame.getTimestamp()); if (user.isNew()) { m_pUserTracker->startSkeletonTracking(user.getId()); m_pUserTracker->startPoseDetection(user.getId(), nite::POSE_CROSSED_HANDS); } else if (!user.isLost()) { if (g_drawStatusLabel) { DrawStatusLabel(m_pUserTracker, user); } if (g_drawCenterOfMass) { DrawCenterOfMass(m_pUserTracker, user); } if (g_drawBoundingBox) { DrawBoundingBox(user); } if (user.getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawSkeleton) { DrawSkeleton(m_pUserTracker, user); } } if (m_poseUser == 0 || m_poseUser == user.getId()) { const nite::PoseData& pose = user.getPose(nite::POSE_CROSSED_HANDS); if (pose.isEntered()) { // Start timer sprintf(g_generalMessage, "In exit pose. Keep it for %d second%s to exit\n", g_poseTimeoutToExit/1000, g_poseTimeoutToExit/1000 == 1 ? "" : "s"); printf("Counting down %d second to exit\n", g_poseTimeoutToExit/1000); m_poseUser = user.getId(); m_poseTime = userTrackerFrame.getTimestamp(); } else if (pose.isExited()) { memset(g_generalMessage, 0, sizeof(g_generalMessage)); printf("Count-down interrupted\n"); m_poseTime = 0; m_poseUser = 0; } else if (pose.isHeld()) { // tick if (userTrackerFrame.getTimestamp() - m_poseTime > g_poseTimeoutToExit * 1000) { printf("Count down complete. Exit...\n"); Finalize(); exit(2); } } } //user char buffer[80] = ""; // ---------------- calculate right shoulder 2 DOF ---------------------- int rightShoulderX, rightShoulderY, rightShoulderZ; rightShoulderX = user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().x - user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER).getPosition().x; rightShoulderY = user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().y - user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER).getPosition().y; rightShoulderZ = user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().z - user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER).getPosition().z; Spherical rightShoulderSpherical; rightShoulderSpherical = Cartesian2Spherical(rightShoulderX, rightShoulderY, rightShoulderZ); int rightShoulderPhi, rightShoulderTheta; rightShoulderTheta = radian2Degree(rightShoulderSpherical.radianTheta, rightShoulderThetaInit); // horizontal rise it's -60 degree rightShoulderPhi = radian2Degree(rightShoulderSpherical.radianPhi, rightShoulderPitchInit); // when hand's down, it's 70 degree int rightShoulderYawNow = rightShoulderPhi * sin(rightShoulderTheta * PI / 180); int rightShoulderPitchNow = rightShoulderPhi * cos(rightShoulderTheta * PI / 180); sprintf(buffer,"(rightShoulderTheta=%d, rightShoulderYaw=%d, rightShoulderPitch=%d)", rightShoulderTheta, rightShoulderYawNow, rightShoulderPitchNow); glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(20, 20); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); // ------------------- calculate right elbow 2 DOF ----------------------- int rightElbowX, rightElbowY, rightElbowZ; rightElbowX = user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND).getPosition().x - user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().x; rightElbowY = user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND).getPosition().y - user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().y; rightElbowZ = user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND).getPosition().z - user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().z; Spherical rightElbowSpherical; rightElbowSpherical = Cartesian2Spherical(rightElbowX, rightElbowY, rightElbowZ); int rightElbowThetaNow = - radian2Degree(rightElbowSpherical.radianTheta, rightElbowThetaInit); // reverse for system int rightElbowYawNow = radian2Degree(rightElbowSpherical.radianPhi, rightElbowYawInit); sprintf(buffer,"(rightElbowThetaNow=%d, rightElbowYawNow=%d)", rightElbowThetaNow, rightElbowYawNow); glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(20, 60); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); // ---------------- calculate left shoulder 2 DOF ---------------------- int leftShoulderX, leftShoulderY, leftShoulderZ; leftShoulderX = user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().x - user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().x; leftShoulderY = user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().y - user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().y; leftShoulderZ = user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().z - user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().z; Spherical leftShoulderSpherical; leftShoulderSpherical = Cartesian2Spherical(leftShoulderX, leftShoulderY, leftShoulderZ); int leftShoulderPhi, leftShoulderTheta; leftShoulderTheta = radian2Degree(leftShoulderSpherical.radianTheta, leftShoulderThetaInit); // horizontal rise it's -60 degree leftShoulderPhi = radian2Degree(leftShoulderSpherical.radianPhi, leftShoulderPitchInit); // when hand's down, it's 70 degree // need to reverse in left side int leftShoulderYawNow = - leftShoulderPhi * sin(leftShoulderTheta * PI / 180); int leftShoulderPitchNow = - leftShoulderPhi * cos(leftShoulderTheta * PI / 180); sprintf(buffer,"(leftShoulderTheta=%d, leftShoulderYaw=%d, leftShoulderPitch=%d)", leftShoulderTheta, leftShoulderYawNow, leftShoulderPitchNow); glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(20, 100); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); // ------------------- calculate left elbow 2 DOF ----------------------- int leftElbowX, leftElbowY, leftElbowZ; leftElbowX = user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().x - user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().x; leftElbowY = user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().y - user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().y; leftElbowZ = user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().z - user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().z; Spherical leftElbowSpherical; leftElbowSpherical = Cartesian2Spherical(leftElbowX, leftElbowY, leftElbowZ); int leftElbowThetaNow = radian2Degree(leftElbowSpherical.radianTheta, leftElbowThetaInit); int leftElbowYawNow = radian2Degree(leftElbowSpherical.radianPhi, leftElbowYawInit); sprintf(buffer,"(leftElbowTheta=%d, leftElbowYawNow=%d)", leftElbowThetaNow, leftElbowYawNow); glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(20, 140); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); // ---------------- constraint movement and publish message ------------ int rightShoulderYawDiff = abs(rightShoulderYaw - rightShoulderYawNow); int rightShoulderPitchDiff = abs(rightShoulderPitch - rightShoulderPitchNow); int rightElbowThetaDiff = abs(rightElbowTheta - rightElbowThetaNow); int rightElbowYawDiff = abs(rightElbowYaw - rightElbowYawNow); int leftShoulderYawDiff = abs(leftShoulderYaw - leftShoulderYawNow); int leftShoulderPitchDiff = abs(leftShoulderPitch - leftShoulderPitchNow); int leftElbowThetaDiff = abs(leftElbowTheta - leftElbowThetaNow); int leftElbowYawDiff = abs(leftElbowYaw - leftElbowYawNow); if ((rightShoulderYawDiff < moveLimitDegree) && (rightShoulderPitchDiff < moveLimitDegree) && (rightElbowThetaDiff < moveLimitDegree) && (rightElbowYawDiff < moveLimitDegree) && (rightShoulderTheta >= 0)) { // in range then refresh robot angle rightShoulderYaw = rightShoulderYawNow; rightShoulderPitch = rightShoulderPitchNow; rightElbowTheta = rightElbowThetaNow; rightElbowYaw = rightElbowYawNow; // change the angle to 0 to 360 and publish rightShoulderYawPub = angleHandler(rightShoulderYaw); rightShoulderPitchPub = angleHandler(rightShoulderPitch); rightElbowThetaPub = angleHandler(rightElbowTheta); rightElbowYawPub = angleHandler(rightElbowYaw); sprintf(buffer,"tracking!"); glColor3f(0.0f, 0.0f, 1.0f); glRasterPos2i(20, 180); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); actionPublish(rightShoulderYawPub, rightShoulderPitchPub, rightElbowThetaPub, rightElbowYawPub, leftShoulderYawPub, leftShoulderPitchPub, leftElbowThetaPub, leftElbowYawPub); } else { sprintf(buffer,"soulder yaw: %d, soulder pitch: %d, elbow yaw: %d, rightShoulderTheta > 0: %d", rightShoulderYaw - rightShoulderYawNow, rightShoulderPitch - rightShoulderPitchNow, rightElbowYaw - rightElbowYawNow, rightShoulderTheta); glColor3f(1.0f, 0.0f, 0.5f); glRasterPos2i(20, 180); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); } if((leftShoulderYawDiff < moveLimitDegree) && (leftShoulderPitchDiff < moveLimitDegree) && (leftElbowThetaDiff < moveLimitDegree) && (leftElbowYawDiff < moveLimitDegree) && (leftShoulderTheta >= 0)) { leftShoulderYaw = leftShoulderYawNow; leftShoulderPitch = leftShoulderPitchNow; leftElbowTheta = leftElbowThetaNow; leftElbowYaw = leftElbowYawNow; leftShoulderYawPub = angleHandler(leftShoulderYaw); leftShoulderPitchPub = angleHandler(leftShoulderPitch); leftElbowThetaPub = angleHandler(leftElbowTheta); leftElbowYawPub = angleHandler(leftElbowYaw); sprintf(buffer,"tracking!"); glColor3f(0.0f, 0.3f, 1.0f); glRasterPos2i(20, 220); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); actionPublish(rightShoulderYawPub, rightShoulderPitchPub, rightElbowThetaPub, rightElbowYawPub, leftShoulderYawPub, leftShoulderPitchPub, leftElbowThetaPub, leftElbowYawPub); } else { sprintf(buffer,"soulder yaw: %d, soulder pitch: %d, elbow yaw: %d, leftShoulderTheta > 0: %d", leftShoulderYaw - leftShoulderYawNow, leftShoulderPitch - leftShoulderPitchNow, leftElbowYaw - leftElbowYawNow, leftShoulderTheta); glColor3f(0.7f, 0.8f, 2.0f); glRasterPos2i(20, 220); glPrintString(GLUT_BITMAP_HELVETICA_18, buffer); } } if (g_drawFrameId) { DrawFrameId(userTrackerFrame.getFrameIndex()); } if (g_generalMessage[0] != '\0') { char *msg = g_generalMessage; glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(100, 20); glPrintString(GLUT_BITMAP_HELVETICA_18, msg); } // Swap the OpenGL display buffers glutSwapBuffers(); }
void SampleViewer::display() { int changedIndex; openni::Status rc = openni::OpenNI::waitForAnyStream(m_streams, 2, &changedIndex); if (rc != openni::STATUS_OK) { printf("Wait failed\n"); return; } switch (changedIndex) { case 0: m_depthStream.readFrame(&m_depthFrame); break; case 1: m_colorStream.readFrame(&m_colorFrame); break; default: printf("Error in wait\n"); } glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0); if (m_depthFrame.isValid()) { calculateHistogram(m_pDepthHist, MAX_DEPTH, m_depthFrame); } memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel)); // check if we need to draw image frame to texture if ((m_eViewState == DISPLAY_MODE_OVERLAY || m_eViewState == DISPLAY_MODE_IMAGE) && m_colorFrame.isValid()) { const openni::RGB888Pixel* pImageRow = (const openni::RGB888Pixel*)m_colorFrame.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + m_colorFrame.getCropOriginY() * m_nTexMapX; int rowSize = m_colorFrame.getStrideInBytes() / sizeof(openni::RGB888Pixel); for (int y = 0; y < m_colorFrame.getHeight(); ++y) { const openni::RGB888Pixel* pImage = pImageRow; openni::RGB888Pixel* pTex = pTexRow + m_colorFrame.getCropOriginX(); for (int x = 0; x < m_colorFrame.getWidth(); ++x, ++pImage, ++pTex) { *pTex = *pImage; } pImageRow += rowSize; pTexRow += m_nTexMapX; } } // check if we need to draw depth frame to texture if ((m_eViewState == DISPLAY_MODE_OVERLAY || m_eViewState == DISPLAY_MODE_DEPTH) && m_depthFrame.isValid()) { const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)m_depthFrame.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + m_depthFrame.getCropOriginY() * m_nTexMapX; int rowSize = m_depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel); for (int y = 0; y < m_depthFrame.getHeight(); ++y) { const openni::DepthPixel* pDepth = pDepthRow; openni::RGB888Pixel* pTex = pTexRow + m_depthFrame.getCropOriginX(); for (int x = 0; x < m_depthFrame.getWidth(); ++x, ++pDepth, ++pTex) { if (*pDepth != 0) { int nHistValue = m_pDepthHist[*pDepth]; pTex->r = nHistValue; pTex->g = nHistValue; pTex->b = 0; } } pDepthRow += rowSize; pTexRow += m_nTexMapX; } } 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, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glBegin(GL_QUADS); int nXRes = m_width; int nYRes = m_height; // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)nXRes/(float)m_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)nXRes/(float)m_nTexMapX, (float)nYRes/(float)m_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)nYRes/(float)m_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); // Swap the OpenGL display buffers glutSwapBuffers(); KVector4 vec; m_device.getProperty( KINECT_DEVICE_PROPERTY_ACCELEROMETER, &vec ); printf( "%f, %f, %f\n", vec.x, vec.y, vec.z ); }
void SampleViewer::Display() { if (g_pause) return; nite::UserTrackerFrameRef userTrackerFrame; nite::Status rc1 = m_pUserTracker->readFrame(&userTrackerFrame); if (rc1 != nite::STATUS_OK) { printf("GetNextData failed\n"); return; } openni::VideoFrameRef depthFrameSide = userTrackerFrame.getDepthFrame(); int height = depthFrameSide.getHeight(); int width = depthFrameSide.getWidth(); if (!label) { label = (int *)malloc(width*height*sizeof(int)); } openni::VideoFrameRef depthFrameTop; openni::Status rc2 = depthStreamTop.readFrame(&depthFrameTop); if (rc2 != openni::STATUS_OK) { printf("GetNextData failed\n"); return; } if (m_pTexMap == NULL) { // Texture map init m_nTexMapX = MIN_CHUNKS_SIZE(depthFrameSide.getVideoMode().getResolutionX(), TEXTURE_SIZE); m_nTexMapY = MIN_CHUNKS_SIZE(depthFrameSide.getVideoMode().getResolutionY(), TEXTURE_SIZE); m_pTexMap = new openni::RGB888Pixel[m_nTexMapX * m_nTexMapY]; } const nite::UserMap& userLabels = userTrackerFrame.getUserMap(); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0); if (depthFrameSide.isValid() && g_drawDepth) { calculateHistogram(m_pDepthHistSide, MAX_DEPTH, depthFrameSide); } memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel)); float factor[3] = {1, 1, 1}; // check if we need to draw depth frame to texture if (depthFrameSide.isValid() && g_drawDepth) { const nite::UserId* pLabels = userLabels.getPixels(); const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)depthFrameSide.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + depthFrameSide.getCropOriginY() * m_nTexMapX; int rowSize = depthFrameSide.getStrideInBytes() / sizeof(openni::DepthPixel); for (int y = 0; y < height; ++y) { const openni::DepthPixel* pDepth = pDepthRow; openni::RGB888Pixel* pTex = pTexRow + depthFrameSide.getCropOriginX(); for (int x = 0; x < width; ++x, ++pDepth, ++pTex, ++pLabels) { if (*pDepth != 0) { if (*pLabels == 0) { if (!g_drawBackground) { factor[0] = factor[1] = factor[2] = 0; } else { factor[0] = Colors[colorCount][0]; factor[1] = Colors[colorCount][1]; factor[2] = Colors[colorCount][2]; } } else { factor[0] = Colors[*pLabels % colorCount][0]; factor[1] = Colors[*pLabels % colorCount][1]; factor[2] = Colors[*pLabels % colorCount][2]; } // // Add debug lines - every 10cm // else if ((*pDepth / 10) % 10 == 0) // { // factor[0] = factor[2] = 0; // } int nHistValue = m_pDepthHistSide[*pDepth]; pTex->r = nHistValue*factor[0]; pTex->g = nHistValue*factor[1]; pTex->b = nHistValue*factor[2]; factor[0] = factor[1] = factor[2] = 1; } } pDepthRow += rowSize; pTexRow += m_nTexMapX; } } const openni::DepthPixel *imgBufferSide = (const openni::DepthPixel *)depthFrameSide.getData(); const openni::DepthPixel *imgBufferTop = (const openni::DepthPixel *)depthFrameTop.getData(); calculateHistogram(m_pDepthHistTop, MAX_DEPTH, depthFrameTop); imgTop = Mat(depthFrameTop.getHeight(), depthFrameTop.getWidth(), CV_8UC3); Mat(depthFrameTop.getHeight(), depthFrameTop.getWidth(), CV_16U, (void *)imgBufferTop).convertTo(depthTop, CV_8U, 1.0/256); for (int i = 0; i < imgTop.rows; i++) { for (int j = 0; j < imgTop.cols; j++) { int val = (int)m_pDepthHistTop[imgBufferTop[j + i*imgTop.cols]]; imgTop.at<Vec3b>(i, j).val[0] = val; imgTop.at<Vec3b>(i, j).val[1] = val; imgTop.at<Vec3b>(i, j).val[2] = val; } } if (g_getBackground) bgSubtractor->processImages(depthTop); bgSubtractor->getMask(depthTop, mask); imshow("Mask", mask); 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, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glEnable(GL_TEXTURE_2D); glBegin(GL_QUADS); // 320x240 g_nXRes = depthFrameSide.getVideoMode().getResolutionX(); g_nYRes = depthFrameSide.getVideoMode().getResolutionY(); // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, (float)g_nYRes/(float)m_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)g_nYRes/(float)m_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); glDisable(GL_TEXTURE_2D); const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers(); float maxSize = -1; int maxIdx = -1; for (int i = 0; i < users.getSize(); ++i) { const nite::UserData &user = users[i]; if (!user.isVisible()) continue; if (getSize(user) > maxSize) { maxSize = getSize(user); maxIdx = i; } //printf("user %d: size=%f\n, lost=%d, new=%d, visible=%d\n", // i, getSize(user), user.isLost(), user.isNew(), user.isVisible()); } for (int i = 0; i < users.getSize(); ++i) { const nite::UserData &user = users[i]; updateUserState(user, userTrackerFrame.getTimestamp()); if (user.isNew()) { m_pUserTracker->startSkeletonTracking(user.getId()); m_pUserTracker->startPoseDetection(user.getId(), nite::POSE_CROSSED_HANDS); } else if (!user.isLost()) { if (g_drawStatusLabel) { DrawStatusLabel(m_pUserTracker, user); } if (g_drawCenterOfMass) { DrawCenterOfMass(m_pUserTracker, user); } if (g_drawBoundingBox) { DrawBoundingBox(user); } if (users[i].getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawSkeleton) { if (maxIdx == i) { DrawSkeleton(m_pUserTracker, user); sideSkel.setTo(Scalar(0, 0, 0)); drawSkeleton(sideSkel, sideJoints); topSkel.setTo(Scalar(0, 0, 0)); drawSkeleton(topSkel, topJoints); drawSkeleton(imgTop, topJoints); } } } // exit the program after a few seconds if PoseType == POSE_CROSSED_HANDS if (m_poseUser == 0 || m_poseUser == user.getId()) { const nite::PoseData& pose = user.getPose(nite::POSE_CROSSED_HANDS); if (pose.isEntered()) { // Start timer sprintf(g_generalMessage, "In exit pose. Keep it for %d second%s to exit\n", g_poseTimeoutToExit/1000, g_poseTimeoutToExit/1000 == 1 ? "" : "s"); printf("Counting down %d second to exit\n", g_poseTimeoutToExit/1000); m_poseUser = user.getId(); m_poseTime = userTrackerFrame.getTimestamp(); } else if (pose.isExited()) { memset(g_generalMessage, 0, sizeof(g_generalMessage)); printf("Count-down interrupted\n"); m_poseTime = 0; m_poseUser = 0; } else if (pose.isHeld()) { // tick if (userTrackerFrame.getTimestamp() - m_poseTime > g_poseTimeoutToExit * 1000) { printf("Count down complete. Exit...\n"); Finalize(); exit(2); } } } } if (g_drawFrameId) { DrawFrameId(userTrackerFrame.getFrameIndex()); } if (g_generalMessage[0] != '\0') { char *msg = g_generalMessage; glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(100, 20); glPrintString(GLUT_BITMAP_HELVETICA_18, msg); } imshow("Side", sideSkel); imshow("Top", topSkel); imshow("DepthTop", imgTop); if (!g_getBackground) { knnsearch(topJoints, imgBufferTop, mask, labelTop, label, 320, 240); drawSkeleton(labelTop, topJoints); cv::resize(labelTop, labelTop, Size(), 2, 2); imshow("Label", labelTop); if (g_capture2) { // c style string path = outDir + "/depth-top" + to_string(nFrame) + ".txt"; FILE *f = fopen(path.c_str(), "w"); for (int i = 0; i < width*height; i++) { fprintf(f, "%u\n", imgBufferTop[i]); } fclose(f); path = outDir + "/depth-side" + to_string(nFrame) + ".txt"; f = fopen(path.c_str(), "w"); for (int i = 0; i < width*height; i++) { fprintf(f, "%u\n", imgBufferSide[i]); } fclose(f); path = outDir + "/joints-top" + to_string(nFrame) + ".txt"; f = fopen(path.c_str(), "w"); for (int i = 0; i < N_JOINTS; i++) { fprintf(f, "%f, %f, %f, %f, %f\n", topJoints[i][0], topJoints[i][1], topJoints[i][2], topJoints[i][3], topJoints[i][4]); } fclose(f); path = outDir + "/joints-side" + to_string(nFrame) + ".txt"; f = fopen(path.c_str(), "w"); for (int i = 0; i < N_JOINTS; i++) { fprintf(f, "%f, %f, %f, %f, %f\n", sideJoints[i][0], sideJoints[i][1], sideJoints[i][2], sideJoints[i][3], sideJoints[i][4]); } fclose(f); path = outDir + "/label-top" + to_string(nFrame) + ".txt"; f = fopen(path.c_str(), "w"); for (int i = 0; i < width*height; i++) { fprintf(f, "%d\n", label[i]); } fclose(f); path = outDir + "/label-side" + to_string(nFrame) + ".txt"; f = fopen(path.c_str(), "w"); const nite::UserId* labelsTop = userLabels.getPixels(); for (int i = 0; i < width*height; i++) { fprintf(f, "%d\n", (int)labelsTop[i]); } fclose(f); nFrame++; } } // Swap the OpenGL display buffers glutSwapBuffers(); }
void SampleViewer::Display() { nite::UserTrackerFrameRef userTrackerFrame; openni::VideoFrameRef depthFrame; nite::Status rc = m_pUserTracker->readFrame(&userTrackerFrame); if (rc != nite::STATUS_OK) { printf("GetNextData failed\n"); return; } depthFrame = userTrackerFrame.getDepthFrame(); m_colorStream.readFrame(&m_colorFrame); if (m_pTexMap == NULL) { // Texture map init m_nTexMapX = MIN_CHUNKS_SIZE(depthFrame.getVideoMode().getResolutionX(), TEXTURE_SIZE); m_nTexMapY = MIN_CHUNKS_SIZE(depthFrame.getVideoMode().getResolutionY(), TEXTURE_SIZE); m_pTexMap = new openni::RGB888Pixel[m_nTexMapX * m_nTexMapY]; } const nite::UserMap& userLabels = userTrackerFrame.getUserMap(); glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -2000.0, 2000.0); if (depthFrame.isValid() && g_drawDepth) { calculateHistogram(m_pDepthHist, MAX_DEPTH, depthFrame); } memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel)); // check if we need to draw image frame to texture if (m_eViewState == DISPLAY_MODE_IMAGE && m_colorFrame.isValid()) { const openni::RGB888Pixel* pImageRow = (const openni::RGB888Pixel*)m_colorFrame.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + m_colorFrame.getCropOriginY() * m_nTexMapX; int rowSize = m_colorFrame.getStrideInBytes() / sizeof(openni::RGB888Pixel); for (int y = 0; y < m_colorFrame.getHeight(); ++y) { const openni::RGB888Pixel* pImage = pImageRow; openni::RGB888Pixel* pTex = pTexRow + m_colorFrame.getCropOriginX(); for (int x = 0; x < m_colorFrame.getWidth(); ++x, ++pImage, ++pTex) { *pTex = *pImage; } pImageRow += rowSize; pTexRow += m_nTexMapX; } } float factor[3] = {1, 1, 1}; // check if we need to draw depth frame to texture if (depthFrame.isValid() && g_drawDepth) { const nite::UserId* pLabels = userLabels.getPixels(); const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)depthFrame.getData(); openni::RGB888Pixel* pTexRow = m_pTexMap + depthFrame.getCropOriginY() * m_nTexMapX; int rowSize = depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel); for (int y = 0; y < depthFrame.getHeight(); ++y) { const openni::DepthPixel* pDepth = pDepthRow; openni::RGB888Pixel* pTex = pTexRow + depthFrame.getCropOriginX(); for (int x = 0; x < depthFrame.getWidth(); ++x, ++pDepth, ++pTex, ++pLabels) { if (*pDepth != 0) { if (*pLabels == 0) { if (!g_drawBackground) { factor[0] = factor[1] = factor[2] = 0; } else { factor[0] = Colors[colorCount][0]; factor[1] = Colors[colorCount][1]; factor[2] = Colors[colorCount][2]; } } else { factor[0] = Colors[*pLabels % colorCount][0]; factor[1] = Colors[*pLabels % colorCount][1]; factor[2] = Colors[*pLabels % colorCount][2]; } // // Add debug lines - every 10cm // else if ((*pDepth / 10) % 10 == 0) // { // factor[0] = factor[2] = 0; // } int nHistValue = m_pDepthHist[*pDepth]; pTex->r = nHistValue*factor[0]; pTex->g = nHistValue*factor[1]; pTex->b = nHistValue*factor[2]; factor[0] = factor[1] = factor[2] = 1; } } pDepthRow += rowSize; pTexRow += m_nTexMapX; } } 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, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap); // Display the OpenGL texture map glColor4f(1,1,1,1); glEnable(GL_TEXTURE_2D); glBegin(GL_QUADS); g_nXRes = depthFrame.getVideoMode().getResolutionX(); g_nYRes = depthFrame.getVideoMode().getResolutionY(); // upper left glTexCoord2f(0, 0); glVertex2f(0, 0); // upper right glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, 0); glVertex2f(GL_WIN_SIZE_X, 0); // bottom right glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, (float)g_nYRes/(float)m_nTexMapY); glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // bottom left glTexCoord2f(0, (float)g_nYRes/(float)m_nTexMapY); glVertex2f(0, GL_WIN_SIZE_Y); glEnd(); glDisable(GL_TEXTURE_2D); const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers(); for (int i = 0; i < users.getSize(); ++i) { const nite::UserData& user = users[i]; updateUserState(user, userTrackerFrame.getTimestamp()); if (user.isNew()) { m_pUserTracker->startSkeletonTracking(user.getId()); m_pUserTracker->startPoseDetection(user.getId(), nite::POSE_CROSSED_HANDS); } else if (!user.isLost()) { if (g_drawStatusLabel) { DrawStatusLabel(m_pUserTracker, user); } if (g_drawCenterOfMass) { DrawCenterOfMass(m_pUserTracker, user); } if (g_drawBoundingBox) { DrawBoundingBox(user); } if (users[i].getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawSkeleton) { DrawSkeleton(m_pUserTracker, user); } if (users[i].getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawHat) { DrawHat(m_pUserTracker, user); } if (users[i].getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawCube) { DrawCube(m_pUserTracker, user); } if (users[i].getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawCubeFront) { DrawCubeFront(m_pUserTracker, user); } } if (m_poseUser == 0 || m_poseUser == user.getId()) { const nite::PoseData& pose = user.getPose(nite::POSE_CROSSED_HANDS); if (pose.isEntered()) { // Start timer sprintf(g_generalMessage, "In exit pose. Keep it for %d second%s to exit\n", g_poseTimeoutToExit/1000, g_poseTimeoutToExit/1000 == 1 ? "" : "s"); printf("Counting down %d second to exit\n", g_poseTimeoutToExit/1000); m_poseUser = user.getId(); m_poseTime = userTrackerFrame.getTimestamp(); } else if (pose.isExited()) { memset(g_generalMessage, 0, sizeof(g_generalMessage)); printf("Count-down interrupted\n"); m_poseTime = 0; m_poseUser = 0; } else if (pose.isHeld()) { // tick if (userTrackerFrame.getTimestamp() - m_poseTime > g_poseTimeoutToExit * 1000) { printf("Count down complete. Exit...\n"); Finalize(); exit(2); } } } } if (g_drawFrameId) { DrawFrameId(userTrackerFrame.getFrameIndex()); } if (g_generalMessage[0] != '\0') { char *msg = g_generalMessage; glColor3f(1.0f, 0.0f, 0.0f); glRasterPos2i(100, 20); glPrintString(GLUT_BITMAP_HELVETICA_18, msg); } // Swap the OpenGL display buffers glutSwapBuffers(); }