bool KinectInterface::start_device() {
HRESULT hr;
kinectSensor = nullptr;
hr = GetDefaultKinectSensor(&kinectSensor);
// initialize Kinect Sensor
if (FAILED(hr) || !kinectSensor) {
std::cout << "ERROR hr=" << hr << "; sensor=" << kinectSensor << std::endl;
return false;
}
CHECKERROR(kinectSensor->Open())
kinectSensor->get_CoordinateMapper(&mapper);
// initialize depth frame reader
//CHECKERROR(kinectSensor->OpenMultiSourceFrameReader(
// FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
// &reader));
IDepthFrameSource* depthFrameSource = nullptr;
CHECKERROR(kinectSensor->get_DepthFrameSource(&depthFrameSource));
CHECKERROR(depthFrameSource->OpenReader(&depthFrameReader));
if (depthFrameSource) depthFrameSource->Release();
IColorFrameSource* colorFrameSource = nullptr;
CHECKERROR(kinectSensor->get_ColorFrameSource(&colorFrameSource));
CHECKERROR(colorFrameSource->OpenReader(&colorFrameReader));
if (depthFrameSource) colorFrameSource->Release();
return true;
}
// コンストラクタ
KinectV2::KinectV2()
{
// センサを取得する
if (sensor == NULL && GetDefaultKinectSensor(&sensor) == S_OK)
{
HRESULT hr;
// センサの使用を開始する
hr = sensor->Open();
assert(hr == S_OK);
// デプスデータの読み込み設定
IDepthFrameSource *depthSource;
hr = sensor->get_DepthFrameSource(&depthSource);
assert(hr == S_OK);
hr = depthSource->OpenReader(&depthReader);
assert(hr == S_OK);
IFrameDescription *depthDescription;
hr = depthSource->get_FrameDescription(&depthDescription);
assert(hr == S_OK);
depthSource->Release();
// デプスデータのサイズを得る
depthDescription->get_Width(&depthWidth);
depthDescription->get_Height(&depthHeight);
depthDescription->Release();
// カラーデータの読み込み設定
IColorFrameSource *colorSource;
hr = sensor->get_ColorFrameSource(&colorSource);
assert(hr == S_OK);
hr = colorSource->OpenReader(&colorReader);
assert(hr == S_OK);
IFrameDescription *colorDescription;
hr = colorSource->get_FrameDescription(&colorDescription);
assert(hr == S_OK);
colorSource->Release();
// カラーデータのサイズを得る
colorDescription->get_Width(&colorWidth);
colorDescription->get_Height(&colorHeight);
colorDescription->Release();
// 座標のマッピング
hr = sensor->get_CoordinateMapper(&coordinateMapper);
assert(hr == S_OK);
// depthCount と colorCount を計算してテクスチャとバッファオブジェクトを作成する
makeTexture();
// デプスデータからカメラ座標を求めるときに用いる一時メモリを確保する
position = new GLfloat[depthCount][3];
// カラーデータを変換する用いる一時メモリを確保する
color = new GLubyte[colorCount * 4];
}
}
IFrameDescription* Kinect2StreamImpl::getFrameDescription(OniSensorType sensorType)
{
if (!m_pKinectSensor) {
return NULL;
}
IFrameDescription* frameDescription = NULL;
if (sensorType == ONI_SENSOR_COLOR) {
IColorFrameSource* frameSource = NULL;
HRESULT hr = m_pKinectSensor->get_ColorFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->get_FrameDescription(&frameDescription);
if (FAILED(hr) && frameDescription) {
frameDescription->Release();
frameDescription = NULL;
}
}
if (frameSource) {
frameSource->Release();
}
}
else if (sensorType == ONI_SENSOR_DEPTH) {
IDepthFrameSource* frameSource = NULL;
HRESULT hr = m_pKinectSensor->get_DepthFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->get_FrameDescription(&frameDescription);
if (FAILED(hr) && frameDescription) {
frameDescription->Release();
frameDescription = NULL;
}
}
if (frameSource) {
frameSource->Release();
}
}
else { // ONI_SENSOR_IR
IInfraredFrameSource* frameSource = NULL;
HRESULT hr = m_pKinectSensor->get_InfraredFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->get_FrameDescription(&frameDescription);
if (FAILED(hr) && frameDescription) {
frameDescription->Release();
frameDescription = NULL;
}
}
if (frameSource) {
frameSource->Release();
}
}
return frameDescription;
}
void Kinect2StreamImpl::openFrameReader()
{
if (!m_pKinectSensor) {
return;
}
if (m_sensorType == ONI_SENSOR_COLOR && !m_pFrameReader.color) {
IColorFrameSource* frameSource = NULL;
HRESULT hr = m_pKinectSensor->get_ColorFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->OpenReader(&m_pFrameReader.color);
if (FAILED(hr) && m_pFrameReader.color) {
m_pFrameReader.color->Release();
m_pFrameReader.color = NULL;
}
}
if (frameSource) {
frameSource->Release();
}
}
else if (m_sensorType == ONI_SENSOR_DEPTH && !m_pFrameReader.depth) {
IDepthFrameSource* frameSource = NULL;
HRESULT hr = m_pKinectSensor->get_DepthFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->OpenReader(&m_pFrameReader.depth);
if (FAILED(hr) && m_pFrameReader.depth) {
m_pFrameReader.depth->Release();
m_pFrameReader.depth = NULL;
}
}
if (frameSource) {
frameSource->Release();
}
}
else if(!m_pFrameReader.infrared) { // ONI_SENSOR_IR
IInfraredFrameSource* frameSource = NULL;
HRESULT hr = m_pKinectSensor->get_InfraredFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->OpenReader(&m_pFrameReader.infrared);
if (FAILED(hr) && m_pFrameReader.infrared) {
m_pFrameReader.infrared->Release();
m_pFrameReader.infrared = NULL;
}
}
if (frameSource) {
frameSource->Release();
}
}
}
int TextureManager::initializeDefaultSensor(char *errorMessage)
{
HRESULT hr;
sprintf_s(errorMessage, MAX_ERROR_LENGTH, "");
hr = GetDefaultKinectSensor(&kinect_sensor_);
if (FAILED(hr))
{
sprintf_s(errorMessage, MAX_ERROR_LENGTH, "Could not initialize Kinect");
kinect_sensor_ = NULL;
return -1;
}
if (kinect_sensor_)
{
// Initialize the Kinect and get the depth reader
IDepthFrameSource* depthFrameSource = NULL;
hr = kinect_sensor_->Open();
if (SUCCEEDED(hr))
{
hr = kinect_sensor_->get_DepthFrameSource(&depthFrameSource);
}
if (SUCCEEDED(hr))
{
hr = depthFrameSource->OpenReader(&depth_frame_reader_);
}
if (depthFrameSource)
{
depthFrameSource->Release();
depthFrameSource = NULL;
}
}
if (!kinect_sensor_ || FAILED(hr))
{
sprintf_s(errorMessage, MAX_ERROR_LENGTH,
"Could not open kinect depth reader");
return -1;
}
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;
}
int main()
{
// 1. Sensor related code
cout << "Try to get default sensor" << endl;
{
if (GetDefaultKinectSensor(&pSensor) != S_OK) {
cerr << "Get Sensor failed" << endl;
return -1;
}
cout << "Try to open sensor" << endl;
if (pSensor->Open() != S_OK) {
cerr << "Can't open sensor" << endl;
return -1;
}
}
// 2. Color related code
cout << "Try to get color source" << endl;
{
// Get frame source
IColorFrameSource* pFrameSource = nullptr;
if (pSensor->get_ColorFrameSource(&pFrameSource) != S_OK) {
cerr << "Can't get color frame source" << endl;
return -1;
}
// Get frame description
cout << "get color frame description" << endl;
IFrameDescription* pFrameDescription = nullptr;
if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK) {
pFrameDescription->get_Width(&iColorWidth);
pFrameDescription->get_Height(&iColorHeight);
uColorPointNum = iColorWidth * iColorHeight;
uColorBufferSize = uColorPointNum * 4 * sizeof(BYTE);
pCSPoints = new CameraSpacePoint[uColorPointNum];
pColorBuffer = new BYTE[4 * uColorPointNum];
}
pFrameDescription->Release();
pFrameDescription = nullptr;
// 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;
}
// release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
}
// 3. Depth related code
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) {
int iDepthWidth = 0,
iDepthHeight = 0;
pFrameDescription->get_Width(&iDepthWidth);
pFrameDescription->get_Height(&iDepthHeight);
uDepthPointNum = iDepthWidth * iDepthHeight;
pDepthBuffer = new UINT16[uDepthPointNum];
}
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. Coordinate Mapper
if (pSensor->get_CoordinateMapper(&pCoordinateMapper) != S_OK) {
cerr << "get_CoordinateMapper failed" << endl;
return -1;
}
while (1) {
idle();
if ((int)pColorBuffer[0] != 0) {
capture_point();
}
}
ExitFunction();
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;
}
