HRESULT BinaryDumpReader::processDepth() { if(m_CurrFrame >= m_NumFrames) { GlobalAppState::get().s_playData = false; std::cout << "binary dump sequence complete - press space to run again" << std::endl; m_CurrFrame = 0; } if(GlobalAppState::get().s_playData) { float* depth = getDepthFloat(); memcpy(depth, m_data.m_DepthImages[m_CurrFrame], sizeof(float)*getDepthWidth()*getDepthHeight()); incrementRingbufIdx(); if (m_bHasColorData) { memcpy(m_colorRGBX, m_data.m_ColorImages[m_CurrFrame], sizeof(vec4uc)*getColorWidth()*getColorHeight()); } m_CurrFrame++; return S_OK; } else { return S_FALSE; } }
bool SensorDataReader::processDepth() { if (m_currFrame >= m_numFrames) { GlobalAppState::get().s_playData = false; //std::cout << "binary dump sequence complete - press space to run again" << std::endl; stopReceivingFrames(); std::cout << "binary dump sequence complete - stopped receiving frames" << std::endl; m_currFrame = 0; } if (GlobalAppState::get().s_playData) { float* depth = getDepthFloat(); //TODO check why the frame cache is not used? ml::SensorData::RGBDFrameCacheRead::FrameState frameState = m_sensorDataCache->getNext(); //ml::SensorData::RGBDFrameCacheRead::FrameState frameState; //frameState.m_colorFrame = m_sensorData->decompressColorAlloc(m_currFrame); //frameState.m_depthFrame = m_sensorData->decompressDepthAlloc(m_currFrame); for (unsigned int i = 0; i < getDepthWidth()*getDepthHeight(); i++) { if (frameState.m_depthFrame[i] == 0) depth[i] = -std::numeric_limits<float>::infinity(); else depth[i] = (float)frameState.m_depthFrame[i] / m_sensorData->m_depthShift; } incrementRingbufIdx(); if (m_bHasColorData) { for (unsigned int i = 0; i < getColorWidth()*getColorHeight(); i++) { m_colorRGBX[i] = vec4uc(frameState.m_colorFrame[i]); } } frameState.free(); m_currFrame++; return true; } else { return false; } }
HRESULT PrimeSenseSensor::readDepthAndColor(float* depthFloat, vec4uc* colorRGBX ) { HRESULT hr = S_OK; int changedIndex; openni::Status rc = openni::OpenNI::waitForAnyStream(&m_streams[0], 1, &changedIndex, 0); if (rc != openni::STATUS_OK) { return S_FALSE; //no frame available } rc = openni::OpenNI::waitForAnyStream(&m_streams[1], 1, &changedIndex, 0); if (rc != openni::STATUS_OK) { return S_FALSE; //no frame available } openni::Status sd = m_depthStream.readFrame(&m_depthFrame); openni::Status sc = m_colorStream.readFrame(&m_colorFrame); assert(m_colorFrame.getWidth() == m_depthFrame.getWidth()); assert(m_colorFrame.getHeight() == m_depthFrame.getHeight()); const openni::DepthPixel* pDepth = (const openni::DepthPixel*)m_depthFrame.getData(); const openni::RGB888Pixel* pImage = (const openni::RGB888Pixel*)m_colorFrame.getData(); // check if we need to draw depth frame to texture if (m_depthFrame.isValid() && m_colorFrame.isValid()) { unsigned int width = m_depthFrame.getWidth(); unsigned int nPixels = m_depthFrame.getWidth()*m_depthFrame.getHeight(); for (unsigned int i = 0; i < nPixels; i++) { const int x = i%width; const int y = i/width; const int src = y*width + (width-1-x); const openni::DepthPixel& p = pDepth[src]; float dF = (float)p*0.001f; if(dF >= GlobalAppState::get().s_sensorDepthMin && dF <= GlobalAppState::get().s_sensorDepthMax) depthFloat[i] = dF; else depthFloat[i] = -std::numeric_limits<float>::infinity(); } incrementRingbufIdx(); } // check if we need to draw depth frame to texture if (m_depthFrame.isValid() && m_colorFrame.isValid()) { int cropX, cropY, cropW, cropH; if(m_colorStream.getCropping(&cropX, &cropY, &cropW, &cropH)) { const openni::RGB888Pixel* pImage = (const openni::RGB888Pixel*)m_colorFrame.getData(); ZeroMemory(m_colorRGBX, sizeof(LONG)*m_colorWidth*m_colorHeight); #pragma omp parallel for for (int y = 0; y < cropH; ++y) { for (int x = 0; x < cropW; ++x) { unsigned int width = m_colorFrame.getWidth(); unsigned int height = m_colorFrame.getHeight(); int y2 = 0; if(m_colorWidth == 1280) y2 = (y+cropY)-64/2/*-(unsigned int)(((float)(y+cropY)/((float)(height-1)))*64+0.5f)*/-cropY; else y2 = y; if(y2 >= 0 && y2 < (int)height) { const openni::RGB888Pixel& pixel = pImage[(y2)*cropW+(cropW - x - 1)]; unsigned int c = 0; c |= pixel.r; c <<= 8; c |= pixel.g; c <<= 8; c |= pixel.b; c |= 0xFF000000; ((LONG*)m_colorRGBX)[(cropY+y)*m_colorWidth+(cropX+x)] = c; } } } } else { unsigned int width = m_colorFrame.getWidth(); unsigned int height = m_colorFrame.getHeight(); unsigned int nPixels = m_colorFrame.getWidth()*m_colorFrame.getHeight(); for(unsigned int i = 0; i<nPixels; i++) { const int x = i%width; const int y = i/width; int y2 = 0; if(m_colorWidth == 1280) y2 = y+64/2-10-(unsigned int)(((float)y/((float)(height-1)))*64+0.5f); else y2 = y; if(y2 >= 0 && y2 < (int)height) { //unsigned int Index1D = y2*width+x; unsigned int Index1D = y2*width+ (width-1-x); //x-flip here const openni::RGB888Pixel& pixel = pImage[Index1D]; unsigned int c = 0; c |= pixel.r; c <<= 8; c |= pixel.g; c <<= 8; c |= pixel.b; c |= 0xFF000000; ((LONG*)colorRGBX)[y*width+x] = c; } } } } return hr; }