HRESULT BodyIndexMulticaster::InitializeMulticasterAndSubscribeHandle(IKinectSensor* kinectSensor, WAITABLE_HANDLE &waitableHandle) { VideoMulticastingPipelineCreationParameters input; input.multicastIP = multicastIP; input.port = port; input.useHighestQuality = true; input.inputHeight = K4W2_BODY_INDEX_HEIGHT; input.inputWidth = K4W2_BODY_INDEX_WIDTH; input.multicastHeight = BODY_INDEX_MULTICAST_HEIGHT; input.multicastWidth = BODY_INDEX_MULTICAST_WIDTH; input.pixelFormat = BODY_INDEX_PIXEL_FORMAT; gstSender.Initialize(CreateAppSrc_RGB8_EncodedMulticastingPipeline(input)); gstSender.SetPipelineState(GST_STATE_PLAYING); HRESULT hr; IBodyIndexFrameSource* bodyIndexFrameSource = NULL; if (kinectSensor) { hr = kinectSensor->get_BodyIndexFrameSource(&bodyIndexFrameSource); if (SUCCEEDED(hr)) { hr = bodyIndexFrameSource->OpenReader(&bodyIndexFrameReader); } } SafeRelease(bodyIndexFrameSource); bodyIndexFrameReader->SubscribeFrameArrived(&waitableHandle); return hr; }
void KinectDevice::getFrameInformation() { // Get the frame information IDepthFrameSource *depthSrc; IColorFrameSource *colorSrc; IInfraredFrameSource *irSrc; ILongExposureInfraredFrameSource *hdirSrc; IBodyIndexFrameSource *indexSrc; IFrameDescription *depthDesc, *colorDesc, *irDesc, *hdirDesc, *indexDesc; if (_streams & Streams::DEPTH_STREAM) { _sensor->get_DepthFrameSource(&depthSrc); depthSrc->get_FrameDescription(&depthDesc); depthFrameInfo = FrameInfo(depthDesc); // Min/max vals depthSrc->get_DepthMinReliableDistance(&depthFrameInfo.minVal); depthSrc->get_DepthMaxReliableDistance(&depthFrameInfo.maxVal); // Allocate depthData = std::shared_ptr<uint16_t>(new uint16_t[depthFrameInfo.frameSize]); prevDepthData = std::shared_ptr<uint16_t>(new uint16_t[depthFrameInfo.frameSize]); } else { depthData = nullptr; prevDepthData = nullptr; } if (_streams & Streams::COLOR_STREAM) { _sensor->get_ColorFrameSource(&colorSrc); colorSrc->get_FrameDescription(&colorDesc); colorFrameInfo = FrameInfo(colorDesc); colorData = std::shared_ptr<uint16_t>(new uint16_t[colorFrameInfo.frameSize]); } if (_streams & Streams::IR_STREAM) { _sensor->get_InfraredFrameSource(&irSrc); irSrc->get_FrameDescription(&irDesc); irFrameInfo = FrameInfo(irDesc); irData = std::shared_ptr<uint16_t>(new uint16_t[irFrameInfo.frameSize]); } if (_streams & Streams::HDIR_STREAM) { _sensor->get_LongExposureInfraredFrameSource(&hdirSrc); hdirSrc->get_FrameDescription(&hdirDesc); hdirFrameInfo = FrameInfo(hdirDesc); hdirData = std::shared_ptr<uint16_t>(new uint16_t[hdirFrameInfo.frameSize]); } if (_streams & Streams::INDEX_STREAM) { _sensor->get_BodyIndexFrameSource(&indexSrc); indexSrc->get_FrameDescription(&indexDesc); indexFrameInfo = FrameInfo(indexDesc); indexData = std::shared_ptr<BYTE>(new BYTE[indexFrameInfo.frameSize]); } }
//---------- void BodyIndex::initReader(IKinectSensor * sensor) { this->reader = NULL; try { IBodyIndexFrameSource * source = NULL; if (FAILED(sensor->get_BodyIndexFrameSource(&source))) { throw(Exception("Failed to initialise BodyIndex source")); } if (FAILED(source->OpenReader(&this->reader))) { throw(Exception("Failed to initialise BodyIndex reader")); } SafeRelease(source); } catch (std::exception & e) { SafeRelease(this->reader); throw (e); } }
bool BodyIndexStream::open() { if (!m_Device->isOpen()) { ofLogWarning("ofxKinect2::BodyIndexStream") << "No ready Kinect2 found."; return false; } m_IsInvert = true; IBodyIndexFrameSource *frameSource = nullptr; HRESULT hr = E_FAIL; hr = m_Device->get().kinect2->get_BodyIndexFrameSource(&frameSource); if (SUCCEEDED(hr)) { hr = frameSource->OpenReader(&m_StreamHandle.bodyIndexFrameReader); if (SUCCEEDED(hr)) { IFrameDescription *frameDescription = nullptr; frameSource->get_FrameDescription(&frameDescription); if (SUCCEEDED(hr)) { int resX, resY = 0; hr = frameDescription->get_Width(&resX); hr = frameDescription->get_Width(&resY); m_Frame.mode.resolutionX = resX; m_Frame.mode.resolutionY = resY; m_Frame.width = resX; m_Frame.height = resY; m_DoubleBuffer.allocate(resX, resY, 4); } safeRelease(frameDescription); } } safeRelease(frameSource); if (FAILED(hr)) { ofLogWarning("ofxKinect2::BodyIndexStream") << "Can't open stream."; return false; } return Stream::open(); }
HRESULT CBodyIndexCV::initBodyIndexReader() { HRESULT hr; IBodyIndexFrameSource* pBodyIndexFrameSource = NULL; hr = m_pKinectSensor->get_BodyIndexFrameSource(&pBodyIndexFrameSource); if (SUCCEEDED(hr)) { hr = pBodyIndexFrameSource->OpenReader(&m_pBodyIndexFrameReader); } SafeRelease(pBodyIndexFrameSource); if (!m_pKinectSensor || SUCCEEDED(hr)) { return E_FAIL; } return hr; }
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; return -1; } // 1b. Open sensor cout << "Try to open sensor" << endl; if (pSensor->Open() != S_OK) { cerr << "Can't open sensor" << endl; return -1; } // 2a. Get frame source cout << "Try to get body index source" << endl; IBodyIndexFrameSource* pFrameSource = nullptr; if (pSensor->get_BodyIndexFrameSource(&pFrameSource) != S_OK) { cerr << "Can't get body index frame source" << endl; return -1; } // 2b. Get frame description cout << "get body index frame description" << endl; 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; // 3a. get frame reader cout << "Try to get body index frame reader" << endl; IBodyIndexFrameReader* pFrameReader = nullptr; if (pFrameSource->OpenReader(&pFrameReader) != S_OK) { cerr << "Can't get body index frame reader" << endl; return -1; } // 2c. release Frame source cout << "Release frame source" << endl; pFrameSource->Release(); pFrameSource = nullptr; // Prepare OpenCV data cv::Mat mImg(iHeight, iWidth, CV_8UC3); cv::namedWindow("Body Index Image"); // color array cv::Vec3b aColorTable[7] = { cv::Vec3b(255,0,0), cv::Vec3b(0,255,0), cv::Vec3b(0,0,255), cv::Vec3b(255,255,0), cv::Vec3b(255,0,255), cv::Vec3b(0,255,255), cv::Vec3b(0,0,0), }; // Enter main loop while (true) { // 4a. Get last frame IBodyIndexFrame* pFrame = nullptr; if (pFrameReader->AcquireLatestFrame(&pFrame) == S_OK) { // 4c. Fill OpenCV image UINT uSize = 0; BYTE* pBuffer = nullptr; if (pFrame->AccessUnderlyingBuffer(&uSize,&pBuffer) == S_OK) { for (int y = 0; y < iHeight; ++y) { for (int x = 0; x < iWidth; ++x) { int uBodyIdx = pBuffer[x + y * iWidth]; if (uBodyIdx < 6) mImg.at<cv::Vec3b>(y, x) = aColorTable[uBodyIdx]; else mImg.at<cv::Vec3b>(y, x) = aColorTable[6]; } } cv::imshow("Body Index Image", mImg); } else { cerr << "Data access 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; // 1c. Close Sensor cout << "close sensor" << endl; pSensor->Close(); // 1d. Release Sensor cout << "Release sensor" << endl; pSensor->Release(); pSensor = nullptr; return 0; }
int _tmain( int argc, _TCHAR* argv[] ) { cv::setUseOptimized( true ); // Sensor IKinectSensor* pSensor; HRESULT hResult = S_OK; hResult = GetDefaultKinectSensor( &pSensor ); if( FAILED( hResult ) ){ std::cerr << "Error : GetDefaultKinectSensor" << std::endl; return -1; } hResult = pSensor->Open(); if( FAILED( hResult ) ){ std::cerr << "Error : IKinectSensor::Open()" << std::endl; return -1; } // Source IBodyIndexFrameSource* pBodyIndexSource; hResult = pSensor->get_BodyIndexFrameSource( &pBodyIndexSource ); if( FAILED( hResult ) ){ std::cerr << "Error : IKinectSensor::get_BodyIndexFrameSource()" << std::endl; return -1; } // Reader IBodyIndexFrameReader* pBodyIndexReader; hResult = pBodyIndexSource->OpenReader( &pBodyIndexReader ); if( FAILED( hResult ) ){ std::cerr << "Error : IBodyIndexFrameSource::OpenReader()" << std::endl; return -1; } // Description IFrameDescription* pDescription; hResult = pBodyIndexSource->get_FrameDescription( &pDescription ); if( FAILED( hResult ) ){ std::cerr << "Error : IBodyIndexFrameSource::get_FrameDescription()" << std::endl; return -1; } int width = 0; int height = 0; pDescription->get_Width( &width ); // 512 pDescription->get_Height( &height ); // 424 cv::Mat bodyIndexMat( height, width, CV_8UC3 ); cv::namedWindow( "BodyIndex" ); // Color Table cv::Vec3b color[BODY_COUNT]; color[0] = cv::Vec3b( 255, 0, 0 ); color[1] = cv::Vec3b( 0, 255, 0 ); color[2] = cv::Vec3b( 0, 0, 255 ); color[3] = cv::Vec3b( 255, 255, 0 ); color[4] = cv::Vec3b( 255, 0, 255 ); color[5] = cv::Vec3b( 0, 255, 255 ); while( 1 ){ // Frame IBodyIndexFrame* pBodyIndexFrame = nullptr; hResult = pBodyIndexReader->AcquireLatestFrame( &pBodyIndexFrame ); if( SUCCEEDED( hResult ) ){ unsigned int bufferSize = 0; unsigned char* buffer = nullptr; hResult = pBodyIndexFrame->AccessUnderlyingBuffer( &bufferSize, &buffer ); if( SUCCEEDED( hResult ) ){ for( int y = 0; y < height; y++ ){ for( int x = 0; x < width; x++ ){ unsigned int index = y * width + x; if( buffer[index] != 0xff ){ bodyIndexMat.at<cv::Vec3b>( y, x ) = color[buffer[index]]; } else{ bodyIndexMat.at<cv::Vec3b>( y, x ) = cv::Vec3b( 0, 0, 0 ); } } } } } SafeRelease( pBodyIndexFrame ); cv::imshow( "BodyIndex", bodyIndexMat ); if( cv::waitKey( 30 ) == VK_ESCAPE ){ break; } } SafeRelease( pBodyIndexSource ); SafeRelease( pBodyIndexReader ); SafeRelease( pDescription ); if( pSensor ){ pSensor->Close(); } SafeRelease( pSensor ); cv::destroyAllWindows(); return 0; }
/// Initializes the default Kinect sensor HRESULT CBodyBasics::InitializeDefaultSensor() { //用于判断每次读取操作的成功与否 HRESULT hr; //搜索kinect hr = GetDefaultKinectSensor(&m_pKinectSensor); if (FAILED(hr)){ return hr; } //找到kinect设备 if (m_pKinectSensor) { // Initialize the Kinect and get coordinate mapper and the body reader IBodyFrameSource* pBodyFrameSource = NULL;//读取骨架 IDepthFrameSource* pDepthFrameSource = NULL;//读取深度信息 IBodyIndexFrameSource* pBodyIndexFrameSource = NULL;//读取背景二值图 //打开kinect hr = m_pKinectSensor->Open(); //coordinatemapper if (SUCCEEDED(hr)) { hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper); } //bodyframe if (SUCCEEDED(hr)) { hr = m_pKinectSensor->get_BodyFrameSource(&pBodyFrameSource); } if (SUCCEEDED(hr)) { hr = pBodyFrameSource->OpenReader(&m_pBodyFrameReader); } //depth frame if (SUCCEEDED(hr)){ hr = m_pKinectSensor->get_DepthFrameSource(&pDepthFrameSource); } if (SUCCEEDED(hr)){ hr = pDepthFrameSource->OpenReader(&m_pDepthFrameReader); } //body index frame if (SUCCEEDED(hr)){ hr = m_pKinectSensor->get_BodyIndexFrameSource(&pBodyIndexFrameSource); } if (SUCCEEDED(hr)){ hr = pBodyIndexFrameSource->OpenReader(&m_pBodyIndexFrameReader); } SafeRelease(pBodyFrameSource); SafeRelease(pDepthFrameSource); SafeRelease(pBodyIndexFrameSource); } if (!m_pKinectSensor || FAILED(hr)) { std::cout << "Kinect initialization failed!" << std::endl; return E_FAIL; } //skeletonImg,用于画骨架、背景二值图的MAT skeletonImg.create(cDepthHeight, cDepthWidth, CV_8UC3); skeletonImg.setTo(0); //depthImg,用于画深度信息的MAT depthImg.create(cDepthHeight, cDepthWidth, CV_8UC1); depthImg.setTo(0); return hr; }
HRESULT InitializeDefaultSensor() { HRESULT hr; hr = GetDefaultKinectSensor(&m_pKinectSensor); if (FAILED(hr)) { return hr; } if (m_pKinectSensor) { // Initialize the Kinect and get coordinate mapper and the body reader IBodyFrameSource* pBodyFrameSource = NULL; hr = m_pKinectSensor->Open(); if (SUCCEEDED(hr)) { hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper); } if (SUCCEEDED(hr)) { hr = m_pKinectSensor->get_BodyFrameSource(&pBodyFrameSource); } if (SUCCEEDED(hr)) { hr = pBodyFrameSource->OpenReader(&m_pBodyFrameReader); } SafeRelease(pBodyFrameSource); // get the color reader IColorFrameSource* pColorFrameSource = NULL; if (SUCCEEDED(hr)) { hr = m_pKinectSensor->get_ColorFrameSource(&pColorFrameSource); } if (SUCCEEDED(hr)) { hr = pColorFrameSource->OpenReader(&m_pColorFrameReader); } SafeRelease(pColorFrameSource); // get the depth reader IDepthFrameSource* pDepthFrameSource = NULL; if (SUCCEEDED(hr)) { hr = m_pKinectSensor->get_DepthFrameSource(&pDepthFrameSource); } if (SUCCEEDED(hr)) { hr = pDepthFrameSource->OpenReader(&m_pDepthFrameReader); } SafeRelease(pDepthFrameSource); //get the body frame index reader IBodyIndexFrameSource * pBodyIndexFrameSource = NULL; if(SUCCEEDED(hr)){ hr = m_pKinectSensor->get_BodyIndexFrameSource(&pBodyIndexFrameSource); } if(SUCCEEDED(hr)){ hr = pBodyIndexFrameSource->OpenReader(&m_pBodyIndexFrameReader); } SafeRelease(pBodyIndexFrameSource); } if (!m_pKinectSensor || FAILED(hr)) { std::cout << "no ready Kinect found!"; return E_FAIL; } return hr; }
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; }