void idle() { // Read color data IColorFrame* pCFrame = nullptr; if (pColorFrameReader->AcquireLatestFrame(&pCFrame) == S_OK) { pCFrame->CopyConvertedFrameDataToArray(uColorBufferSize, pColorBuffer, ColorImageFormat_Rgba); pCFrame->Release(); pCFrame = nullptr; } // Read depth data IDepthFrame* pDFrame = nullptr; if (pDepthFrameReader->AcquireLatestFrame(&pDFrame) == S_OK) { pDFrame->CopyFrameDataToArray(uDepthPointNum, pDepthBuffer); pDFrame->Release(); pDFrame = nullptr; // map to camera space pCoordinateMapper->MapColorFrameToCameraSpace(uDepthPointNum, pDepthBuffer, uColorPointNum, pCSPoints); } }
// デプスデータを取得する GLuint KinectV2::getDepth() const { // デプスのテクスチャを指定する glBindTexture(GL_TEXTURE_2D, depthTexture); // 次のデプスのフレームデータが到着していれば IDepthFrame *depthFrame; if (depthReader->AcquireLatestFrame(&depthFrame) == S_OK) { // デプスデータのサイズと格納場所を得る UINT depthSize; UINT16 *depthBuffer; depthFrame->AccessUnderlyingBuffer(&depthSize, &depthBuffer); // カラーのテクスチャ座標を求めてバッファオブジェクトに転送する glBindBuffer(GL_ARRAY_BUFFER, coordBuffer); ColorSpacePoint *const texcoord(static_cast<ColorSpacePoint *>(glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY))); coordinateMapper->MapDepthFrameToColorSpace(depthCount, depthBuffer, depthCount, texcoord); glUnmapBuffer(GL_ARRAY_BUFFER); // デプスデータをテクスチャに転送する glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, depthWidth, depthHeight, GL_RED, GL_UNSIGNED_SHORT, depthBuffer); // デプスフレームを開放する depthFrame->Release(); } return depthTexture; }
// カメラ座標を取得する GLuint KinectV2::getPoint() const { // カメラ座標のテクスチャを指定する glBindTexture(GL_TEXTURE_2D, pointTexture); // 次のデプスのフレームデータが到着していれば IDepthFrame *depthFrame; if (depthReader->AcquireLatestFrame(&depthFrame) == S_OK) { // デプスデータのサイズと格納場所を得る UINT depthSize; UINT16 *depthBuffer; depthFrame->AccessUnderlyingBuffer(&depthSize, &depthBuffer); // カラーのテクスチャ座標を求めてバッファオブジェクトに転送する glBindBuffer(GL_ARRAY_BUFFER, coordBuffer); ColorSpacePoint *const texcoord(static_cast<ColorSpacePoint *>(glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY))); coordinateMapper->MapDepthFrameToColorSpace(depthCount, depthBuffer, depthCount, texcoord); glUnmapBuffer(GL_ARRAY_BUFFER); // カメラ座標への変換テーブルを得る UINT32 entry; PointF *table; coordinateMapper->GetDepthFrameToCameraSpaceTable(&entry, &table); // すべての点について for (unsigned int i = 0; i < entry; ++i) { // デプス値の単位をメートルに換算する係数 static const GLfloat zScale(-0.001f); // その点のデプス値を得る const unsigned short d(depthBuffer[i]); // デプス値の単位をメートルに換算する (計測不能点は maxDepth にする) const GLfloat z(d == 0 ? -maxDepth : GLfloat(d) * zScale); // その点のスクリーン上の位置を求める const GLfloat x(table[i].X); const GLfloat y(-table[i].Y); // その点のカメラ座標を求める position[i][0] = x * z; position[i][1] = y * z; position[i][2] = z; } // カメラ座標を転送する glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, depthWidth, depthHeight, GL_RGB, GL_FLOAT, position); // テーブルを開放する CoTaskMemFree(table); // デプスフレームを開放する depthFrame->Release(); } return pointTexture; }
bool KinectInterface::getFrameData(IMultiSourceFrame* frame, cv::Mat& intensity_mat, cv::Mat& depth_mat, cv::Mat& pos_mat) { //Obtain depth frame IDepthFrame* depthframe = nullptr; if (FAILED(depthFrameReader->AcquireLatestFrame(&depthframe))) return false; if (!depthframe) return false; // Get data from frame unsigned int sz; unsigned short* buf; if (FAILED(depthframe->AccessUnderlyingBuffer(&sz, &buf))) return false; //get depth -> xyz mapping if (FAILED(mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz))) return false; //get depth -> rgb image mapping if (FAILED(mapper->MapDepthFrameToColorSpace(width*height, buf, width*height, depth2rgb))) return false; //save depth if (FAILED(depthframe->CopyFrameDataToArray(height * width, depth_data))); if (depthframe) depthframe->Release(); //Obtain RGB frame IColorFrame* colorframe; if (FAILED(colorFrameReader->AcquireLatestFrame(&colorframe))) return false; if (!colorframe) return false; // Get data from frame if (FAILED(colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight * 4, rgbimage, ColorImageFormat_Rgba))) return false; cv::Mat tmp_depth = cv::Mat::zeros(colorheight, colorwidth, CV_16UC1); cv::Mat tmp_pos = cv::Mat::zeros(colorheight, colorwidth, CV_32FC3); cv::Mat depth_org(height, width, CV_16UC1, depth_data); cv::Mat tmp_rgb(colorheight, colorwidth, CV_8UC4, rgbimage); // Write color array for vertices for (int i = 0; i < width*height; i++) { ColorSpacePoint p = depth2rgb[i]; int iY = (int)(p.Y + 0.5); int iX = (int)(p.X + 0.5); if (iX >= 0 && iY >= 0 && iX < colorwidth && iY < colorheight) { // Check if color pixel coordinates are in bounds tmp_depth.at<unsigned short>(iY, iX) = depth_data[i]; //tmp_pos.at<float>(iY, iX, 0) = depth2xyz[i].X; //tmp_pos.at<float>(iY, iX, 1) = depth2xyz[i].Y; //tmp_pos.at<float>(iY, iX, 2) = depth2xyz[i].Z; } } if (colorframe) colorframe->Release(); cv::resize(tmp_rgb(cv::Rect(240, 0, 1440, 1080)), intensity_mat, cv::Size(640, 480)); cv::resize(tmp_depth(cv::Rect(240, 0, 1440, 1080)), depth_mat, cv::Size(640, 480)); cv::resize(tmp_pos(cv::Rect(240, 0, 1440, 1080)), pos_mat, cv::Size(640, 480)); cv::cvtColor(intensity_mat, intensity_mat, CV_RGBA2GRAY); return true; }
void MKinect::getDepthData(IMultiSourceFrame* frame, float* dest) { IDepthFrame* depthframe; IDepthFrameReference* frameref = NULL; frame->get_DepthFrameReference(&frameref); frameref->AcquireFrame(&depthframe); if (frameref) frameref->Release(); if (!depthframe) return; // Get data from frame unsigned int sz; UINT16 * buf; while (!SUCCEEDED(depthframe->AccessUnderlyingBuffer(&sz, &buf))) { } HRESULT res = S_OK; res = mapper->MapDepthFrameToCameraSpace( KinectColorWidth*KinectColorHeight, buf, // Depth frame data and size of depth frame KinectColorWidth*KinectColorHeight, depth2xyz); // Output CameraSpacePoint array and size // Process depth frame data... if (depthframe) depthframe->Release(); }
int main(int argc, char** argv) { // 1a. Get default Sensor cout << "Try to get default sensor" << endl; IKinectSensor* pSensor = nullptr; if (GetDefaultKinectSensor(&pSensor) != S_OK) { cerr << "Get Sensor failed" << endl; } else { // 1b. Open sensor cout << "Try to open sensor" << endl; if (pSensor->Open() != S_OK) { cerr << "Can't open sensor" << endl; } else { // 2a. Get frame source cout << "Try to get source" << endl; IDepthFrameSource* pFrameSource = nullptr; if (pSensor->get_DepthFrameSource(&pFrameSource) != S_OK) { cerr << "Can't get frame source" << endl; } else { // 2b. Get frame description int iWidth = 0; int iHeight = 0; IFrameDescription* pFrameDescription = nullptr; if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK) { pFrameDescription->get_Width(&iWidth); pFrameDescription->get_Height(&iHeight); pFrameDescription->Release(); pFrameDescription = nullptr; } // 2c. get some dpeth only meta UINT16 uDepthMin = 0, uDepthMax = 0; pFrameSource->get_DepthMinReliableDistance(&uDepthMin); pFrameSource->get_DepthMaxReliableDistance(&uDepthMax); cout << "Reliable Distance: " << uDepthMin << " - " << uDepthMax << endl; // perpare OpenCV cv::Mat mDepthImg(iHeight, iWidth, CV_16UC1); cv::Mat mImg8bit(iHeight, iWidth, CV_8UC1); cv::namedWindow( "Depth Map" ); // 3a. get frame reader cout << "Try to get frame reader" << endl; IDepthFrameReader* pFrameReader = nullptr; if (pFrameSource->OpenReader(&pFrameReader) != S_OK) { cerr << "Can't get frame reader" << endl; } else { // Enter main loop cout << "Enter main loop" << endl; while (true) { // 4a. Get last frame IDepthFrame* pFrame = nullptr; if (pFrameReader->AcquireLatestFrame(&pFrame) == S_OK) { // 4c. copy the depth map to image if (pFrame->CopyFrameDataToArray(iWidth * iHeight, reinterpret_cast<UINT16*>(mDepthImg.data)) == S_OK) { // 4d. convert from 16bit to 8bit mDepthImg.convertTo(mImg8bit, CV_8U, 255.0f / uDepthMax); cv::imshow("Depth Map", mImg8bit); } else { cerr << "Data copy error" << endl; } // 4e. release frame pFrame->Release(); } // 4f. check keyboard input if (cv::waitKey(30) == VK_ESCAPE){ break; } } // 3b. release frame reader cout << "Release frame reader" << endl; pFrameReader->Release(); pFrameReader = nullptr; } // 2d. release Frame source cout << "Release frame source" << endl; pFrameSource->Release(); pFrameSource = nullptr; } // 1c. Close Sensor cout << "close sensor" << endl; pSensor->Close(); } // 1d. Release Sensor cout << "Release sensor" << endl; pSensor->Release(); pSensor = nullptr; } return 0; }
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; } }
int main(int argc, char* argv[]) { int similarity = 0; string line; ifstream myfile ("source_config.txt"); if (myfile.is_open()) { getline (myfile,line); hauteurCamera = stoi(line); getline (myfile,line); fountainXPosition = stoi(line); getline (myfile,line); fountainYPosition = stoi(line); getline (myfile,line); fountainWidth = stoi(line); getline (myfile,line); blasterWidth = stoi(line); getline (myfile,line); int numberOfBlaster = stoi(line); for(int i = 0;i<numberOfBlaster;i++){ getline (myfile,line); blasterXPosition.push_back(stoi(line)); getline (myfile,line); blasterYPosition.push_back(stoi(line)); } myfile.close(); } else { cout << "Unable to open file"; exit(-1); } IKinectSensor* m_pKinectSensor; IDepthFrameReader* pDepthReader; IDepthFrameSource* pDepthFrameSource = NULL; // Depth image IColorFrameReader* pColorReader; IColorFrameSource* pColorFrameSource = NULL; HRESULT hr; hr = GetDefaultKinectSensor(&m_pKinectSensor); if (FAILED(hr)){ cout << "ColorTrackerv2 Error : GetDefaultKinectSensor failed." << endl; return false; } hr = m_pKinectSensor->Open(); if (FAILED(hr)){ cout << "ColorTrackerv2 Error : Open failed." << endl; return false; } hr = m_pKinectSensor->get_DepthFrameSource( &pDepthFrameSource ); if (FAILED(hr)){ cout << "ColorTrackerv2 Error : get_DepthFrameSource failed." << endl; return false; } hr = pDepthFrameSource->OpenReader( &pDepthReader ); if (FAILED(hr)){ cout << "ColorTrackerv2 Error : OpenReader failed." << endl; return false; } IFrameDescription* pDepthDescription; hr = pDepthFrameSource->get_FrameDescription( &pDepthDescription ); if( FAILED( hr ) ){ std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl; return -1; } w = 0,h = 0; pDepthDescription->get_Width( &w ); // 512 pDepthDescription->get_Height( &h ); // 424 //unsigned int irBufferSize = w * h * sizeof( unsigned short ); hr = m_pKinectSensor->get_ColorFrameSource( &pColorFrameSource ); if (FAILED(hr)){ cout << "ColorTrackerv2 Error : get_ColorFrameSource failed." << endl; return false; } hr = pColorFrameSource->OpenReader( &pColorReader ); if (FAILED(hr)){ cout << "ColorTrackerv2 Error : OpenReader failed." << endl; return false; } // Description IFrameDescription* pColorDescription; hr = pColorFrameSource->get_FrameDescription( &pColorDescription ); if( FAILED( hr ) ){ std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl; return -1; } int colorWidth = 0; int colorHeight = 0; pColorDescription->get_Width( &colorWidth ); // 1920 pColorDescription->get_Height( &colorHeight ); // 1080 unsigned int colorBufferSize = colorWidth * colorHeight * 4 * sizeof( unsigned char ); Mat colorBufferMat = Mat::zeros( cvSize(colorWidth,colorHeight), CV_8UC4 ); //colorMat = Mat::zeros( cvSize(colorWidth/2,colorHeight/2), CV_8UC4 ); ICoordinateMapper* pCoordinateMapper; hr = m_pKinectSensor->get_CoordinateMapper( &pCoordinateMapper ); // open a opencv window cv::namedWindow("source_config", CV_WINDOW_AUTOSIZE ); setMouseCallback("source_config", MouseCallBackFunc, NULL); Mat frame(h,w, CV_8UC3, Scalar(255,255,255)); Mat display; //Mat img; char k = 0; while(k!=27){ HRESULT hResult = S_OK; if(displayColor){ IColorFrame* pColorFrame = nullptr; hResult = pColorReader->AcquireLatestFrame( &pColorFrame ); while(!SUCCEEDED(hResult)){ Sleep(50); hResult = pColorReader->AcquireLatestFrame(&pColorFrame); } if( SUCCEEDED( hResult ) ){ hResult = pColorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<BYTE*>( colorBufferMat.data ), ColorImageFormat::ColorImageFormat_Bgra ); if( !SUCCEEDED( hResult ) ){ return false; } resize(colorBufferMat,display,Size(displaySize*w,displaySize*h)); flip(display,display,1); cv::line(display,Point(displaySize*w/2,0),Point(displaySize*w/2,displaySize*h),Scalar(0,0,255),2); cv::line(display,Point(0,displaySize*h/2),Point(displaySize*w,displaySize*h/2),Scalar(0,0,255),2); if (pColorFrame ) { pColorFrame ->Release(); pColorFrame = NULL; } } else return false; } else { IDepthFrame* pDepthFrame = nullptr; hResult = pDepthReader->AcquireLatestFrame( &pDepthFrame ); while(!SUCCEEDED(hResult)){ Sleep(10); hResult = pDepthReader->AcquireLatestFrame( &pDepthFrame ); } if( SUCCEEDED( hResult ) ){ unsigned int bufferSize = 0; unsigned short* buffer = nullptr; hResult = pDepthFrame->AccessUnderlyingBuffer( &bufferSize, &buffer ); if( SUCCEEDED( hResult ) ){ for( int y = 0; y < h; y++ ){ for( int x = 0; x < w; x++ ){ Vec3b intensity = frame.at<Vec3b>(y, x); if(buffer[ y * w + (w - x - 1) ] < hauteurCamera){ int d = buffer[ y * w + (w - x - 1) ]; intensity.val[0] = 2.55*(d % 100); intensity.val[1] = 1.22*(d % 200); intensity.val[2] = 256.0*d/hauteurCamera; } else { intensity.val[0] = 255; intensity.val[1] = 255; intensity.val[2] = 255; } /*intensity.val[0] = buffer[ y * w + x ] >> 8; intensity.val[1] = buffer[ y * w + x ] >> 8; intensity.val[2] = buffer[ y * w + x ] >> 8;*/ frame.at<Vec3b>(y, x) = intensity; } } // changer la couleur du rectangle en fonction de la hauteur des coins (similaire ou non) ( moins de 4cm) float d1 = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))]; float d2 = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))]; float d3 = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))]; float d4 = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))]; if((d1 < 0)||(d1>3500)||(d2 < 0)||(d2>3500)||(d3 < 0)||(d3>3500)||(d4 < 0)||(d4>3500)){ similarity = 0; } else { int mn = 100; if((abs(d1-d2) < mn) &&(abs(d1-d3) < mn) &&(abs(d1-d4) < mn) &&(abs(d2-d3) < mn) &&(abs(d2-d4) < mn) &&(abs(d3-d4) < mn)) similarity = 255; else{ int md = abs(d1-d2); md = MAX(md,abs(d1-d3)); md = MAX(md,abs(d1-d4)); md = MAX(md,abs(d2-d3)); md = MAX(md,abs(d2-d4)); md = MAX(md,abs(d3-d4)); if(md-mn>128) similarity = 0; else similarity = 128 - (md - mn); } } if(k=='s'){ // get hauteur camera // Depthframe get 3D position of 1m20 jets et les enregistrer dans un autre fichier pour etre charger par un Observer CameraSpacePoint cameraPoint = { 0 }; DepthSpacePoint depthPoint = { 0 }; UINT16 depth; // Compute hauteur camera, by average of four points float h = 0; int cptH = 0; float d; d = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))]; if((d>500)&&(d<3500)){h+=d; cptH++;} d = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))]; if((d>500)&&(d<3500)){h+=d; cptH++;} d = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))]; if((d>500)&&(d<3500)){h+=d; cptH++;} d = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))]; if((d>500)&&(d<3500)){h+=d; cptH++;} if(cptH>0){ //hauteurCamera = h/cptH; cout << "H = " << hauteurCamera << endl; } // Compute real size of blaster /*depthPoint.X = static_cast<float>(blasterXPosition[0] - blasterWidth); depthPoint.Y = static_cast<float>(blasterYPosition[0] - blasterWidth); depth = hauteurCamera - 1000.0f; // TODO change pCoordinateMapper->MapDepthPointToCameraSpace(depthPoint,depth,&cameraPoint); float corner1X = static_cast<float>(cameraPoint.X); float corner1Y = static_cast<float>(cameraPoint.Y); depthPoint.X = static_cast<float>(blasterXPosition[0] + blasterWidth); depthPoint.Y = static_cast<float>(blasterYPosition[0] + blasterWidth); pCoordinateMapper->MapDepthPointToCameraSpace(depthPoint,depth,&cameraPoint); float corner2X = static_cast<float>(cameraPoint.X); float corner2Y = static_cast<float>(cameraPoint.Y);*/ //float realBlasterWidth = 1000.0*(abs(corner2X-corner1X)+abs(corner2Y-corner1Y))/2.0; ofstream myfile; myfile.open ("source_config.txt"); myfile << hauteurCamera << "\n"; myfile << fountainXPosition << "\n"; myfile << fountainYPosition << "\n"; myfile << fountainWidth << "\n"; myfile << blasterWidth << "\n"; myfile << blasterXPosition.size() << "\n"; for(int i = 0;i<blasterXPosition.size();i++){ myfile << blasterXPosition[i] << "\n"; myfile << blasterYPosition[i] << "\n"; } myfile.close(); // save real positions to file myfile.open ("source3D.txt"); myfile << hauteurCamera << "\n"; myfile << blasterWidth << "\n"; myfile << blasterXPosition.size() << "\n"; for(int i = 0;i<blasterXPosition.size();i++){ depthPoint.X = static_cast<float>(blasterXPosition[i]); depthPoint.Y = static_cast<float>(blasterYPosition[i]); depth = hauteurCamera; pCoordinateMapper->MapDepthPointToCameraSpace(depthPoint,depth,&cameraPoint); cout << depthPoint.X << " - " << depthPoint.Y << endl; cout << static_cast<float>(cameraPoint.X) << " - " << static_cast<float>(cameraPoint.Y) << endl; pCoordinateMapper->MapCameraPointToDepthSpace(cameraPoint, &depthPoint); cout << depthPoint.X << " - " << depthPoint.Y << endl; cout << static_cast<float>(cameraPoint.X) << " - " << static_cast<float>(cameraPoint.Y) << endl; myfile << 1000.0*static_cast<float>(cameraPoint.X) << "\n"; myfile << 1000.0*static_cast<float>(cameraPoint.Y) << "\n"; } myfile.close(); } } else{ return false; } } else return false; if( pDepthFrame != NULL ){ pDepthFrame->Release(); pDepthFrame = NULL; } rectangle(frame, Rect(fountainXPosition-fountainWidth/2.0, fountainYPosition-fountainWidth/2.0, fountainWidth , fountainWidth), Scalar(0,similarity,255-similarity), 3); for(int i = -2; i <= 2; i++) { rectangle(frame, Rect(fountainXPosition - (i*blasterWidth) - (blasterWidth/2.0), fountainYPosition-fountainWidth/2.0, blasterWidth, fountainWidth), Scalar(0,255,0), 1); rectangle(frame, Rect(fountainXPosition-fountainWidth/2.0, fountainYPosition - (i*blasterWidth) - (blasterWidth/2.0), fountainWidth, blasterWidth), Scalar(0,255,0), 1); } char textbuffer [33]; for(int i = 0;i<blasterXPosition.size();i++){ sprintf(textbuffer,"%i",i+1); putText(frame,textbuffer, Point2f(blasterXPosition[i]-blasterWidth/2.0,blasterYPosition[i]), FONT_HERSHEY_PLAIN, 1, Scalar(0,0,255,255), 2); //rectangle(frame,Rect(blasterXPosition[i]-blasterWidth/2.0,blasterYPosition[i]-blasterWidth/2.0,blasterWidth,blasterWidth),Scalar(255,0,0)); circle(frame,Point(blasterXPosition[i],blasterYPosition[i]),blasterWidth/2.0,Scalar(255,0,0)); } resize(frame,display,Size(displaySize*w,displaySize*h)); } cv::imshow("source_config", display); k=cv::waitKey(1); if(k == 32) // Space displayColor = ! displayColor; if(k=='a') alignBuseFromLayout(); if(k=='r') resetFountainPosition(); } if (pDepthReader) { pDepthReader->Release(); pDepthReader = NULL; } if (pCoordinateMapper) { pCoordinateMapper->Release(); pCoordinateMapper = NULL; } m_pKinectSensor->Close(); if (m_pKinectSensor) { m_pKinectSensor->Release(); m_pKinectSensor = NULL; } //system("pause"); return 0; }
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; }
void KinectDevice::listen() { if (_listening) throw std::exception("Already listening for new frames"); _listening = true; while (_listening) { int idx = WaitForSingleObject((HANDLE) _frameEvent, 100); switch (idx) { case WAIT_TIMEOUT: std::cout << "."; continue; case WAIT_OBJECT_0: IMultiSourceFrameArrivedEventArgs *frameArgs = nullptr; IMultiSourceFrameReference *frameRef = nullptr; HRESULT hr = _reader->GetMultiSourceFrameArrivedEventData(_frameEvent, &frameArgs); if (hr == S_OK) { hr = frameArgs->get_FrameReference(&frameRef); frameArgs->Release(); } if (hr == S_OK) { //if (_lastFrame) _lastFrame->Release(); hr = frameRef->AcquireFrame(&_lastFrame); frameRef->Release(); } if (hr == S_OK) { // Store frame data //std::cout << "Got a frame YEAH" << std::endl; IDepthFrameReference *depthRef = nullptr; IColorFrameReference *colorRef = nullptr; IInfraredFrameReference *irRef = nullptr; ILongExposureInfraredFrameReference *hdirRef = nullptr; IBodyIndexFrameReference *indexRef = nullptr; IDepthFrame *depth = nullptr; IColorFrame *color = nullptr; IInfraredFrame *ir = nullptr; ILongExposureInfraredFrame *hdir = nullptr; IBodyIndexFrame *index = nullptr; size_t size; uint16_t *buff; BYTE *cbuff; frameLock.lock(); if (_streams & Streams::DEPTH_STREAM) { _lastFrame->get_DepthFrameReference(&depthRef); depthRef->AcquireFrame(&depth); if (depth) { depthSwap(); depth->AccessUnderlyingBuffer(&size, &buff); memcpy(depthData.get(), buff, size * sizeof(uint16_t)); depth->Release(); } depthRef->Release(); } if (_streams & Streams::COLOR_STREAM) { _lastFrame->get_ColorFrameReference(&colorRef); colorRef->AcquireFrame(&color); //color->AccessUnderlyingBuffer(&size, &buff); //memcpy(_colorData.get(), buff, size); color->Release(); colorRef->Release(); } if (_streams & Streams::IR_STREAM) { _lastFrame->get_InfraredFrameReference(&irRef); irRef->AcquireFrame(&ir); ir->AccessUnderlyingBuffer(&size, &buff); memcpy(irData.get(), buff, size); ir->Release(); irRef->Release(); } if (_streams & Streams::HDIR_STREAM) { _lastFrame->get_LongExposureInfraredFrameReference(&hdirRef); hdirRef->AcquireFrame(&hdir); hdir->AccessUnderlyingBuffer(&size, &buff); memcpy(hdirData.get(), buff, size); hdir->Release(); hdirRef->Release(); } if (_streams & Streams::INDEX_STREAM) { _lastFrame->get_BodyIndexFrameReference(&indexRef); indexRef->AcquireFrame(&index); index->AccessUnderlyingBuffer(&size, &cbuff); memcpy(indexData.get(), cbuff, size); index->Release(); indexRef->Release(); } frameLock.unlock(); _lastFrame->Release(); } } } }
int TextureManager::CreateSensorTexture(char *errorString, const char *name) { if (!depth_frame_reader_) { sprintf_s(errorString, MAX_ERROR_LENGTH, "No depth sensor exists for texture creation"); return -1; } glGenTextures(1, textureID + numTextures); strcpy_s(textureName[numTextures], TM_MAX_FILENAME_LENGTH, name); glBindTexture(GL_TEXTURE_2D, textureID[numTextures]); glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); //gluBuild2DMipmaps(GL_TEXTURE_2D, GL_RGBA, // TM_NOISE_TEXTURE_SIZE, TM_NOISE_TEXTURE_SIZE, // GL_BGRA, GL_UNSIGNED_BYTE, noiseIntData); IDepthFrame* pDepthFrame = NULL; HRESULT hr; bool hasSucceeded = false; for (int tries = 0; tries < 20 && !hasSucceeded; tries++) { Sleep(100); hr = depth_frame_reader_->AcquireLatestFrame(&pDepthFrame); if (SUCCEEDED(hr)) hasSucceeded = true; } if (!hasSucceeded) { sprintf_s(errorString, MAX_ERROR_LENGTH, "Could not acquire last depth sensor frame"); return -1; } pDepthFrame->get_RelativeTime(&last_frame_time_); IFrameDescription* pFrameDescription = NULL; int nWidth = 0; int nHeight = 0; hr = pDepthFrame->get_FrameDescription(&pFrameDescription); if (FAILED(hr)) { pDepthFrame->Release(); sprintf_s(errorString, MAX_ERROR_LENGTH, "Could not get Depth Frame description"); return -1; } pFrameDescription->get_Width(&nWidth); pFrameDescription->get_Height(&nHeight); depth_sensor_width_ = nWidth; depth_sensor_height_ = nHeight; if (cpu_depth_sensor_buffer_) delete[] cpu_depth_sensor_buffer_; cpu_depth_sensor_buffer_ = new float[nWidth * nHeight]; memset(cpu_depth_sensor_buffer_, 0, nWidth * nHeight); if (smoothed_depth_sensor_buffer_[0]) { delete[] smoothed_depth_sensor_buffer_[0]; delete[] smoothed_depth_sensor_buffer_[1]; } smoothed_depth_sensor_buffer_[0] = new float[nWidth * nHeight]; smoothed_depth_sensor_buffer_[1] = new float[nWidth * nHeight]; memset(smoothed_depth_sensor_buffer_[0], 0, nWidth*nHeight*sizeof(smoothed_depth_sensor_buffer_[0][0])); memset(smoothed_depth_sensor_buffer_[1], 0, nWidth*nHeight*sizeof(smoothed_depth_sensor_buffer_[1][0])); glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, nWidth, nHeight, 0, GL_RED, GL_FLOAT, smoothed_depth_sensor_buffer_[0]); textureWidth[numTextures] = nWidth; textureHeight[numTextures] = nHeight; numTextures++; pFrameDescription->Release(); pDepthFrame->Release(); return 0; }
int main(int argc, char** argv) { int first_time = 0; Size screen_size(1440, 900);//the dst image size,e.g.100x100 Scalar text_color = Scalar(0, 255, 0); Scalar text_color2 = Scalar(0, 255, 255); Scalar text_color3 = Scalar(0, 0, 255); inhaler_coach coach; coach.control = 0; thread mThread(test_func, &coach); // 1a. Get Kinect Sensor cout << "Try to get default sensor" << endl; IKinectSensor* pSensor = nullptr; if (GetDefaultKinectSensor(&pSensor) != S_OK) { cerr << "Get Sensor failed" << endl; return -1; } // 1b. Open sensor cout << "Try to open sensor" << endl; if (pSensor->Open() != S_OK) { cerr << "Can't open sensor" << endl; return -1; } // 2. Color Related code IColorFrameReader* pColorFrameReader = nullptr; cv::Mat mColorImg; UINT uBufferSize = 0; UINT uColorPointNum = 0; int iWidth = 0; int iHeight = 0; { // 2a. Get color frame source cout << "Try to get color source" << endl; IColorFrameSource* pFrameSource = nullptr; if (pSensor->get_ColorFrameSource(&pFrameSource) != S_OK) { cerr << "Can't get color frame source" << endl; return -1; } // 2b. Get frame description cout << "get color frame description" << endl; IFrameDescription* pFrameDescription = nullptr; if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK) { pFrameDescription->get_Width(&iWidth); pFrameDescription->get_Height(&iHeight); } pFrameDescription->Release(); pFrameDescription = nullptr; // 2c. get frame reader cout << "Try to get color frame reader" << endl; if (pFrameSource->OpenReader(&pColorFrameReader) != S_OK) { cerr << "Can't get color frame reader" << endl; return -1; } // 2d. release Frame source cout << "Release frame source" << endl; pFrameSource->Release(); pFrameSource = nullptr; // Prepare OpenCV data mColorImg = cv::Mat(iHeight, iWidth, CV_8UC4); uBufferSize = iHeight * iWidth * 4 * sizeof(BYTE); uColorPointNum = iHeight * iWidth; } // 3. Depth related code IDepthFrameReader* pDepthFrameReader = nullptr; UINT uDepthPointNum = 0; int iDepthWidth = 0, iDepthHeight = 0; cout << "Try to get depth source" << endl; { // Get frame source IDepthFrameSource* pFrameSource = nullptr; if (pSensor->get_DepthFrameSource(&pFrameSource) != S_OK) { cerr << "Can't get depth frame source" << endl; return -1; } // Get frame description cout << "get depth frame description" << endl; IFrameDescription* pFrameDescription = nullptr; if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK) { pFrameDescription->get_Width(&iDepthWidth); pFrameDescription->get_Height(&iDepthHeight); uDepthPointNum = iDepthWidth * iDepthHeight; } pFrameDescription->Release(); pFrameDescription = nullptr; // get frame reader cout << "Try to get depth frame reader" << endl; if (pFrameSource->OpenReader(&pDepthFrameReader) != S_OK) { cerr << "Can't get depth frame reader" << endl; return -1; } // release Frame source cout << "Release frame source" << endl; pFrameSource->Release(); pFrameSource = nullptr; } // 4. Body related code IBodyFrameReader* pBodyFrameReader = nullptr; IBody** aBodyData = nullptr; INT32 iBodyCount = 0; { // 3a. Get frame source cout << "Try to get body source" << endl; IBodyFrameSource* pFrameSource = nullptr; if (pSensor->get_BodyFrameSource(&pFrameSource) != S_OK) { cerr << "Can't get body frame source" << endl; return -1; } // 3b. Get the number of body if (pFrameSource->get_BodyCount(&iBodyCount) != S_OK) { cerr << "Can't get body count" << endl; return -1; } cout << " > Can trace " << iBodyCount << " bodies" << endl; aBodyData = new IBody*[iBodyCount]; for (int i = 0; i < iBodyCount; ++i) aBodyData[i] = nullptr; // 3c. get frame reader cout << "Try to get body frame reader" << endl; if (pFrameSource->OpenReader(&pBodyFrameReader) != S_OK) { cerr << "Can't get body frame reader" << endl; return -1; } // 3d. release Frame source cout << "Release frame source" << endl; pFrameSource->Release(); pFrameSource = nullptr; } // 4. Body Index releated code IBodyIndexFrameReader* pBIFrameReader = nullptr; cout << "Try to get body index source" << endl; { // Get frame source IBodyIndexFrameSource* pFrameSource = nullptr; if (pSensor->get_BodyIndexFrameSource(&pFrameSource) != S_OK) { cerr << "Can't get body index frame source" << endl; return -1; } // get frame reader cout << "Try to get body index frame reader" << endl; if (pFrameSource->OpenReader(&pBIFrameReader) != S_OK) { cerr << "Can't get depth frame reader" << endl; return -1; } // release Frame source cout << "Release frame source" << endl; pFrameSource->Release(); pFrameSource = nullptr; } // 5. background cv::Mat imgBG(iHeight, iWidth, CV_8UC3); imgBG.setTo(0); // 4. get CoordinateMapper ICoordinateMapper* pCoordinateMapper = nullptr; if (pSensor->get_CoordinateMapper(&pCoordinateMapper) != S_OK) { cout << "Can't get coordinate mapper" << endl; return -1; } // Enter main loop UINT16* pDepthPoints = new UINT16[uDepthPointNum]; BYTE* pBodyIndex = new BYTE[uDepthPointNum]; DepthSpacePoint* pPointArray = new DepthSpacePoint[uColorPointNum]; cv::namedWindow("Inhaler Coach"); while (true) { // 4a. Get last frame IColorFrame* pColorFrame = nullptr; if (pColorFrameReader->AcquireLatestFrame(&pColorFrame) == S_OK) { pColorFrame->CopyConvertedFrameDataToArray(uBufferSize, mColorImg.data, ColorImageFormat_Bgra); pColorFrame->Release(); pColorFrame = nullptr; } cv::Mat mImg = mColorImg.clone(); // 8b. read depth frame IDepthFrame* pDepthFrame = nullptr; if (pDepthFrameReader->AcquireLatestFrame(&pDepthFrame) == S_OK) { pDepthFrame->CopyFrameDataToArray(uDepthPointNum, pDepthPoints); pDepthFrame->Release(); pDepthFrame = nullptr; } // 8c. read body index frame IBodyIndexFrame* pBIFrame = nullptr; if (pBIFrameReader->AcquireLatestFrame(&pBIFrame) == S_OK) { pBIFrame->CopyFrameDataToArray(uDepthPointNum, pBodyIndex); pBIFrame->Release(); pBIFrame = nullptr; } #ifdef COACH_DEBUG cv::Mat imgTarget = imgBG.clone(); // 9b. map color to depth if (pCoordinateMapper->MapColorFrameToDepthSpace(uDepthPointNum, pDepthPoints, uColorPointNum, pPointArray) == S_OK) { for (int y = 0; y < imgTarget.rows; ++y) { for (int x = 0; x < imgTarget.cols; ++x) { // ( x, y ) in color frame = rPoint in depth frame const DepthSpacePoint& rPoint = pPointArray[y * imgTarget.cols + x]; // check if rPoint is in range if (rPoint.X >= 0 && rPoint.X < iDepthWidth && rPoint.Y >= 0 && rPoint.Y < iDepthHeight) { // fill color from color frame if this pixel is user int iIdx = (int)rPoint.X + iDepthWidth * (int)rPoint.Y; if (pBodyIndex[iIdx] < 6) { cv::Vec4b& rPixel = mImg.at<cv::Vec4b>(y, x); imgTarget.at<cv::Vec3b>(y, x) = cv::Vec3b(rPixel[0], rPixel[1], rPixel[2]); } } } } } #else cv::Mat imgTarget = mImg.clone(); #endif // 4b. Get body data IBodyFrame* pBodyFrame = nullptr; if (pBodyFrameReader->AcquireLatestFrame(&pBodyFrame) == S_OK) { // 4b. get Body data if (pBodyFrame->GetAndRefreshBodyData(iBodyCount, aBodyData) == S_OK) { // 4c. for each body for (int i = 0; i < iBodyCount; ++i) { IBody* pBody = aBodyData[i]; // check if is tracked BOOLEAN bTracked = false; if ((pBody->get_IsTracked(&bTracked) == S_OK) && bTracked) { // get joint position Joint aJoints[JointType::JointType_Count]; if (pBody->GetJoints(JointType::JointType_Count, aJoints) == S_OK) { if (coach.state == 0){ coach.state = 1; if (first_time == 0){ first_time = 1; PlaySound(TEXT("welcome.wav"), NULL, SND_FILENAME); } } #ifdef COACH_DEBUG DrawLine(imgTarget, aJoints[JointType_SpineBase], aJoints[JointType_SpineMid], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_SpineMid], aJoints[JointType_SpineShoulder], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_SpineShoulder], aJoints[JointType_Neck], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_Neck], aJoints[JointType_Head], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderLeft], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_ShoulderLeft], aJoints[JointType_ElbowLeft], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_ElbowLeft], aJoints[JointType_WristLeft], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_WristLeft], aJoints[JointType_HandLeft], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_HandLeft], aJoints[JointType_HandTipLeft], pCoordinateMapper); //DrawLine(imgTarget, aJoints[JointType_HandLeft], aJoints[JointType_ThumbLeft], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderRight], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_ShoulderRight], aJoints[JointType_ElbowRight], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_ElbowRight], aJoints[JointType_WristRight], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_WristRight], aJoints[JointType_HandRight], pCoordinateMapper); DrawLine(imgTarget, aJoints[JointType_HandRight], aJoints[JointType_HandTipRight], pCoordinateMapper); //DrawLine(imgTarget, aJoints[JointType_HandRight], aJoints[JointType_ThumbRight], pCoordinateMapper); #endif ColorSpacePoint q; ColorSpacePoint head; //ColorSpacePoint w; pCoordinateMapper->MapCameraPointToColorSpace(aJoints[JointType_Head].Position, &head); // check shaking coach.shaking_detection(aJoints, pCoordinateMapper); q = coach.position_checking(aJoints, pCoordinateMapper); #ifdef COACH_DEBUG circle(imgTarget, cv::Point(q.X, q.Y), 10, Scalar(0, 255, 255), 10, 8, 0); //circle(imgTarget, cv::Point(q.X, q.Y), 10, Scalar(0, 255, 255), 10, 8, 0); rectangle(imgTarget, Point(head.X - 50, head.Y - 40), Point(head.X + 50, head.Y + 90), Scalar(0, 255, 255), 1, 8, 0); //circle(imgTarget, cv::Point(w.X, w.Y), 10, Scalar(255, 0, 255), 10, 8, 0); #endif coach.state_change_rule(); } } } } else { cerr << "Can't read body data" << endl; } // 4e. release frame pBodyFrame->Release(); } switch (coach.state){ case 0: putText(imgTarget, "CMU Inhaler Coaching System", Point(120, 120), FONT_HERSHEY_DUPLEX, 2, text_color); break; case 1: putText(imgTarget, "Please shake the inhaler", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break; case 2: putText(imgTarget, "Shaking detected", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break; case 3: putText(imgTarget, "Please put the inhaler in front of your mouth", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break; case 4: putText(imgTarget, "Position check OK", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break; case 5: putText(imgTarget, "You forget to shake the inhaler first!!!", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color3); break; } // show image Mat dst; resize(imgTarget, dst, screen_size); imshow("Coach", dst); // 4c. check keyboard input if (cv::waitKey(30) == VK_ESCAPE){ break; } } mThread.join(); // 3. delete body data array delete[] aBodyData; // 3. release frame reader cout << "Release body frame reader" << endl; pBodyFrameReader->Release(); pBodyFrameReader = nullptr; // 2. release color frame reader cout << "Release color frame reader" << endl; pColorFrameReader->Release(); pColorFrameReader = nullptr; // 1c. Close Sensor cout << "close sensor" << endl; pSensor->Close(); // 1d. Release Sensor cout << "Release sensor" << endl; pSensor->Release(); pSensor = nullptr; return 0; }
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; } }