void KinectHDFaceGrabber::update() { if (!m_pColorFrameReader || !m_pBodyFrameReader){ return; } IColorFrame* pColorFrame = nullptr; HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); IDepthFrame* depthFrame = nullptr; if (SUCCEEDED(hr)){ hr = m_pDepthFrameReader->AcquireLatestFrame(&depthFrame); } if (SUCCEEDED(hr)){ ColorImageFormat imageFormat = ColorImageFormat_None; if (SUCCEEDED(hr)){ hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)){ UINT nBufferSize = m_colorWidth * m_colorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(m_colorBuffer.data()), ColorImageFormat_Bgra); } if (SUCCEEDED(hr)){ hr = depthFrame->CopyFrameDataToArray(m_depthBuffer.size(), &m_depthBuffer[0]); } if (SUCCEEDED(hr)){ renderColorFrameAndProcessFaces(); } } SafeRelease(depthFrame); SafeRelease(pColorFrame); }
/// <summary> /// Main processing function /// </summary> void CCoordinateMappingBasics::Update() { if (!m_pMultiSourceFrameReader) { return; } IMultiSourceFrame* pMultiSourceFrame = NULL; IDepthFrame* pDepthFrame = NULL; IColorFrame* pColorFrame = NULL; IBodyIndexFrame* pBodyIndexFrame = NULL; IBodyFrame* pBodyFrame = NULL; HRESULT hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame); if (SUCCEEDED(hr)) { IDepthFrameReference* pDepthFrameReference = NULL; hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference); if (SUCCEEDED(hr)) { hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); } SafeRelease(pDepthFrameReference); } if (SUCCEEDED(hr)) { IColorFrameReference* pColorFrameReference = NULL; hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); if (SUCCEEDED(hr)) { hr = pColorFrameReference->AcquireFrame(&pColorFrame); } SafeRelease(pColorFrameReference); } if (SUCCEEDED(hr)) { IBodyIndexFrameReference* pBodyIndexFrameReference = NULL; hr = pMultiSourceFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference); if (SUCCEEDED(hr)) { hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame); } SafeRelease(pBodyIndexFrameReference); } if (SUCCEEDED(hr)) { IBodyFrameReference* pBodyFrameReference = NULL; hr = pMultiSourceFrame->get_BodyFrameReference(&pBodyFrameReference); if (SUCCEEDED(hr)) { hr = pBodyFrameReference->AcquireFrame(&pBodyFrame); } SafeRelease(pBodyFrameReference); } if (SUCCEEDED(hr)) { // Depth INT64 nDepthTime = 0; IFrameDescription* pDepthFrameDescription = NULL; int nDepthWidth = 0; int nDepthHeight = 0; UINT nDepthBufferSize = 0; UINT16 *pDepthBuffer = NULL; // Color IFrameDescription* pColorFrameDescription = NULL; int nColorWidth = 0; int nColorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = NULL; // BodyIndex IFrameDescription* pBodyIndexFrameDescription = NULL; int nBodyIndexWidth = 0; int nBodyIndexHeight = 0; UINT nBodyIndexBufferSize = 0; BYTE *pBodyIndexBuffer = NULL; // Body IBody* ppBodies[BODY_COUNT] = { 0 }; // get depth frame data hr = pDepthFrame->get_RelativeTime(&nDepthTime); // Depth if (SUCCEEDED(hr)) { hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription); } if (SUCCEEDED(hr)) { hr = pDepthFrameDescription->get_Width(&nDepthWidth); } if (SUCCEEDED(hr)) { hr = pDepthFrameDescription->get_Height(&nDepthHeight); } if (SUCCEEDED(hr)) { hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer); } // get color frame data if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Width(&nColorWidth); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Height(&nColorHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (m_pColorRGBX) { pColorBuffer = m_pColorRGBX; nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } // get body index frame data if (SUCCEEDED(hr)) { hr = pBodyIndexFrame->get_FrameDescription(&pBodyIndexFrameDescription); } if (SUCCEEDED(hr)) { hr = pBodyIndexFrameDescription->get_Width(&nBodyIndexWidth); } if (SUCCEEDED(hr)) { hr = pBodyIndexFrameDescription->get_Height(&nBodyIndexHeight); } if (SUCCEEDED(hr)) { hr = pBodyIndexFrame->AccessUnderlyingBuffer(&nBodyIndexBufferSize, &pBodyIndexBuffer); } // get body frame data if (SUCCEEDED(hr)) { hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies); } if (SUCCEEDED(hr)) { ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight, pColorBuffer, nColorWidth, nColorHeight, pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight, BODY_COUNT, ppBodies); } SafeRelease(pDepthFrameDescription); SafeRelease(pColorFrameDescription); SafeRelease(pBodyIndexFrameDescription); for (int i = 0; i < _countof(ppBodies); ++i) { SafeRelease(ppBodies[i]); } } SafeRelease(pDepthFrame); SafeRelease(pColorFrame); SafeRelease(pBodyIndexFrame); SafeRelease(pBodyFrame); SafeRelease(pMultiSourceFrame); }
void MyKinect2::Update() { // récupération de l'image en 2D if (!m_pColorFrameReader) { return; } IColorFrame* pColorFrame = NULL; HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pFrameDescription = NULL; int nWidth = 0; int nHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nBufferSize = 0; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pFrameDescription); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Width(&nWidth); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Height(&nHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr) && (nWidth == cColorWidth) && (nHeight == cColorHeight)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&webcam.data)); } else if (m_pColorRGBX) { nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(webcam.data), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } SafeRelease(pFrameDescription); } // récupération du squelette if (!m_pBodyFrameReader) { return; } IBodyFrame* pBodyFrame = NULL; hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; hr = pBodyFrame->get_RelativeTime(&nTime); IBody* ppBodies[BODY_COUNT] = {0}; if (SUCCEEDED(hr)) { hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies); } if (SUCCEEDED(hr)) { ProcessBody(BODY_COUNT, ppBodies); // BODY_COUNT est un define de Kinect.h égal à 6... peut etre mettre 1 plus tard afin déviter des problemes lors de la récupération des positions de joints } for (int i = 0; i < _countof(ppBodies); ++i) { SafeRelease(ppBodies[i]); } } SafeRelease(pBodyFrame); SafeRelease(pColorFrame); }
void Device::update() { if ( mSensor != 0 ) { mSensor->get_Status( &mStatus ); } if ( mFrameReader == 0 ) { return; } IAudioBeamFrame* audioFrame = 0; IBodyFrame* bodyFrame = 0; IBodyIndexFrame* bodyIndexFrame = 0; IColorFrame* colorFrame = 0; IDepthFrame* depthFrame = 0; IMultiSourceFrame* frame = 0; IInfraredFrame* infraredFrame = 0; ILongExposureInfraredFrame* infraredLongExposureFrame = 0; HRESULT hr = mFrameReader->AcquireLatestFrame( &frame ); // TODO audio if ( SUCCEEDED( hr ) ) { console() << "SUCCEEDED " << getElapsedFrames() << endl; } if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyEnabled() ) { IBodyFrameReference* frameRef = 0; hr = frame->get_BodyFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &bodyFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyIndexEnabled() ) { IBodyIndexFrameReference* frameRef = 0; hr = frame->get_BodyIndexFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &bodyIndexFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isColorEnabled() ) { IColorFrameReference* frameRef = 0; hr = frame->get_ColorFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &colorFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isDepthEnabled() ) { IDepthFrameReference* frameRef = 0; hr = frame->get_DepthFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &depthFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredEnabled() ) { IInfraredFrameReference* frameRef = 0; hr = frame->get_InfraredFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &infraredFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredLongExposureEnabled() ) { ILongExposureInfraredFrameReference* frameRef = 0; hr = frame->get_LongExposureInfraredFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &infraredLongExposureFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) ) { long long time = 0L; // TODO audio IFrameDescription* bodyFrameDescription = 0; int32_t bodyWidth = 0; int32_t bodyHeight = 0; uint32_t bodyBufferSize = 0; uint8_t* bodyBuffer = 0; IFrameDescription* bodyIndexFrameDescription = 0; int32_t bodyIndexWidth = 0; int32_t bodyIndexHeight = 0; uint32_t bodyIndexBufferSize = 0; uint8_t* bodyIndexBuffer = 0; IFrameDescription* colorFrameDescription = 0; int32_t colorWidth = 0; int32_t colorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; uint32_t colorBufferSize = 0; uint8_t* colorBuffer = 0; IFrameDescription* depthFrameDescription = 0; int32_t depthWidth = 0; int32_t depthHeight = 0; uint16_t depthMinReliableDistance = 0; uint16_t depthMaxReliableDistance = 0; uint32_t depthBufferSize = 0; uint16_t* depthBuffer = 0; IFrameDescription* infraredFrameDescription = 0; int32_t infraredWidth = 0; int32_t infraredHeight = 0; uint32_t infraredBufferSize = 0; uint16_t* infraredBuffer = 0; IFrameDescription* infraredLongExposureFrameDescription = 0; int32_t infraredLongExposureWidth = 0; int32_t infraredLongExposureHeight = 0; uint32_t infraredLongExposureBufferSize = 0; uint16_t* infraredLongExposureBuffer = 0; hr = depthFrame->get_RelativeTime( &time ); // TODO audio if ( mDeviceOptions.isAudioEnabled() ) { } // TODO body if ( mDeviceOptions.isBodyEnabled() ) { } if ( mDeviceOptions.isBodyIndexEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->get_FrameDescription( &bodyIndexFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrameDescription->get_Width( &bodyIndexWidth ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrameDescription->get_Height( &bodyIndexHeight ); } if ( SUCCEEDED( hr ) ) { //hr = bodyIndexFrame->AccessUnderlyingBuffer( &bodyIndexBufferSize, &bodyIndexBuffer ); } } if ( mDeviceOptions.isColorEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = colorFrame->get_FrameDescription( &colorFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = colorFrameDescription->get_Width( &colorWidth ); } if ( SUCCEEDED( hr ) ) { hr = colorFrameDescription->get_Height( &colorHeight ); } if ( SUCCEEDED( hr ) ) { hr = colorFrame->get_RawColorImageFormat( &imageFormat ); } if ( SUCCEEDED( hr ) ) { bool isAllocated = false; SurfaceChannelOrder channelOrder = SurfaceChannelOrder::BGRA; if ( imageFormat == ColorImageFormat_Bgra ) { hr = colorFrame->AccessRawUnderlyingBuffer( &colorBufferSize, reinterpret_cast<uint8_t**>( &colorBuffer ) ); channelOrder = SurfaceChannelOrder::BGRA; } else if ( imageFormat == ColorImageFormat_Rgba ) { hr = colorFrame->AccessRawUnderlyingBuffer( &colorBufferSize, reinterpret_cast<uint8_t**>( &colorBuffer ) ); channelOrder = SurfaceChannelOrder::RGBA; } else { isAllocated = true; colorBufferSize = colorWidth * colorHeight * sizeof( uint8_t ) * 4; colorBuffer = new uint8_t[ colorBufferSize ]; hr = colorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<uint8_t*>( colorBuffer ), ColorImageFormat_Rgba ); channelOrder = SurfaceChannelOrder::RGBA; } if ( SUCCEEDED( hr ) ) { colorFrame->get_RelativeTime( &time ); Surface8u colorSurface = Surface8u( colorBuffer, colorWidth, colorHeight, colorWidth * sizeof( uint8_t ) * 4, channelOrder ); mFrame.mSurfaceColor = Surface8u( colorWidth, colorHeight, false, channelOrder ); mFrame.mSurfaceColor.copyFrom( colorSurface, colorSurface.getBounds() ); console() << "Color\n\twidth: " << colorWidth << "\n\theight: " << colorHeight << "\n\tbuffer size: " << colorBufferSize << "\n\ttime: " << time << endl; } if ( isAllocated && colorBuffer != 0 ) { delete[] colorBuffer; colorBuffer = 0; } } } if ( mDeviceOptions.isDepthEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_FrameDescription( &depthFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = depthFrameDescription->get_Width( &depthWidth ); } if ( SUCCEEDED( hr ) ) { hr = depthFrameDescription->get_Height( &depthHeight ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_DepthMinReliableDistance( &depthMinReliableDistance ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_DepthMaxReliableDistance( &depthMaxReliableDistance ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->AccessUnderlyingBuffer( &depthBufferSize, &depthBuffer ); } if ( SUCCEEDED( hr ) ) { Channel16u depthChannel = Channel16u( depthWidth, depthHeight, depthWidth * sizeof( uint16_t ), 1, depthBuffer ); mFrame.mChannelDepth = Channel16u( depthWidth, depthHeight ); mFrame.mChannelDepth.copyFrom( depthChannel, depthChannel.getBounds() ); console( ) << "Depth\n\twidth: " << depthWidth << "\n\theight: " << depthHeight << endl; } } if ( mDeviceOptions.isInfraredEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = infraredFrame->get_FrameDescription( &infraredFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrameDescription->get_Width( &infraredWidth ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrameDescription->get_Height( &infraredHeight ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrame->AccessUnderlyingBuffer( &infraredBufferSize, &infraredBuffer ); } if ( SUCCEEDED( hr ) ) { Channel16u infraredChannel = Channel16u( infraredWidth, infraredHeight, infraredWidth * sizeof( uint16_t ), 1, infraredBuffer ); mFrame.mChannelInfrared = Channel16u( infraredWidth, infraredHeight ); mFrame.mChannelInfrared.copyFrom( infraredChannel, infraredChannel.getBounds() ); console( ) << "Infrared\n\twidth: " << infraredWidth << "\n\theight: " << infraredHeight << endl; } } if ( mDeviceOptions.isInfraredLongExposureEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrame->get_FrameDescription( &infraredLongExposureFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrameDescription->get_Width( &infraredLongExposureWidth ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrameDescription->get_Height( &infraredLongExposureHeight ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrame->AccessUnderlyingBuffer( &infraredLongExposureBufferSize, &infraredLongExposureBuffer ); } if ( SUCCEEDED( hr ) ) { Channel16u infraredLongExposureChannel = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight, infraredLongExposureWidth * sizeof( uint16_t ), 1, infraredLongExposureBuffer ); mFrame.mChannelInfraredLongExposure = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight ); mFrame.mChannelInfraredLongExposure.copyFrom( infraredLongExposureChannel, infraredLongExposureChannel.getBounds() ); int64_t irLongExpTime = 0; hr = infraredLongExposureFrame->get_RelativeTime( &irLongExpTime ); console( ) << "Infrared Long Exposure\n\twidth: " << infraredLongExposureWidth << "\n\theight: " << infraredLongExposureHeight; if ( SUCCEEDED( hr ) ) { console() << "\n\ttimestamp: " << irLongExpTime; } console() << endl; } } if ( SUCCEEDED( hr ) ) { // TODO build Kinect2::Frame from buffers, data mFrame.mTimeStamp = time; } if ( bodyFrameDescription != 0 ) { bodyFrameDescription->Release(); bodyFrameDescription = 0; } if ( bodyIndexFrameDescription != 0 ) { bodyIndexFrameDescription->Release(); bodyIndexFrameDescription = 0; } if ( colorFrameDescription != 0 ) { colorFrameDescription->Release(); colorFrameDescription = 0; } if ( depthFrameDescription != 0 ) { depthFrameDescription->Release(); depthFrameDescription = 0; } if ( infraredFrameDescription != 0 ) { infraredFrameDescription->Release(); infraredFrameDescription = 0; } if ( infraredLongExposureFrameDescription != 0 ) { infraredLongExposureFrameDescription->Release(); infraredLongExposureFrameDescription = 0; } } if ( audioFrame != 0 ) { audioFrame->Release(); audioFrame = 0; } if ( bodyFrame != 0 ) { bodyFrame->Release(); bodyFrame = 0; } if ( bodyIndexFrame != 0 ) { bodyIndexFrame->Release(); bodyIndexFrame = 0; } if ( colorFrame != 0 ) { colorFrame->Release(); colorFrame = 0; } if ( depthFrame != 0 ) { depthFrame->Release(); depthFrame = 0; } if ( frame != 0 ) { frame->Release(); frame = 0; } if ( infraredFrame != 0 ) { infraredFrame->Release(); infraredFrame = 0; } if ( infraredLongExposureFrame != 0 ) { infraredLongExposureFrame->Release(); infraredLongExposureFrame = 0; } }
void UpdateColor() { if (!m_pColorFrameReader) { return; } IColorFrame* pColorFrame = NULL; HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pFrameDescription = NULL; int nWidth = 0; int nHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nBufferSize = 0; RGBQUAD *pBuffer = NULL; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pFrameDescription); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Width(&nWidth); } if (SUCCEEDED(hr)) { m_nColorWidth = nWidth; hr = pFrameDescription->get_Height(&nHeight); } if (SUCCEEDED(hr)) { m_nColorHeight = nHeight; hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&pBuffer)); } else if (m_pColorRGBX) { pBuffer = m_pColorRGBX; nBufferSize = nWidth * nHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } if (SUCCEEDED(hr)) { { ProcessColor(nTime, pBuffer, nWidth, nHeight); } } SafeRelease(pFrameDescription); } else{ DumpHR(hr); } SafeRelease(pColorFrame); }
void* Kinect2StreamImpl::populateFrameBuffer(int& buffWidth, int& buffHeight) { buffWidth = 0; buffHeight = 0; if (m_sensorType == ONI_SENSOR_COLOR) { if (m_pFrameReader.color && m_pFrameBuffer.color) { buffWidth = 1920; buffHeight = 1080; IColorFrame* frame = NULL; HRESULT hr = m_pFrameReader.color->AcquireLatestFrame(&frame); if (SUCCEEDED(hr)) { ColorImageFormat imageFormat = ColorImageFormat_None; hr = frame->get_RawColorImageFormat(&imageFormat); if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { RGBQUAD* data; UINT bufferSize; frame->AccessRawUnderlyingBuffer(&bufferSize, reinterpret_cast<BYTE**>(&data)); memcpy(m_pFrameBuffer.color, data, 1920*1080*sizeof(RGBQUAD)); } else { frame->CopyConvertedFrameDataToArray(1920*1080*sizeof(RGBQUAD), reinterpret_cast<BYTE*>(m_pFrameBuffer.color), ColorImageFormat_Bgra); } } } if (frame) { frame->Release(); } return reinterpret_cast<void*>(m_pFrameBuffer.color); } } else if (m_sensorType == ONI_SENSOR_DEPTH) { if (m_pFrameReader.depth && m_pFrameBuffer.depth) { buffWidth = 512; buffHeight = 424; IDepthFrame* frame = NULL; HRESULT hr = m_pFrameReader.depth->AcquireLatestFrame(&frame); if (SUCCEEDED(hr)) { UINT16* data; UINT bufferSize; frame->AccessUnderlyingBuffer(&bufferSize, &data); memcpy(m_pFrameBuffer.depth, data, 512*424*sizeof(UINT16)); } if (frame) { frame->Release(); } return reinterpret_cast<void*>(m_pFrameBuffer.depth); } } else { // ONI_SENSOR_IR if (m_pFrameReader.infrared && m_pFrameBuffer.infrared) { buffWidth = 512; buffHeight = 424; IInfraredFrame* frame = NULL; HRESULT hr = m_pFrameReader.infrared->AcquireLatestFrame(&frame); if (SUCCEEDED(hr)) { UINT16* data; UINT bufferSize; frame->AccessUnderlyingBuffer(&bufferSize, &data); memcpy(m_pFrameBuffer.infrared, data, 512*424*sizeof(UINT16)); } if (frame) { frame->Release(); } return reinterpret_cast<void*>(m_pFrameBuffer.infrared); } } return NULL; }
/// <summary> /// Main processing function /// </summary> // この関数がループ処理される void CColorBasics::Update() { HRESULT hr = NULL; // (1) カラーフレーム(背景描画のみに使用) if (m_pColorFrameReader) { IColorFrame* pColorFrame = NULL; hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pFrameDescription = NULL; int nWidth = 0; int nHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nBufferSize = 0; RGBQUAD *pBuffer = NULL; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pFrameDescription); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Width(&nWidth); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Height(&nHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&pBuffer)); } else if (m_pColorRGBX) { pBuffer = m_pColorRGBX; nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } if (SUCCEEDED(hr)) { ProcessColor(nTime, pBuffer, nWidth, nHeight); } SafeRelease(pFrameDescription); } SafeRelease(pColorFrame); } // これ以降にBody処理を実装 TIMESPAN nBodyTime = 0; if (m_pBodyFrameReader) { IBodyFrame* pBodyFrame = NULL; hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame); if (SUCCEEDED(hr)) { hr = pBodyFrame->get_RelativeTime(&nBodyTime); // ここに、UI描画処理 ボタン等 // ゲームのステータスによって描画を設定 if (!m_pGame){ m_pGame = new CSemaphoreGame(m_pDrawColor, m_pCoordinateMapper); } if (m_pGame){ m_pGame->Display(nBodyTime); } IBody* ppBodies[BODY_COUNT] = { 0 }; if (SUCCEEDED(hr)) { hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies); } if (SUCCEEDED(hr)) { // ここに、UI処理->ProcessBody()内でOK // ボディデータにてUI処理を行う // ステータスの変更 if (m_pGame){ m_pGame->Play(nBodyTime, BODY_COUNT, ppBodies); } ProcessBody(nBodyTime, BODY_COUNT, ppBodies); } for (int i = 0; i < _countof(ppBodies); ++i) { SafeRelease(ppBodies[i]); } } SafeRelease(pBodyFrame); } return; }
void processColor() { if (!device) return; if (!m_pColorFrameReader) return; IColorFrame* pColorFrame = NULL; HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pFrameDescription = NULL; int nWidth = 0; int nHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nBufferSize = 0; RGBQUAD *src = NULL; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pFrameDescription); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Width(&nWidth); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Height(&nHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (imageFormat != ColorImageFormat_Bgra) { if (!rgb_buffer) { rgb_buffer = new RGBQUAD[nWidth * nHeight]; } //post("image format %d", imageFormat); //error("not brga"); nBufferSize = nWidth * nHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(rgb_buffer), ColorImageFormat_Rgba); if (FAILED(hr)) { error("failed to convert image"); return; } src = rgb_buffer; } hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&src)); ARGB * dst = (ARGB *)rgb_mat.back; int cells = nWidth * nHeight; //if (align_depth_to_color) { for (int i = 0; i < cells; ++i) { dst[i].r = src[i].rgbRed; dst[i].g = src[i].rgbGreen; dst[i].b = src[i].rgbBlue; } /*} else { // align color to depth: //std::fill(dst, dst + cells, RGB(0, 0, 0)); for (int i = 0; i < cells; ++i) { int c = colorCoordinates[i * 2]; int r = colorCoordinates[i * 2 + 1]; if (c >= 0 && c < KINECT_DEPTH_WIDTH && r >= 0 && r < KINECT_DEPTH_HEIGHT) { // valid location: depth value: int idx = r*KINECT_DEPTH_WIDTH + c; dst[i].r = src[idx].r; dst[i].g = src[idx].g; dst[i].b = src[idx].b; } } }*/ new_rgb_data = 1; } }
HRESULT KinectHandler::GetColorData(RGBQUAD* &dest) { if (!m_pMultiFrameReader) { cout << "No frame reader!" << endl; return E_FAIL; } IColorFrame* pColorFrame = NULL; IMultiSourceFrame* pMultiSourceFrame = NULL; HRESULT hr = m_pMultiFrameReader->AcquireLatestFrame(&pMultiSourceFrame); if (SUCCEEDED(hr)) { IColorFrameReference* pColorFrameReference = NULL; hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); if (SUCCEEDED(hr)) { hr = pColorFrameReference->AcquireFrame(&pColorFrame); } SafeRelease(pColorFrameReference); } if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pColorFrameDescription = NULL; int nColorWidth = 0; int nColorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = NULL; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Width(&nColorWidth); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Height(&nColorHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { cout << "Acessando o raw format!!!" << endl; hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (m_pColorRGBX) { cout << "Acessando o convert format" << endl; pColorBuffer = m_pColorRGBX; nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { cout << "FAILED" << endl; hr = E_FAIL; } } if (SUCCEEDED(hr)) { dest = pColorBuffer; } } else { cout << "Acquire last frame FAILED " << endl; hr = E_FAIL; SafeRelease(pColorFrame); return hr; } SafeRelease(pColorFrame); SafeRelease(pMultiSourceFrame); return hr; }
HRESULT KinectHandler::GetColorAndDepth(RGBQUAD* &color, RGBQUAD* &depth, UINT16*& depthBuffer) { if (!m_pMultiFrameReader) { cout << "No frame reader!" << endl; return E_FAIL; } IColorFrame* pColorFrame = NULL; IDepthFrame* pDepthFrame = NULL; IMultiSourceFrame* pMultiSourceFrame = NULL; HRESULT hr = m_pMultiFrameReader->AcquireLatestFrame(&pMultiSourceFrame); if (SUCCEEDED(hr)) { IColorFrameReference* pColorFrameReference = NULL; hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); if (SUCCEEDED(hr)) { hr = pColorFrameReference->AcquireFrame(&pColorFrame); } IDepthFrameReference* pDepthFrameReference = NULL; hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference); if (SUCCEEDED(hr)) { hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); } SafeRelease(pColorFrameReference); SafeRelease(pDepthFrameReference); } if (SUCCEEDED(hr) && pColorFrame != NULL && pDepthFrame != NULL) { INT64 nTime = 0; IFrameDescription* pColorFrameDescription = NULL; int nColorWidth = 0; int nColorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = NULL; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Width(&nColorWidth); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Height(&nColorHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (m_pColorRGBX) { pColorBuffer = m_pColorRGBX; nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { cout << "FAILED" << endl; hr = E_FAIL; } } if (SUCCEEDED(hr)) { color = pColorBuffer; } ///===========================================//// nTime = 0; IFrameDescription* pDepthFrameDescription = NULL; int nDepthWidth = 0; int nDepthHeight = 0; USHORT nDepthMinReliableDistance = 0; USHORT nDepthMaxDistance = 0; UINT nDepthBufferSize = 0; UINT16 *pDepthBuffer = NULL; hr = pDepthFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription); } if (SUCCEEDED(hr)) { hr = pDepthFrameDescription->get_Width(&nDepthWidth); } if (SUCCEEDED(hr)) { hr = pDepthFrameDescription->get_Height(&nDepthHeight); } if (SUCCEEDED(hr)) { hr = pDepthFrame->get_DepthMinReliableDistance(&nDepthMinReliableDistance); } if (SUCCEEDED(hr)) { // In order to see the full range of depth (including the less reliable far field depth) // we are setting nDepthMaxDistance to the extreme potential depth threshold nDepthMaxDistance = USHRT_MAX; // Note: If you wish to filter by reliable depth distance, uncomment the following line. //// hr = pDepthFrame->get_DepthMaxReliableDistance(&nDepthMaxDistance); } if (SUCCEEDED(hr)) { hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer); } if (SUCCEEDED(hr)) { //RGBQUAD* pRGBX = new RGBQUAD[cDepthWidth * cDepthHeight]; // end pixel is start + width*height - 1 const UINT16* pBufferEnd = pDepthBuffer + (nDepthWidth * nDepthHeight); RGBQUAD* auxiliar = m_pDepthRGBX; //const UINT16* pBufferEnd = pDepthBuffer + (640 * 480); int counter = 0; while (pDepthBuffer < pBufferEnd) { //cout << "now:" << pDepthBuffer << " end:" << pBufferEnd << endl; USHORT depth = *pDepthBuffer; //cout << "now:" << pDepthBuffer << " end:" << pBufferEnd << endl; // To convert to a byte, we're discarding the most-significant // rather than least-significant bits. // We're preserving detail, although the intensity will "wrap." // Values outside the reliable depth range are mapped to 0 (black). // Note: Using conditionals in this loop could degrade performance. // Consider using a lookup table instead when writing production code. //BYTE intensity = static_cast<BYTE>((depth >= nDepthMinReliableDistance) && (depth <= nDepthMaxDistance) ? (depth % 256) : 0); BYTE intensity = static_cast<BYTE>((depth >= nDepthMinReliableDistance) && (depth <= nDepthMaxDistance) ? ((depth - nDepthMinReliableDistance) * (0 - 255) / (nDepthMaxDistance / 50 - nDepthMinReliableDistance) + 255) : 0); auxiliar->rgbBlue = intensity; auxiliar->rgbGreen = intensity; auxiliar->rgbRed = intensity; auxiliar->rgbReserved = (BYTE)255; counter++; ++auxiliar; ++pDepthBuffer; } depth = m_pDepthRGBX; } if (m_pDepthRawBuffer) { hr = pDepthFrame->CopyFrameDataToArray((cDepthWidth * cDepthHeight), m_pDepthRawBuffer); if(SUCCEEDED(hr)) depthBuffer = m_pDepthRawBuffer; } SafeRelease(pDepthFrameDescription); } else { cout << "Acquire last frame FAILED " << endl; hr = E_FAIL; SafeRelease(pColorFrame); SafeRelease(pDepthFrame); return hr; } SafeRelease(pColorFrame); SafeRelease(pDepthFrame); SafeRelease(pMultiSourceFrame); return hr; }
//Hao modified it void Kinect2Engine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage) { Vector4u *rgb = rgbImage->GetData(MEMORYDEVICE_CPU); if (colorAvailable) { IColorFrame* pColorFrame = NULL; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nBufferSize = 0; RGBQUAD *c_pBuffer = NULL; HRESULT hr = data->colorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { if (SUCCEEDED(hr)) hr = pColorFrame->get_RawColorImageFormat(&imageFormat); if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&c_pBuffer)); } else if (m_pColorRGBX) { c_pBuffer = m_pColorRGBX; nBufferSize = imageSize_rgb.x * imageSize_rgb.y * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(c_pBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } if (SUCCEEDED(hr) && c_pBuffer) { for (int i = 0; i < imageSize_rgb.x * imageSize_rgb.y; i++) { Vector4u newPix; RGBQUAD oldPix = c_pBuffer[i]; newPix.x = oldPix.rgbRed; newPix.y = oldPix.rgbGreen; newPix.z = oldPix.rgbBlue; newPix.w = 255; rgb[i] = newPix; } } } SafeRelease(pColorFrame); } else memset(rgb, 0, rgbImage->dataSize * sizeof(Vector4u)); short *depth = rawDepthImage->GetData(MEMORYDEVICE_CPU); if (depthAvailable) { IDepthFrame* pDepthFrame = NULL; UINT16 *d_pBuffer = NULL; UINT nBufferSize = 0; HRESULT hr = data->depthFrameReader->AcquireLatestFrame(&pDepthFrame); if (SUCCEEDED(hr)) { if (SUCCEEDED(hr)) hr = pDepthFrame->AccessUnderlyingBuffer(&nBufferSize, &d_pBuffer); if (SUCCEEDED(hr) && d_pBuffer) { for (int i = 0; i < imageSize_d.x * imageSize_d.y; i++) { ushort depthPix = d_pBuffer[i]; depth[i] = (depthPix >= nDepthMinReliableDistance) && (depthPix <= nDepthMaxDistance) ? (short)depthPix : -1; } } } SafeRelease(pDepthFrame); } else memset(depth, 0, rawDepthImage->dataSize * sizeof(short)); //out->inputImageType = ITMView::InfiniTAM_FLOAT_DEPTH_IMAGE; return /*true*/; }
void Device::update() { if ( mFrameReader == 0 ) { return; } IAudioBeamFrame* audioFrame = 0; IBodyFrame* bodyFrame = 0; IBodyIndexFrame* bodyIndexFrame = 0; IColorFrame* colorFrame = 0; IDepthFrame* depthFrame = 0; IMultiSourceFrame* frame = 0; IInfraredFrame* infraredFrame = 0; ILongExposureInfraredFrame* infraredLongExposureFrame = 0; HRESULT hr = mFrameReader->AcquireLatestFrame( &frame ); if ( SUCCEEDED( hr ) && mDeviceOptions.isAudioEnabled() ) { // TODO audio } if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyEnabled() ) { IBodyFrameReference* frameRef = 0; hr = frame->get_BodyFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &bodyFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyIndexEnabled() ) { IBodyIndexFrameReference* frameRef = 0; hr = frame->get_BodyIndexFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &bodyIndexFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isColorEnabled() ) { IColorFrameReference* frameRef = 0; hr = frame->get_ColorFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &colorFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isDepthEnabled() ) { IDepthFrameReference* frameRef = 0; hr = frame->get_DepthFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &depthFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredEnabled() ) { IInfraredFrameReference* frameRef = 0; hr = frame->get_InfraredFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &infraredFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredLongExposureEnabled() ) { ILongExposureInfraredFrameReference* frameRef = 0; hr = frame->get_LongExposureInfraredFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &infraredLongExposureFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) ) { long long timeStamp = 0L; // TODO audio std::vector<Body> bodies; int64_t bodyTime = 0L; IBody* kinectBodies[ BODY_COUNT ] = { 0 }; Vec4f floorClipPlane = Vec4f::zero(); Channel8u bodyIndexChannel; IFrameDescription* bodyIndexFrameDescription = 0; int32_t bodyIndexWidth = 0; int32_t bodyIndexHeight = 0; uint32_t bodyIndexBufferSize = 0; uint8_t* bodyIndexBuffer = 0; int64_t bodyIndexTime = 0L; Surface8u colorSurface; IFrameDescription* colorFrameDescription = 0; int32_t colorWidth = 0; int32_t colorHeight = 0; ColorImageFormat colorImageFormat = ColorImageFormat_None; uint32_t colorBufferSize = 0; uint8_t* colorBuffer = 0; Channel16u depthChannel; IFrameDescription* depthFrameDescription = 0; int32_t depthWidth = 0; int32_t depthHeight = 0; uint16_t depthMinReliableDistance = 0; uint16_t depthMaxReliableDistance = 0; uint32_t depthBufferSize = 0; uint16_t* depthBuffer = 0; Channel16u infraredChannel; IFrameDescription* infraredFrameDescription = 0; int32_t infraredWidth = 0; int32_t infraredHeight = 0; uint32_t infraredBufferSize = 0; uint16_t* infraredBuffer = 0; Channel16u infraredLongExposureChannel; IFrameDescription* infraredLongExposureFrameDescription = 0; int32_t infraredLongExposureWidth = 0; int32_t infraredLongExposureHeight = 0; uint32_t infraredLongExposureBufferSize = 0; uint16_t* infraredLongExposureBuffer = 0; hr = depthFrame->get_RelativeTime( &timeStamp ); // TODO audio if ( mDeviceOptions.isAudioEnabled() ) { } if ( mDeviceOptions.isBodyEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = bodyFrame->get_RelativeTime( &bodyTime ); } if ( SUCCEEDED( hr ) ) { hr = bodyFrame->GetAndRefreshBodyData( BODY_COUNT, kinectBodies ); } if ( SUCCEEDED( hr ) ) { Vector4 v; hr = bodyFrame->get_FloorClipPlane( &v ); floorClipPlane = toVec4f( v ); } if ( SUCCEEDED( hr ) ) { for ( uint8_t i = 0; i < BODY_COUNT; ++i ) { IBody* kinectBody = kinectBodies[ i ]; if ( kinectBody != 0 ) { uint8_t isTracked = false; hr = kinectBody->get_IsTracked( &isTracked ); if ( SUCCEEDED( hr ) && isTracked ) { Joint joints[ JointType_Count ]; kinectBody->GetJoints( JointType_Count, joints ); JointOrientation jointOrientations[ JointType_Count ]; kinectBody->GetJointOrientations( JointType_Count, jointOrientations ); uint64_t id = 0; kinectBody->get_TrackingId( &id ); std::map<JointType, Body::Joint> jointMap; for ( int32_t j = 0; j < JointType_Count; ++j ) { Body::Joint joint( toVec3f( joints[ j ].Position ), toQuatf( jointOrientations[ j ].Orientation ), joints[ j ].TrackingState ); jointMap.insert( pair<JointType, Body::Joint>( static_cast<JointType>( j ), joint ) ); } Body body( id, i, jointMap ); bodies.push_back( body ); } } } } } if ( mDeviceOptions.isBodyIndexEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->get_RelativeTime( &bodyIndexTime ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->get_FrameDescription( &bodyIndexFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrameDescription->get_Width( &bodyIndexWidth ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrameDescription->get_Height( &bodyIndexHeight ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->AccessUnderlyingBuffer( &bodyIndexBufferSize, &bodyIndexBuffer ); } if ( SUCCEEDED( hr ) ) { bodyIndexChannel = Channel8u( bodyIndexWidth, bodyIndexHeight ); memcpy( bodyIndexChannel.getData(), bodyIndexBuffer, bodyIndexWidth * bodyIndexHeight * sizeof( uint8_t ) ); } } if ( mDeviceOptions.isColorEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = colorFrame->get_FrameDescription( &colorFrameDescription ); if ( SUCCEEDED( hr ) ) { float vFov = 0.0f; float hFov = 0.0f; float dFov = 0.0f; colorFrameDescription->get_VerticalFieldOfView( &vFov ); colorFrameDescription->get_HorizontalFieldOfView( &hFov ); colorFrameDescription->get_DiagonalFieldOfView( &dFov ); } } if ( SUCCEEDED( hr ) ) { hr = colorFrameDescription->get_Width( &colorWidth ); } if ( SUCCEEDED( hr ) ) { hr = colorFrameDescription->get_Height( &colorHeight ); } if ( SUCCEEDED( hr ) ) { hr = colorFrame->get_RawColorImageFormat( &colorImageFormat ); } if ( SUCCEEDED( hr ) ) { colorBufferSize = colorWidth * colorHeight * sizeof( uint8_t ) * 4; colorBuffer = new uint8_t[ colorBufferSize ]; hr = colorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<uint8_t*>( colorBuffer ), ColorImageFormat_Rgba ); if ( SUCCEEDED( hr ) ) { colorSurface = Surface8u( colorWidth, colorHeight, false, SurfaceChannelOrder::RGBA ); memcpy( colorSurface.getData(), colorBuffer, colorWidth * colorHeight * sizeof( uint8_t ) * 4 ); } delete [] colorBuffer; colorBuffer = 0; } } if ( mDeviceOptions.isDepthEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_FrameDescription( &depthFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = depthFrameDescription->get_Width( &depthWidth ); } if ( SUCCEEDED( hr ) ) { hr = depthFrameDescription->get_Height( &depthHeight ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_DepthMinReliableDistance( &depthMinReliableDistance ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_DepthMaxReliableDistance( &depthMaxReliableDistance ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->AccessUnderlyingBuffer( &depthBufferSize, &depthBuffer ); } if ( SUCCEEDED( hr ) ) { depthChannel = Channel16u( depthWidth, depthHeight ); memcpy( depthChannel.getData(), depthBuffer, depthWidth * depthHeight * sizeof( uint16_t ) ); } } if ( mDeviceOptions.isInfraredEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = infraredFrame->get_FrameDescription( &infraredFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrameDescription->get_Width( &infraredWidth ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrameDescription->get_Height( &infraredHeight ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrame->AccessUnderlyingBuffer( &infraredBufferSize, &infraredBuffer ); } if ( SUCCEEDED( hr ) ) { infraredChannel = Channel16u( infraredWidth, infraredHeight ); memcpy( infraredChannel.getData(), infraredBuffer, infraredWidth * infraredHeight * sizeof( uint16_t ) ); } } if ( mDeviceOptions.isInfraredLongExposureEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrame->get_FrameDescription( &infraredLongExposureFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrameDescription->get_Width( &infraredLongExposureWidth ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrameDescription->get_Height( &infraredLongExposureHeight ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrame->AccessUnderlyingBuffer( &infraredLongExposureBufferSize, &infraredLongExposureBuffer ); } if ( SUCCEEDED( hr ) ) { infraredLongExposureChannel = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight ); memcpy( infraredLongExposureChannel.getData(), infraredLongExposureBuffer, infraredLongExposureWidth * infraredLongExposureHeight * sizeof( uint16_t ) ); } } if ( SUCCEEDED( hr ) ) { mFrame.mBodies = bodies; mFrame.mChannelBodyIndex = bodyIndexChannel; mFrame.mChannelDepth = depthChannel; mFrame.mChannelInfrared = infraredChannel; mFrame.mChannelInfraredLongExposure = infraredLongExposureChannel; mFrame.mDeviceId = mDeviceOptions.getDeviceId(); mFrame.mSurfaceColor = colorSurface; mFrame.mTimeStamp = timeStamp; mFrame.mFloorClipPlane = floorClipPlane; } if ( bodyIndexFrameDescription != 0 ) { bodyIndexFrameDescription->Release(); bodyIndexFrameDescription = 0; } if ( colorFrameDescription != 0 ) { colorFrameDescription->Release(); colorFrameDescription = 0; } if ( depthFrameDescription != 0 ) { depthFrameDescription->Release(); depthFrameDescription = 0; } if ( infraredFrameDescription != 0 ) { infraredFrameDescription->Release(); infraredFrameDescription = 0; } if ( infraredLongExposureFrameDescription != 0 ) { infraredLongExposureFrameDescription->Release(); infraredLongExposureFrameDescription = 0; } } if ( audioFrame != 0 ) { audioFrame->Release(); audioFrame = 0; } if ( bodyFrame != 0 ) { bodyFrame->Release(); bodyFrame = 0; } if ( bodyIndexFrame != 0 ) { bodyIndexFrame->Release(); bodyIndexFrame = 0; } if ( colorFrame != 0 ) { colorFrame->Release(); colorFrame = 0; } if ( depthFrame != 0 ) { depthFrame->Release(); depthFrame = 0; } if ( frame != 0 ) { frame->Release(); frame = 0; } if ( infraredFrame != 0 ) { infraredFrame->Release(); infraredFrame = 0; } if ( infraredLongExposureFrame != 0 ) { infraredLongExposureFrame->Release(); infraredLongExposureFrame = 0; } }
/// <summary> /// Main processing function /// </summary> void CFaceBasics::Update() { if (!m_pColorFrameReader || !m_pBodyFrameReader) { return; } IColorFrame* pColorFrame = nullptr; HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pFrameDescription = nullptr; int nWidth = 0; int nHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nBufferSize = 0; RGBQUAD *pBuffer = nullptr; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pFrameDescription); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Width(&nWidth); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Height(&nHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&pBuffer)); } else if (m_pColorRGBX) { pBuffer = m_pColorRGBX; nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } if (SUCCEEDED(hr)) { DrawStreams(nTime, pBuffer, nWidth, nHeight); } SafeRelease(pFrameDescription); } SafeRelease(pColorFrame); }
void capture() { IMultiSourceFrame *multiFrame = NULL; IColorFrame *colorFrame = NULL; IColorFrameReference *colorFrameReference = NULL; UINT colorBufferSize = 0; RGBQUAD *colorBuffer = NULL; IDepthFrame *depthFrame = NULL; IDepthFrameReference *depthFrameReference = NULL; UINT bufferSize = 0; // UINT16 *depthBuffer = NULL; IBodyIndexFrame *bodyIndexFrame = NULL; IBodyIndexFrameReference *bodyIndexFrameReference = NULL; UINT bodyIndexBufferSize = 0; static int lastTime = 0; static int currentTime = 0; HRESULT hr = -1; //フレームリーダーが読み込み可能になるのを待つループ(各Frameしか取らない) while(1) { if((currentTime = GetTickCount()) > 33) { hr = multiFrameReader->AcquireLatestFrame(&multiFrame); lastTime = currentTime; }else continue; if(FAILED(hr)) { //fprintf(stderr, "AcquireLatestFrame(&multiFrame)\n"); Sleep(1); continue; } hr = multiFrame->get_ColorFrameReference(&colorFrameReference); if(FAILED(hr)) { Sleep(1); fprintf(stderr, "ColorFrameReference(&colorFrameReference)\n"); SafeRelease(multiFrame); continue; } hr = colorFrameReference->AcquireFrame(&colorFrame); if(FAILED(hr)) { Sleep(1); fprintf(stderr, "AcquireFrame(&colorFrame)\n"); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } hr = multiFrame->get_DepthFrameReference(&depthFrameReference); if (FAILED(hr)) { Sleep(1); fprintf(stderr, "DepthFrameReference(&depthFrameReference)\n"); SafeRelease(colorFrame); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } hr = depthFrameReference->AcquireFrame(&depthFrame); if (FAILED(hr)) { Sleep(1); fprintf(stderr, "AcquireFrame(&depthFrame)\n"); SafeRelease(depthFrameReference); SafeRelease(colorFrame); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } // hr = depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer); hr = depthFrame->CopyFrameDataToArray( dPixels, &depthBuffer[0] ); if (FAILED(hr)) { Sleep(1); fprintf(stderr, "AccessUnderlyingBuffer(&bufferSize, &depthBuffer\n"); SafeRelease(depthFrame); SafeRelease(depthFrameReference); SafeRelease(colorFrame); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } hr = multiFrame->get_BodyIndexFrameReference(&bodyIndexFrameReference); if (FAILED(hr)) { Sleep(1); fprintf(stderr, "BodyIndexReference(&colorFrameReference)\n"); free(depthBuffer); SafeRelease(depthFrame); SafeRelease(depthFrameReference); SafeRelease(colorFrame); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } hr = bodyIndexFrameReference->AcquireFrame(&bodyIndexFrame); if (FAILED(hr)) { Sleep(1); fprintf(stderr, "AcquireFrame(&bodyIndexFrame)\n"); SafeRelease(bodyIndexFrameReference); free(depthBuffer); SafeRelease(depthFrame); SafeRelease(depthFrameReference); SafeRelease(colorFrame); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } hr = bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer); if(FAILED(hr)) { Sleep(1); fprintf(stderr, "bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer)"); SafeRelease(bodyIndexFrame); SafeRelease(bodyIndexFrameReference); free(depthBuffer); SafeRelease(depthFrame); SafeRelease(depthFrameReference); SafeRelease(colorFrame); SafeRelease(colorFrameReference); SafeRelease(multiFrame); continue; } SafeRelease(colorFrameReference); SafeRelease(bodyIndexFrameReference); SafeRelease(depthFrameReference); break; } //深度値をBufferへ格納 // ERROR_CHECK(depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer)); //カラーマップの設定データの読み込みと、colorBufferメモリの確保 if(colorRGBX == NULL) { IFrameDescription *colorFrameDescription = NULL; ERROR_CHECK2(colorFrame->get_FrameDescription(&colorFrameDescription), "FrameDescription"); ERROR_CHECK2(colorFrameDescription->get_Width(&colorWidth), "get_Width"); ERROR_CHECK2(colorFrameDescription->get_Height(&colorHeight), "get_Height"); colorRGBX = new RGBQUAD[colorWidth * colorHeight]; glutReshapeWindow(width, height); ERROR_CHECK2(colorFrame->get_RawColorImageFormat(&imageFormat), "get_RawColorImageFormat"); SafeRelease(colorFrameDescription); } //カラーイメージをcolorBufferへコピー if(imageFormat == ColorImageFormat_Bgra) { ERROR_CHECK2(colorFrame->AccessRawUnderlyingBuffer(&colorBufferSize, reinterpret_cast<BYTE**>(&colorBuffer)), "AccessRawUnderlyingBuffer"); }else if(colorRGBX) { colorBuffer = colorRGBX; colorBufferSize = colorWidth * colorHeight * sizeof(RGBQUAD); ERROR_CHECK2(colorFrame->CopyConvertedFrameDataToArray(colorBufferSize, reinterpret_cast<BYTE*>(colorBuffer), ColorImageFormat_Bgra), "CopyConvertedFrameDataToArray"); }else { //Error } //colorMapの初期化 一度だけ実行される if(colorMap == NULL) { colorMap = new float[colorWidth * colorHeight][3]; } if (colorCoordinates == NULL) { colorCoordinates = new ColorSpacePoint[width * height]; } if (cameraSpacePoints == NULL) { cameraSpacePoints = new CameraSpacePoint[width * height]; } if (cameraSpacePoints_ave == NULL) { cameraSpacePoints_ave = new CameraSpacePoint[width * height]; } ERROR_CHECK2(coordinateMapper->MapDepthFrameToColorSpace( width * height, (UINT16*)depthBuffer, width * height, colorCoordinates), "MapDepthFrameToColorSpace"); ERROR_CHECK2(coordinateMapper->MapDepthFrameToCameraSpace( width * height, (UINT16*)depthBuffer, width * height, cameraSpacePoints),"MapDepthFrameToCameraSpace"); //colorBufferのデータを、colorMapにコピー for(int i = 0; i < height; i++){ for(int j = 0; j < width; j++){ int index = i * width + j; ColorSpacePoint colorPoint = colorCoordinates[index]; int colorX = (int)(floor(colorPoint.X + 0.5)); int colorY = (int)(floor(colorPoint.Y + 0.5)); if(colorX >= 0 && colorX < colorWidth && colorY >= 0 && colorY < colorHeight) { int colorIndex = colorX + colorY * colorWidth; //格納先はミラーモードを解除 float* colorp = colorMap[index]; UCHAR* data = (UCHAR*)(colorBuffer + colorIndex); colorp[0] = (float)data[2] / 255.0f; colorp[1] = (float)data[1] / 255.0f; colorp[2] = (float)data[0] / 255.0f; }else { float* colorp = colorMap[index]; colorp[0] = 0; colorp[1] = 0; colorp[2] = 0; } } } //フレームリソースを解放 SafeRelease(colorFrame); SafeRelease(multiFrame); SafeRelease(bodyIndexFrame); SafeRelease(depthFrame); }
void KinectManager::Update(void) { if (!m_pColorFrameReader) { return; } IColorFrame* pColorFrame = NULL; HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); if (SUCCEEDED(hr)) { INT64 nTime = 0; IFrameDescription* pFrameDescription = NULL; int nWidth = 0; int nHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; hr = pColorFrame->get_RelativeTime(&nTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pFrameDescription); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Width(&nWidth); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_Height(&nHeight); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_BytesPerPixel(&color_frame_bytes_per_pixel); } if (SUCCEEDED(hr)) { hr = pFrameDescription->get_LengthInPixels(&color_frame_length_in_pixels); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Rgba) { UINT nBufferSize = KINECT_STREAM_COLOR_WIDTH * KINECT_STREAM_COLOR_HEIGHT * color_frame_bytes_per_pixel; hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&m_pColorRGBX)); } else if (m_pColorRGBX) { IFrameDescription* pRGBFrameDescription = NULL; pColorFrame->CreateFrameDescription(ColorImageFormat_Rgba, &pRGBFrameDescription); unsigned int clpp = 0; pRGBFrameDescription->get_BytesPerPixel(&clpp); unsigned int clip = 0; pRGBFrameDescription->get_LengthInPixels(&clip); color_frame_bytes_per_pixel = clpp; color_frame_length_in_pixels = clip; UINT nBufferSize = clip * clpp; hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, (reinterpret_cast<BYTE*>(&m_pColorRGBX)), ColorImageFormat_Rgba); } else { hr = E_FAIL; } } SafeRelease(pFrameDescription); } SafeRelease(pColorFrame); }
void Microsoft2Grabber::ColorFrameArrived(IColorFrameReference* pColorFrameReference) { IColorFrame* pColorFrame = NULL; HRESULT hr = pColorFrameReference->AcquireFrame(&pColorFrame); if(FAILED(hr)) { //cout << "Couldn't acquire color frame" << endl; return; } //cout << "got a color frame" << endl; INT64 nColorTime = 0; IFrameDescription* pColorFrameDescription = NULL; int nColorWidth = 0; int nColorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = NULL; // get color frame data hr = pColorFrame->get_RelativeTime(&nColorTime); if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Width(&nColorWidth); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Height(&nColorHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (m_pColorRGBX) { pColorBuffer = m_pColorRGBX; nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } if(SUCCEEDED(hr)) { //WaitForSingleObject(hColorMutex,INFINITE); //cout << "creating the image" << endl; Mat tmp = Mat(m_colorSize, COLOR_PIXEL_TYPE, pColorBuffer, Mat::AUTO_STEP); boost::shared_ptr<Mat> img(new Mat()); *img = tmp.clone(); m_rgbTime = nColorTime; if (image_signal_->num_slots () > 0) { //cout << "img signal num slot!" << endl; image_signal_->operator()(img); } if (num_slots<sig_cb_microsoft_point_cloud_rgba>() > 0 || all_data_signal_->num_slots() > 0 || image_depth_image_signal_->num_slots() > 0) { rgb_sync_.add0 (img, m_rgbTime); } //ReleaseMutex(hColorMutex); } } SafeRelease(pColorFrameDescription); SafeRelease(pColorFrame); }
void capture(Image::Ptr& pImage) { HRESULT hr; if (m_pMultiSourceFrameReader==nullptr) { camera->getContext().error("CameraKinectDevice::capture: m_pMultiSourceFrameReader is nullptr\n"); // this is bad news - perhaps throw? return; // @@@ } IMultiSourceFrame* pMultiSourceFrame = nullptr; IDepthFrame* pDepthFrame = nullptr; IColorFrame* pColorFrame = nullptr; const golem::MSecTmU32 waitStep = 1; golem::MSecTmU32 timeWaited = 0; golem::Sleep timer; while (FAILED(hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame))) { // this is in CameraOpenNI, but suspect may be causing problem here // if (camera->isTerminating()) return; timer.msleep(waitStep); timeWaited += waitStep; if (timeWaited >= timeout) { camera->getContext().error("CameraKinectDevice::capture: failed to acquire frame within %d ms\n", timeout); // keep going - don't return with nothing; reset stopwatch @@@ timeWaited = 0; } } const golem::SecTmReal systemTime1 = camera->getContext().getTimer().elapsed(); if (SUCCEEDED(hr)) { IDepthFrameReference* pDepthFrameReference = nullptr; hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference); if (SUCCEEDED(hr)) { hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); } RELEASE_PTR(pDepthFrameReference); } if (SUCCEEDED(hr)) { IColorFrameReference* pColorFrameReference = nullptr; hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); if (SUCCEEDED(hr)) { hr = pColorFrameReference->AcquireFrame(&pColorFrame); } RELEASE_PTR(pColorFrameReference); } if (SUCCEEDED(hr)) { INT64 nDepthTime = 0; IFrameDescription* pDepthFrameDescription = nullptr; int nDepthWidth = 0; int nDepthHeight = 0; UINT nDepthBufferSize = 0; UINT16 *pDepthBuffer = nullptr; IFrameDescription* pColorFrameDescription = nullptr; int nColorWidth = 0; int nColorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = nullptr; // get depth frame data hr = pDepthFrame->get_RelativeTime(&nDepthTime); if (SUCCEEDED(hr)) hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription); if (SUCCEEDED(hr)) hr = pDepthFrameDescription->get_Width(&nDepthWidth); if (SUCCEEDED(hr)) hr = pDepthFrameDescription->get_Height(&nDepthHeight); if (SUCCEEDED(hr)) hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer); // get color frame data if (SUCCEEDED(hr)) hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); if (SUCCEEDED(hr)) hr = pColorFrameDescription->get_Width(&nColorWidth); if (SUCCEEDED(hr)) hr = pColorFrameDescription->get_Height(&nColorHeight); if (SUCCEEDED(hr)) hr = pColorFrame->get_RawColorImageFormat(&imageFormat); if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (m_pColorRGBX) { pColorBuffer = m_pColorRGBX; nColorBufferSize = nColorWidth * nColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } } cv::Mat colorFrame; colorFrame.create(cColorHeight, cColorWidth, CV_8UC4); colorFrame.setTo(cv::Scalar(0, 0, 0, 0)); //copying color buffer into cv::mat memcpy(colorFrame.data, pColorBuffer, (nColorWidth * nColorHeight) * 4); //originally kinect has mirrored image cv::flip(colorFrame, colorFrame, 1); //converstion to pImage which is BGR cv::Mat colorFrameT; cv::cvtColor(colorFrame, colorFrameT, CV_BGRA2BGR); if (mode & CameraKinect::MODE_COLOUR) { // initialize buffer if (pImage == nullptr) pImage.reset(new Image()); pImage->reserve((int)1920, (int)1080); //printf("Before clonning frame\n"); IplImage * img = cvCloneImage(&(IplImage)colorFrameT); cvCopy(img, pImage->image); cvReleaseImage(&img); //printf("After clonning frame\n"); const golem::SecTmReal systemTime2 = camera->getContext().getTimer().elapsed(); //seting timestamp pImage->timeStamp = REAL_HALF*(systemTime1 + systemTime2); } //camera->getContext().debug("After colour\n"); //needed for retain information of the begining of the buffer const UINT16* pBufferStart = pDepthBuffer; const UINT16* pBufferEnd = pDepthBuffer + (nDepthWidth * nDepthHeight); //copying depth buffer into cv::Mat long depthSizeP = (pBufferEnd - pBufferStart) * 2; cv::Mat depthFrame; depthFrame.create(cDepthHeight, cDepthWidth, CV_16UC1); depthFrame.setTo(cv::Scalar(0)); memcpy(depthFrame.data, pBufferStart, depthSizeP); //originally kinect has mirrored image cv::flip(depthFrame, depthFrame, 1); //camera->getContext().debug("After getting depth data\n"); //depth mode color data mapped into camera space (remember high resolution 1920x1080) if (mode & CameraKinect::MODE_DEPTH && (std::stoi(this->property.format.c_str()) == 101)) { //camera->getContext().debug("In depth mode\n"); if (pImage == nullptr) pImage.reset(new Image()); pImage->reserve((int)1920, (int)1080); pImage->cloud.reset(new PointSeq((uint32_t)property.width, (uint32_t)property.height)); //Obtaining the pointcloud hasImageStream = false; float missingPoint = std::numeric_limits<float>::quiet_NaN(); hr = m_pCoordinateMapper->MapColorFrameToCameraSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, cColorHeight * cColorWidth, m_pCameraCoordinatesColor); golem::Point* pOutWidth = &pImage->cloud->front(); CameraSpacePoint* pCamCWidth = m_pCameraCoordinatesColor; for (int y = 0; y < cColorHeight; ++y) { golem::Point* pOut = pOutWidth; CameraSpacePoint* pCamC = pCamCWidth; for (int x = 0; x < cColorWidth; ++x, pCamC++, ++pOut) { cv::Vec4b color; int abc = pCamC->Z; if (abc != 0) { pOut->x = pCamC->X; pOut->y = pCamC->Y; pOut->z = pCamC->Z; } else { pOut->x = pOut->y = pOut->z = missingPoint; //printf("Getting to this point4\n"); } pOut->normal_x = pOut->normal_y = pOut->normal_z = golem::numeric_const<float>::ZERO; color = colorFrame.at<cv::Vec4b>(y, x); if (hasImageStream) { pOut->b = color[0]; pOut->g = color[1]; pOut->r = color[2]; pOut->a = colour._rgba.a; } else { pOut->r = colour._rgba.r; pOut->g = colour._rgba.g; pOut->b = colour._rgba.b; pOut->a = colour._rgba.a; } } colorFrame += cColorWidth; pCamCWidth += cColorWidth; pOutWidth += cColorWidth; } } //depth mode depth data mapped into camera space if (mode & CameraKinect::MODE_DEPTH && (std::stoi(this->property.format.c_str()) == 102)) { if (pImage == nullptr) pImage.reset(new Image()); //camera->getContext().debug("In depth mode\n"); pImage->cloud.reset(new PointSeq((uint32_t)property.width, (uint32_t)property.height)); //Obtaining the pointcloud hasImageStream = false; float missingPoint = std::numeric_limits<float>::quiet_NaN(); hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, cDepthHeight * cDepthWidth, m_pCameraCoordinatesDepth); golem::Point* pOutWidth = &pImage->cloud->front(); CameraSpacePoint* pCamDWidth = m_pCameraCoordinatesDepth; for (int y = 0; y < cDepthHeight; ++y) { golem::Point* pOut = pOutWidth; CameraSpacePoint* pCamC = pCamDWidth; for (int x = 0; x < cDepthWidth; ++x, pCamC++, ++pOut) { cv::Vec4b color; //int abc = pCamC->Z; if (pCamC->Z != 0) { pOut->x = pCamC->X; pOut->y = pCamC->Y; pOut->z = pCamC->Z; } else { pOut->x = missingPoint; pOut->y = missingPoint; pOut->z = missingPoint; // //printf("Getting to this point4\n"); } pOut->normal_x = pOut->normal_y = pOut->normal_z = golem::numeric_const<float>::ZERO; /*color = colorframe.at<cv::vec4b>(y, x); if (hasimagestream) { pout->b = color[0]; pout->g = color[1]; pout->r = color[2]; pout->a = colour._rgba.a; }*/ //else { pOut->r = colour._rgba.r; pOut->g = colour._rgba.g; pOut->b = colour._rgba.b; pOut->a = colour._rgba.a; //} } //colorFrame += cDepthWidth; pCamDWidth += cDepthWidth; pOutWidth += cDepthWidth; } golem::Mat33 aMatrix; aMatrix.m11 = golem::Real(-1); aMatrix.m12 = golem::Real(0); aMatrix.m13 = golem::Real(0); aMatrix.m21 = golem::Real(0); aMatrix.m22 = golem::Real(-1); aMatrix.m23 = golem::Real(0); aMatrix.m31 = golem::Real(0); aMatrix.m32 = golem::Real(0); aMatrix.m33 = golem::Real(1); golem::Vec3 aVector; aVector.v1 = 0; aVector.v2 = 0; aVector.v3 = 0; golem::Mat34 aMatrix34; aMatrix34.R = aMatrix; aMatrix34.p = aVector; Cloud::PointSeqPtr pCloud = pImage->cloud; const Mat34 frame = Cloud::getSensorFrame(*pCloud); Cloud::transform(aMatrix34, *pCloud, *pCloud); Cloud::setSensorFrame(frame, *pCloud); // don't change the camera frame !!!!!!!!!!!!!!!!!!!!!!!!!!!! const golem::SecTmReal systemTime2 = camera->getContext().getTimer().elapsed(); //seting timestamp pImage->timeStamp = REAL_HALF*(systemTime1 + systemTime2); } RELEASE_PTR(pDepthFrameDescription); RELEASE_PTR(pColorFrameDescription); } RELEASE_PTR(pDepthFrame); RELEASE_PTR(pColorFrame); RELEASE_PTR(pMultiSourceFrame); }
bool ColorStream::readFrame(IMultiSourceFrame *multiFrame) { bool readed = false; if (!m_StreamHandle.colorFrameReader) { ofLogWarning("ofxKinect2::ColorStream") << "Stream is not open."; return readed; } Stream::readFrame(multiFrame); IColorFrame *colorFrame = nullptr; HRESULT hr = E_FAIL; if (!multiFrame) { hr = m_StreamHandle.colorFrameReader->AcquireLatestFrame(&colorFrame); } else { IColorFrameReference *colorFrameFeference = nullptr; hr = multiFrame->get_ColorFrameReference(&colorFrameFeference); if (SUCCEEDED(hr)) { hr = colorFrameFeference->AcquireFrame(&colorFrame); } safeRelease(colorFrameFeference); } if (SUCCEEDED(hr)) { IFrameDescription *colorFrameDescription = nullptr; ColorImageFormat imageFormat = ColorImageFormat_None; hr = colorFrame->get_RelativeTime((INT64 *)&m_Frame.timestamp); if (SUCCEEDED(hr)) { hr = colorFrame->get_FrameDescription(&colorFrameDescription); } if (SUCCEEDED(hr)) { hr = colorFrameDescription->get_Width(&m_Frame.width); } if (SUCCEEDED(hr)) { hr = colorFrameDescription->get_Height(&m_Frame.height); } if (SUCCEEDED(hr)) { hr = colorFrameDescription->get_HorizontalFieldOfView(&m_Frame.horizontalFieldOfView); } if (SUCCEEDED(hr)) { hr = colorFrameDescription->get_VerticalFieldOfView(&m_Frame.verticalFieldOfView); } if (SUCCEEDED(hr)) { hr = colorFrameDescription->get_DiagonalFieldOfView(&m_Frame.diagonalFieldOfView); } if (SUCCEEDED(hr)) { hr = colorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (m_Buffer == nullptr) { m_Buffer = new unsigned char[m_Frame.width * m_Frame.height * 4]; } if (imageFormat == ColorImageFormat_Rgba) { hr = colorFrame->AccessRawUnderlyingBuffer((UINT *)&m_Frame.dataSize, reinterpret_cast<BYTE **>(&m_Frame.data)); } else { m_Frame.data = m_Buffer; m_Frame.dataSize = m_Frame.width * m_Frame.height * 4 * sizeof(unsigned char); hr = colorFrame->CopyConvertedFrameDataToArray((UINT)m_Frame.dataSize, reinterpret_cast<BYTE *>(m_Frame.data), ColorImageFormat_Rgba); } } if (SUCCEEDED(hr)) { readed = true; setPixels(m_Frame); } safeRelease(colorFrameDescription); } safeRelease(colorFrame); return readed; }
bool KinectV2Controller::UpdateCamera() { if(InitializeMultiFrameReader()) { IMultiSourceFrame* pMultiSourceFrame = NULL; IDepthFrame* pDepthFrame = NULL; IColorFrame* pColorFrame = NULL; IInfraredFrame* pInfraRedFrame = NULL; HRESULT hr = -1; static DWORD lastTime = 0; DWORD currentTime = GetTickCount(); //Check if we do not request data faster than 30 FPS. Kinect V2 can only deliver 30 FPS. if( unsigned int(currentTime - lastTime) > 33 ) { hr = d->m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame); lastTime = currentTime; } if (SUCCEEDED(hr)) { IDepthFrameReference* pDepthFrameReference = NULL; hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference); if (SUCCEEDED(hr)) { hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); } SafeRelease(pDepthFrameReference); } if (SUCCEEDED(hr)) { IColorFrameReference* pColorFrameReference = NULL; hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); if (SUCCEEDED(hr)) { hr = pColorFrameReference->AcquireFrame(&pColorFrame); } SafeRelease(pColorFrameReference); } if (SUCCEEDED(hr)) { IInfraredFrameReference* pInfraredFrameReference = NULL; hr = pMultiSourceFrame->get_InfraredFrameReference(&pInfraredFrameReference); if (SUCCEEDED(hr)) { hr = pInfraredFrameReference->AcquireFrame(&pInfraRedFrame); } SafeRelease(pInfraredFrameReference); } if (SUCCEEDED(hr)) { UINT nDepthBufferSize = 0; UINT16 *pDepthBuffer = NULL; UINT16 *pIntraRedBuffer = NULL; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = NULL; if (SUCCEEDED(hr)) { hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer); } if (SUCCEEDED(hr)) { hr = pInfraRedFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pIntraRedBuffer); } if (SUCCEEDED(hr)) { for(int i = 0; i < d->m_DepthCaptureHeight*d->m_DepthCaptureWidth; ++i) { d->m_Distances[i] = static_cast<float>(*pDepthBuffer); d->m_Amplitudes[i] = static_cast<float>(*pIntraRedBuffer); ++pDepthBuffer; ++pIntraRedBuffer; } } else { MITK_ERROR << "AccessUnderlyingBuffer"; } // get color frame data if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (d->m_pColorRGBX) { pColorBuffer = d->m_pColorRGBX; nColorBufferSize = d->m_RGBCaptureWidth * d->m_RGBCaptureHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } if (SUCCEEDED(hr)) { for(int i = 0; i < d->m_RGBBufferSize; i+=3) { //convert from BGR to RGB d->m_Colors[i+0] = pColorBuffer->rgbRed; d->m_Colors[i+1] = pColorBuffer->rgbGreen; d->m_Colors[i+2] = pColorBuffer->rgbBlue; ++pColorBuffer; } } } } SafeRelease(pDepthFrame); SafeRelease(pColorFrame); SafeRelease(pInfraRedFrame); SafeRelease(pMultiSourceFrame); if( hr != -1 && !SUCCEEDED(hr) ) { //The thread gets here, if the data is requested faster than the device can deliver it. //This may happen from time to time. return false; } return true; } MITK_ERROR << "Unable to initialize MultiFrameReader"; return false; }
void KinectGrabber::GetNextFrame() { if (!m_pMultiSourceFrameReader) { return; } IMultiSourceFrame* pMultiSourceFrame = NULL; IDepthFrame* pDepthFrame = NULL; IColorFrame* pColorFrame = NULL; IBodyIndexFrame* pBodyIndexFrame = NULL; HRESULT hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame); if (SUCCEEDED(hr)) { IDepthFrameReference* pDepthFrameReference = NULL; hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference); if (SUCCEEDED(hr)) { hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); } SafeRelease(pDepthFrameReference); } if (SUCCEEDED(hr)) { IColorFrameReference* pColorFrameReference = NULL; hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); if (SUCCEEDED(hr)) { hr = pColorFrameReference->AcquireFrame(&pColorFrame); } SafeRelease(pColorFrameReference); } if (SUCCEEDED(hr)) { IBodyIndexFrameReference* pBodyIndexFrameReference = NULL; hr = pMultiSourceFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference); if (SUCCEEDED(hr)) { hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame); } SafeRelease(pBodyIndexFrameReference); } if (SUCCEEDED(hr)) { INT64 nDepthTime = 0; IFrameDescription* pDepthFrameDescription = NULL; int nDepthWidth = 0; int nDepthHeight = 0; UINT nDepthBufferSize = 0; IFrameDescription* pColorFrameDescription = NULL; int nColorWidth = 0; int nColorHeight = 0; ColorImageFormat imageFormat = ColorImageFormat_None; UINT nColorBufferSize = 0; RGBQUAD *pColorBuffer = NULL; IFrameDescription* pBodyIndexFrameDescription = NULL; int nBodyIndexWidth = 0; int nBodyIndexHeight = 0; UINT nBodyIndexBufferSize = 0; BYTE *pBodyIndexBuffer = NULL; // get depth frame data hr = pDepthFrame->get_RelativeTime(&nDepthTime); if (SUCCEEDED(hr)) { hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription); } if (SUCCEEDED(hr)) { hr = pDepthFrameDescription->get_Width(&nDepthWidth); } if (SUCCEEDED(hr)) { hr = pDepthFrameDescription->get_Height(&nDepthHeight); } if (SUCCEEDED(hr)) { //m_pDepthBuffer = new UINT16[cDepthWidth * cDepthHeight]; hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &m_pDepthBuffer); //pDepthFrame->CopyFrameDataToArray(nDepthBufferSize,m_pDepthBuffer); WaitForSingleObject(hDepthMutex,INFINITE); m_depthImage.release(); Mat tmp = Mat(m_depthSize, DEPTH_PIXEL_TYPE, m_pDepthBuffer, Mat::AUTO_STEP); m_depthImage = tmp.clone(); //need to deep copy because of the call to SafeRelease(pDepthFrame) to prevent access violation ReleaseMutex(hDepthMutex); } // get color frame data if (SUCCEEDED(hr)) { hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Width(&nColorWidth); } if (SUCCEEDED(hr)) { hr = pColorFrameDescription->get_Height(&nColorHeight); } if (SUCCEEDED(hr)) { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); } if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) { hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer)); } else if (m_pColorRGBX) { pColorBuffer = m_pColorRGBX; nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra); } else { hr = E_FAIL; } if(SUCCEEDED(hr)) { WaitForSingleObject(hColorMutex,INFINITE); m_colorImage.release(); Mat tmp = Mat(m_colorSize, COLOR_PIXEL_TYPE, pColorBuffer, Mat::AUTO_STEP); m_colorImage = tmp.clone(); ReleaseMutex(hColorMutex); } } // get body index frame data if (SUCCEEDED(hr)) { hr = pBodyIndexFrame->get_FrameDescription(&pBodyIndexFrameDescription); } if (SUCCEEDED(hr)) { hr = pBodyIndexFrameDescription->get_Width(&nBodyIndexWidth); } if (SUCCEEDED(hr)) { hr = pBodyIndexFrameDescription->get_Height(&nBodyIndexHeight); } if (SUCCEEDED(hr)) { hr = pBodyIndexFrame->AccessUnderlyingBuffer(&nBodyIndexBufferSize, &pBodyIndexBuffer); } SafeRelease(pDepthFrameDescription); SafeRelease(pColorFrameDescription); SafeRelease(pBodyIndexFrameDescription); } SafeRelease(pDepthFrame); SafeRelease(pColorFrame); SafeRelease(pBodyIndexFrame); SafeRelease(pMultiSourceFrame); }