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 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;
}
Exemplo n.º 3
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;
}
Exemplo n.º 4
0
int main(int argc, char** argv) {
	// 1a. Get default Sensor
	std::cout << "Try to get default sensor" << std::endl;
	IKinectSensor* pSensor = nullptr;
	if (GetDefaultKinectSensor(&pSensor) != S_OK) {
		cerr << "Get Sensor failed" << std::endl;
		return -1;
	}

	// 1b. Open sensor
	std::cout << "Try to open sensor" << std::endl;
	if (pSensor->Open() != S_OK) {
		cerr << "Can't open sensor" << std::endl;
		return -1;
	}

	// 2. Color Related code
	IColorFrameReader* pColorFrameReader = nullptr;
	cv::Mat	mColorImg;
	UINT uBufferSize = 0;
	{
		// 2a. Get color frame source
		std::cout << "Try to get color source" << std::endl;
		IColorFrameSource* pFrameSource = nullptr;
		if (pSensor->get_ColorFrameSource(&pFrameSource) != S_OK) {
			cerr << "Can't get color frame source" << std::endl;
			return -1;
		}

		// 2b. Get frame description
		std::cout << "get color frame description" << std::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;

		// 2c. get frame reader
		std::cout << "Try to get color frame reader" << std::endl;
		if (pFrameSource->OpenReader(&pColorFrameReader) != S_OK) {
			cerr << "Can't get color frame reader" << std::endl;
			return -1;
		}

		// 2d. release Frame source
		std::cout << "Release frame source" << std::endl;
		pFrameSource->Release();
		pFrameSource = nullptr;

		// Prepare OpenCV data
		mColorImg = cv::Mat(iHeight, iWidth, CV_8UC4);
		uBufferSize = iHeight * iWidth * 4 * sizeof(BYTE);
	}

	// 3. Body related code
	IBodyFrameReader* pBodyFrameReader = nullptr;
	IBody** aBodyData = nullptr;
	INT32 iBodyCount = 0;
	{
		// 3a. Get frame source
		std::cout << "Try to get body source" << std::endl;
		IBodyFrameSource* pFrameSource = nullptr;
		if (pSensor->get_BodyFrameSource(&pFrameSource) != S_OK) {
			cerr << "Can't get body frame source" << std::endl;
			return -1;
		}

		// 3b. Get the number of body
		if (pFrameSource->get_BodyCount(&iBodyCount) != S_OK) {
			cerr << "Can't get body count" << std::endl;
			return -1;
		}
		std::cout << " > Can trace " << iBodyCount << " bodies" << std::endl;
		aBodyData = new IBody*[iBodyCount];
		for (int i = 0; i < iBodyCount; ++i)
			aBodyData[i] = nullptr;

		// 3c. get frame reader
		std::cout << "Try to get body frame reader" << std::endl;
		if (pFrameSource->OpenReader(&pBodyFrameReader) != S_OK) {
			cerr << "Can't get body frame reader" << std::endl;
			return -1;
		}

		// 3d. release Frame source
		std::cout << "Release frame source" << std::endl;
		pFrameSource->Release();
		pFrameSource = nullptr;
	}

	// 4. get CoordinateMapper
	ICoordinateMapper* pCoordinateMapper = nullptr;
	if (pSensor->get_CoordinateMapper(&pCoordinateMapper) != S_OK) {
		std::cout << "Can't get coordinate mapper" << std::endl;
		return -1;
	}

	// Enter main loop
	cv::namedWindow("Body Image");

	// Debug:output the velocity of joints
	ofstream current_average_velocityTXT("current_average_velocity.txt");
	ofstream average_velocityTXT("average_velocity.txt");
	int frame_count = 0;
	int frame_count_for_standby = 0;
	float positionX0[25] = {0};
	float positionX1[25] = {0};
	float positionY0[25] = { 0 };
	float positionY1[25] = { 0 };
	float positionZ0[25] = { 0 };
	float positionZ1[25] = { 0 };

	float velocityX[25] = { 0 };
	float velocityY[25] = { 0 };
	float velocityZ[25] = { 0 };
	float current_velocity[25] = { 0 };
	float velocityee[8] = { 0 };
	float current_total_velocity = 0;
	float current_average_velocity = 0;
	float total_velocity = 0;
	float average_velocity = 0;

	while (true)
	{
		// 4a. Get last frame
		IColorFrame* pColorFrame = nullptr;
		if (pColorFrameReader->AcquireLatestFrame(&pColorFrame) == S_OK)	 {
			// 4c. Copy to OpenCV image
			if (pColorFrame->CopyConvertedFrameDataToArray(uBufferSize, mColorImg.data, ColorImageFormat_Bgra) != S_OK)	{
				cerr << "Data copy error" << endl;
			}
			// 4e. release frame
			pColorFrame->Release();
		}
		cv::Mat mImg = mColorImg.clone();
		// 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) {
							DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_SpineMid], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_SpineMid], aJoints[JointType_SpineShoulder], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_Neck], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_Neck], aJoints[JointType_Head], pCoordinateMapper);

							DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_ShoulderLeft], aJoints[JointType_ElbowLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_ElbowLeft], aJoints[JointType_WristLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_WristLeft], aJoints[JointType_HandLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_HandTipLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_ThumbLeft], pCoordinateMapper);

							DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_ShoulderRight], aJoints[JointType_ElbowRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_ElbowRight], aJoints[JointType_WristRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_WristRight], aJoints[JointType_HandRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_HandTipRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_ThumbRight], pCoordinateMapper);

							DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_HipLeft], aJoints[JointType_KneeLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_KneeLeft], aJoints[JointType_AnkleLeft], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_AnkleLeft], aJoints[JointType_FootLeft], pCoordinateMapper);

							DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_HipRight], aJoints[JointType_KneeRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_KneeRight], aJoints[JointType_AnkleRight], pCoordinateMapper);
							DrawLine(mImg, aJoints[JointType_AnkleRight], aJoints[JointType_FootRight], pCoordinateMapper);
						}
						// Debug:print out the number of frame					
						std::cout << "frame " << ++frame_count << std::endl;
					
						for (int j = 1; j < 8; j++) {
							velocityee[j] = velocityee[j-1];
							total_velocity += velocityee[j];
						}
						average_velocity = total_velocity / 8.0;
						
						if (average_velocity <= 0.0015) {	
							// determine if the person is still 
							if (frame_count_for_standby == 0) {
								PlaySound(TEXT("Alarm02.wav"), NULL, SND_FILENAME);
								std::cout << "Start capturing points!" << std::endl;
							}
							// count the number of frame whose velocity is below the threshold
							frame_count_for_standby++;
							if (frame_count_for_standby >= 5) {
								frame_count_for_standby = 0;
							}
						} 
						// Debug:output the average velocity 
						average_velocityTXT << frame_count << " " << average_velocity << std::endl;
						total_velocity = 0;
						// Update the average velocity
						int available_joints = 0;
						for (int i = 0; i < 25; i++) {
							// X 
							positionX1[i] = positionX0[i];
							positionX0[i] = aJoints[i].Position.X;
							velocityX[i] = (positionX1[i] - positionX0[i]) * (positionX1[i] - positionX0[i]);
							// Y
							positionY1[i] = positionY0[i];
							positionY0[i] = aJoints[i].Position.Y;
							velocityY[i] = (positionY1[i] - positionY0[i]) * (positionY1[i] - positionY0[i]);
							// Z
							positionZ1[i] = positionZ0[i];
							positionZ0[i] = aJoints[i].Position.Z;
							velocityZ[i] = (positionZ1[i] - positionZ0[i]) * (positionZ1[i] - positionZ0[i]);
							current_velocity[i] = sqrtf(velocityX[i] + velocityY[i] + velocityZ[i]);
							// exclude the discrete velocity
							if (current_velocity[i] < 0.01) {
								current_total_velocity += current_velocity[i];
								available_joints++;
							}
						}
						// If no joint is available, save the velocity of last frame
						if (available_joints != 0) {
							current_average_velocity = current_total_velocity / available_joints;
						}
						velocityee[0] = current_average_velocity;
						// Debug:output the current average velocity 
						current_average_velocityTXT << frame_count << " " << current_average_velocity << std::endl;
											
						current_total_velocity = 0;					
					}
				}
			} else {
				cerr << "Can't read body data" << endl;
			}
			// 4e. release frame
			pBodyFrame->Release();
		}
		// show image
		cv::imshow("Body Image",mImg);
		// 4c. check keyboard input
		if (cv::waitKey(30) == VK_ESCAPE) {
			break;
		}
	}
	// 3. delete body data array
	delete[] aBodyData;
	// 3. release frame reader
	std::cout << "Release body frame reader" << std::endl;
	pBodyFrameReader->Release();
	pBodyFrameReader = nullptr;
	// 2. release color frame reader
	std::cout << "Release color frame reader" << std::endl;
	pColorFrameReader->Release();
	pColorFrameReader = nullptr;
	// 1c. Close Sensor
	std::cout << "close sensor" << std::endl;
	pSensor->Close();
	// 1d. Release Sensor
	std::cout << "Release sensor" << std::endl;
	pSensor->Release();
	pSensor = nullptr;

	return 0;
}
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 Infrared source" << endl;
	IInfraredFrameSource* pFrameSource = nullptr;
	if (pSensor->get_InfraredFrameSource(&pFrameSource) != S_OK)
	{
		cerr << "Can't get Infrared frame source" << endl;
		return -1;
	}

	// 2b. Get frame description
	cout << "get Infrared 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 Infrared frame reader" << endl;
	IInfraredFrameReader* pFrameReader = nullptr;
	if (pFrameSource->OpenReader(&pFrameReader) != S_OK)
	{
		cerr << "Can't get Infrared frame reader" << endl;
		return -1;
	}

	// 2c. release Frame source
	cout << "Release frame source" << endl;
	pFrameSource->Release();
	pFrameSource = nullptr;

	// create OpenCV window
	cv::namedWindow( "Infrared Image" );

	// Enter main loop
	while (true)
	{
		// 4a. Get last frame
		IInfraredFrame* pFrame = nullptr;
		if (pFrameReader->AcquireLatestFrame(&pFrame) == S_OK)
		{
			// 4c. Copy to OpenCV image
			UINT	uSize = 0;
			UINT16*	pBuffer = nullptr;
			if (pFrame->AccessUnderlyingBuffer(&uSize, &pBuffer) == S_OK)
			{
				cv::Mat mIRImg(iHeight, iWidth, CV_16UC1, pBuffer);
				cv::imshow("Infrared Image", mIRImg);
			}
			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;
}