7#ifdef ENABLE_KINECT_SDK
16CKinectDevice::CKinectDevice(
void)
28 m_state = NI_STATE_DETECT_STOPPED;
40CKinectDevice::~CKinectDevice(
void)
42 DEBUG_INFO(
"DESTRUCTOR: CKinectDevice: START\n");
44 DEBUG_INFO(
"DESTRUCTOR: CKinectDevice: END\n");
49BOOL CKinectDevice::init(DWORD mode, BOOL use_image)
52 HRESULT hrt = ::NuiGetSensorCount(&m_count);
53 if (FAILED(hrt) || m_count<=0) {
54 m_err_mesg = _T(
"CKinectDevice::init(): WARNING: No Kinect is detected.\n");
58 BOOL ret = create_Context(mode);
59 if (ret && use_image) create_Image();
60 if (ret) ret = create_Depth();
64 m_xsize = image->m_xsize;
65 m_ysize = image->m_ysize;
66 m_nfps = image->m_nfps;
68 else if (depth!=NULL) {
69 m_xsize = depth->m_xsize;
70 m_ysize = depth->m_ysize;
71 m_nfps = depth->m_nfps;
74 m_err_mesg = _T(
"CKinectDevice::init(): ERROR: Unknown Error is occurred!!");
84void CKinectDevice::free(
void)
96void CKinectDevice::clear_JointsData(
void)
98 memset(jointPosData, 0,
sizeof(Vector4)*KINECT_JOINT_NUM);
107BOOL CKinectDevice::create_Context(DWORD mode,
int index)
109 HRESULT hrt = NuiCreateSensorByIndex(index, &context);
111 m_err_mesg = _T(
"CKinectDevice::create_Context(): ERROR: Fail to create Device.");
115 hrt = context->NuiInitialize(mode);
118 m_err_mesg = _T(
"CKinectDevice::create_Context(): ERROR: Fail to initalize Device.");
127BOOL CKinectDevice::create_Image(
void)
129 NUI_IMAGE_RESOLUTION reso = NUI_IMAGE_RESOLUTION_640x480;
133 m_err_mesg = _T(
"CKinectDevice::create_Image(): ERROR: context is NULL.");
136 if (image==NULL) image =
new CKinectImage();
139 HANDLE hdl = image->create(TRUE, FALSE, NULL);
142 m_err_mesg = _T(
"CKinectDevice::create_Image(): ERROR: Fail to create Image.");
146 HRESULT hrt = context->NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, reso, NULL, 2, image->m_handle, &image->m_stream);
149 m_err_mesg = _T(
"CKinectDevice::create_Image(): ERROR: Fail to open stream of Image.");
153 NuiImageResolutionToSize(reso, (DWORD&)image->m_xsize, (DWORD&)image->m_ysize);
156 if (image->m_data==NULL) image->make_data();
163BOOL CKinectDevice::create_Depth(
void)
165 NUI_IMAGE_RESOLUTION reso = NUI_IMAGE_RESOLUTION_640x480;
169 m_err_mesg = _T(
"CKinectDevice::create_Depth(): ERROR: context is NULL.");
172 if (depth==NULL) depth =
new CKinectDepth();
175 HANDLE hdl = depth->create(TRUE, FALSE, NULL);
178 m_err_mesg = _T(
"CKinectDevice::create_Depth(): ERROR: Fail to create Depth.");
182 HRESULT hrt = context->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, reso, NULL, 2, depth->m_handle, &depth->m_stream);
185 m_err_mesg = _T(
"CKinectDevice::create_Depth(): ERROR: Fail to open stream of Depth.");
190 NuiImageResolutionToSize(reso, (DWORD&)depth->m_xsize, (DWORD&)depth->m_ysize);
193 if (depth->m_data==NULL) depth->make_data();
201BOOL CKinectDevice::create_Skeleton(
int profile)
204 m_err_mesg = _T(
"CKinectDevice::create_Skeleton(): ERROR: context is NULL.");
208 m_err_mesg = _T(
"CKinectDevice::create_Skeleton(): ERROR: depth is NULL.");
211 if (skeleton==NULL) skeleton =
new CKinectSkeleton();
214 HANDLE hdl = skeleton->create(TRUE, FALSE, NULL);
217 m_err_mesg = _T(
"CKinectDevice::create_Skeleton(): ERROR: Fail to create Skeleton.");
225 if (profile==KINECT_SKEL_PROFILE_UPPER) {
226 ftracking = NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT;
227 ftracking |= NUI_SKELETON_TRACKING_FLAG_ENABLE_IN_NEAR_RANGE;
228 streamode = NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE;
232 hrt = context->NuiImageStreamSetImageFrameFlags(depth->m_stream, streamode);
233 if (SUCCEEDED(hrt)) {
234 if (profile==KINECT_SKEL_PROFILE_UPPER) depth->correct_map(-15, 8);
238 hrt = context->NuiSkeletonTrackingEnable(skeleton->m_handle, ftracking);
241 m_err_mesg = _T(
"CKinectDevice::create_Skeleton(): ERROR: Fail to enable Skeleton Tracking.");
250BOOL CKinectDevice::create_Face(
void)
253 m_err_mesg = _T(
"CKinectDevice::create_Face(): ERROR: context is NULL.");
257 m_err_mesg = _T(
"CKinectDevice::create_Face(): ERROR: image is NULL.");
261 m_err_mesg = _T(
"CKinectDevice::create_Face(): ERROR: depth is NULL.");
265 if (face==NULL) face =
new CKinectFaceTracker();
267 BOOL ret = face->create(image->m_xsize, image->m_ysize, image->m_data, depth->m_data);
270 m_err_mesg = _T(
"CKinectDevice::create_Face(): ERROR: Fail to create Face Tracker\nMaybe FaceTrackData.dll is not found.");
282BOOL CKinectDevice::wait_Image(DWORD msec)
284 if (image==NULL)
return FALSE;
285 if (image->m_data==NULL)
return FALSE;
287 BOOL ret = image->wait(msec);
289 HRESULT hrt = context->NuiImageStreamGetNextFrame(image->m_stream, 0, &imageFrame);
290 if (FAILED(hrt))
return FALSE;
292 NUI_LOCKED_RECT rect;
293 imageFrame.pFrameTexture->LockRect(0, &rect, NULL, 0);
294 long int len = rect.Pitch*image->m_ysize;
296 memcpy(image->m_data, (
char*)rect.pBits, len);
297 imageFrame.pFrameTexture->UnlockRect(0);
298 context->NuiImageStreamReleaseFrame(image->m_stream, &imageFrame);
306BOOL CKinectDevice::wait_Depth(DWORD msec)
308 if (depth==NULL)
return FALSE;
309 if (depth->m_data==NULL)
return FALSE;
311 BOOL ret = depth->wait(msec);
313 HRESULT hrt = context->NuiImageStreamGetNextFrame(depth->m_stream, 0, &depthFrame);
314 if (FAILED(hrt))
return FALSE;
317 NUI_LOCKED_RECT rect;
318 depthFrame.pFrameTexture->LockRect(0, &rect, NULL, 0);
319 long int len = rect.Pitch*depth->m_ysize;
321 memcpy(depth->m_data, (
char*)rect.pBits, len);
322 depthFrame.pFrameTexture->UnlockRect(0);
323 context->NuiImageStreamReleaseFrame(depth->m_stream, &depthFrame);
331int CKinectDevice::wait_Skeleton(
int tuser,
int duser, DWORD msec)
333 if (skeleton==NULL)
return 0;
335 BOOL ret = skeleton->wait(msec);
337 HRESULT hrt = context->NuiSkeletonGetNextFrame(0, &skltnFrame);
338 if (smoothParam!=NULL && !FAILED(hrt)) {
339 hrt = context->NuiTransformSmooth(&skltnFrame, smoothParam);
341 if (FAILED(hrt))
return FALSE;
345 skeleton->m_data = skltnFrame.SkeletonData[tuser-1];
346 if (skeleton->m_data.eTrackingState!=NUI_SKELETON_TRACKED) {
351 tuser = get_TrackingUser(duser);
353 if (tuser==0)
return 0;
356 for (
int j=0; j<KINECT_JOINT_NUM; j++) {
357 jointPosData[j] = skeleton->m_data.SkeletonPositions[j];
371BOOL CKinectDevice::start_Detection(
int profile)
374 if (m_state==NI_STATE_DETECT_EXEC) {
375 m_err_mesg = _T(
"CKinectDevice::start_Detection(): WARNING: detection is already executed.");
379 m_state = NI_STATE_DETECT_STARTING;
382 if (depth==NULL) create_Depth();
385 ret = create_Skeleton(profile);
387 m_state = NI_STATE_DETECT_EXEC;
390 m_state = NI_STATE_DETECT_STOPPING;
392 m_state = NI_STATE_DETECT_STOPPED;
397 BOOL infoff = context->NuiGetForceInfraredEmitterOff();
398 if (infoff) ret = !context->NuiSetForceInfraredEmitterOff(FALSE);
399 if (!ret) m_err_mesg = _T(
"CKinectDevice::start_Detection(): ERROR: Fail to Start Emitter.");
407BOOL CKinectDevice::stop_Detection(
void)
409 if (m_state==NI_STATE_DETECT_STOPPED) {
410 m_err_mesg = _T(
"CKinectDevice::stop_Detection(): WARNING: detection is already stopped.");
414 m_state= NI_STATE_DETECT_STOPPING;
415 context->NuiSetForceInfraredEmitterOff(TRUE);
417 ::Sleep(NI_STOP_WAIT_TIME);
421 m_state = NI_STATE_DETECT_STOPPED;
428int CKinectDevice::get_TrackingUser(
int duser)
432 if (depth!=NULL && skeleton!=NULL) {
434 for (
int i=0; i<KINECT_USERS_NUM; i++) {
435 int idx = (duser + i) % KINECT_USERS_NUM;
436 if (depth->userLabel[idx]>0) {
437 skeleton->m_data = skltnFrame.SkeletonData[idx];
438 if (skeleton->m_data.eTrackingState==NUI_SKELETON_TRACKED) {
455void CKinectDevice::delete_Context(
void)
459 context->NuiShutdown();
467void CKinectDevice::delete_Image(
void)
478void CKinectDevice::delete_Depth(
void)
489void CKinectDevice::delete_Skeleton(
void)
491 if (skeleton!=NULL) {
493 context->NuiSkeletonTrackingDisable();
503void CKinectDevice::delete_Face(
void)
518CKinectImage::CKinectImage(
void)
532void CKinectImage::free(
void)
544void CKinectImage::make_data(
void)
546 if (m_xsize<=0 || m_ysize<=0)
return;
548 m_data_len = m_xsize*m_ysize*4;
549 m_data = (uByte*)malloc(m_data_len);
560CKinectDepth::CKinectDepth(
void)
581void CKinectDepth::free(
void)
596void CKinectDepth::make_data(
void)
598 if (m_xsize<=0 || m_ysize<=0)
return;
600 m_data_len = m_xsize*m_ysize*2;
601 m_data = (uByte*)malloc(m_data_len);
608void CKinectDepth::make_map(
void)
610 if (m_xsize<=0 || m_ysize<=0)
return;
611 if (has_map) delete_map();
613 int sz = m_xsize*m_ysize;
614 int len =
sizeof(int)*sz;
616 m_xmap_i2d = (
int*)malloc(len);
617 m_ymap_i2d = (
int*)malloc(len);
618 m_xmap_d2i = (
int*)malloc(len);
619 m_ymap_d2i = (
int*)malloc(len);
621 if (m_xmap_i2d==NULL || m_ymap_i2d==NULL || m_xmap_d2i==NULL || m_ymap_d2i==NULL) {
622 if (m_xmap_i2d!=NULL) { ::free(m_xmap_i2d); m_xmap_i2d = NULL;}
623 if (m_ymap_i2d!=NULL) { ::free(m_ymap_i2d); m_ymap_i2d = NULL;}
624 if (m_xmap_d2i!=NULL) { ::free(m_xmap_d2i); m_xmap_d2i = NULL;}
625 if (m_ymap_d2i!=NULL) { ::free(m_ymap_d2i); m_ymap_d2i = NULL;}
629 memset(m_xmap_i2d, 0, len);
630 memset(m_ymap_i2d, 0, len);
631 memset(m_xmap_d2i, 0, len);
632 memset(m_ymap_d2i, 0, len);
635 for (
int j=0; j<m_ysize; j++) {
636 for (
int i=0; i<m_xsize; i++) {
643 HRESULT hdr = NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution
644 (NUI_IMAGE_RESOLUTION_640x480, NUI_IMAGE_RESOLUTION_640x480, NULL, (LONG)i, (LONG)j, 0, &ii, &jj);
646 if (ii>=0 && ii<m_xsize && jj>=0 && jj<m_ysize) {
647 kk = jj*m_xsize + ii;
648 m_xmap_i2d[kk] = (int)(i + 1);
649 m_ymap_i2d[kk] = (int)(j + 1);
658 for (
int i=0; i<sz; i++) {
661 if (m_xmap_i2d[i]<0 || m_ymap_i2d[i]<0) {
662 m_xmap_i2d[i] = m_ymap_i2d[i] = -1;
674void CKinectDepth::delete_map(
void)
676 if (m_xmap_i2d!=NULL) {
680 if (m_ymap_i2d!=NULL) {
684 if (m_xmap_d2i!=NULL) {
688 if (m_ymap_d2i!=NULL) {
701void CKinectDepth::correct_map(
int cx,
int cy)
703 if (!has_map) make_map();
704 if (!has_map)
return;
707 if (cx!=0 || cy!=0) {
708 int sz = m_xsize*m_ysize;
709 int i2d_x, i2d_y, d2i_x, d2i_y;
711 for (
int i=0; i<sz; i++) {
712 if (m_xmap_i2d[i]>=0) {
713 i2d_x = m_xmap_i2d[i] + cx;
714 i2d_y = m_ymap_i2d[i] + cy;
715 if (i2d_x>=0 && i2d_x<m_xsize) m_xmap_i2d[i] = i2d_x;
716 if (i2d_y>=0 && i2d_y<m_ysize) m_ymap_i2d[i] = i2d_y;
719 d2i_x = m_xmap_d2i[i] - cx;
720 d2i_y = m_ymap_d2i[i] - cy;
721 if (d2i_x>=0 && d2i_x<m_xsize) m_xmap_d2i[i] = d2i_x;
722 if (d2i_y>=0 && d2i_y<m_ysize) m_ymap_d2i[i] = d2i_y;
731int CKinectDepth::get_user_index(
int i,
int j)
734 if (i<0 || j<0 || i>=m_xsize || j>=m_ysize)
return val;
737 int len = j*m_xsize + i;
738 int ii = m_xmap_i2d[len];
739 int jj = m_ymap_i2d[len];
740 if (ii>=0 && jj>=0 && ii<m_xsize && jj<m_ysize) {
741 val = m_data[(jj*m_xsize + ii)*2] & 0x07;
745 val = m_data[(j*m_xsize + i)*2] & 0x07;
748 if (val>KINECT_USERS_NUM) val = 0;
755uWord CKinectDepth::get_depth(
int i,
int j)
758 if (i<0 || j<0 || i>=m_xsize || j>=m_ysize)
return val;
760 uWord* ptr = (uWord*)m_data;
762 int len = j*m_xsize + i;
763 int ii = m_xmap_i2d[len];
764 int jj = m_xmap_i2d[len];
765 if (ii>=0 && jj>=0) {
766 val = ::NuiDepthPixelToDepth(ptr[jj*m_xsize+ii]);
769 val = ::NuiDepthPixelToDepth(ptr[j*m_xsize+i]);
773 val = ::NuiDepthPixelToDepth(ptr[j*m_xsize+i]);
781void CKinectDepth::get_users(
void)
783 memset(userLabel, 0, KINECT_USERS_NUM*
sizeof(
int));
785 for (
int j=0; j<m_ysize; j++) {
787 for (
int i=0; i<m_xsize; i++) {
788 int val = m_data[(lj + i)*2] & 0x07;
789 if (val>0 && val<=KINECT_USERS_NUM) {
800void CKinectDepth::get_image_coord(
int* i,
int* j)
802 if (i==NULL || j==NULL)
return;
803 if ((*i)<0 || (*j)<0 || (*i)>=m_xsize || (*j)>=m_ysize) {
809 int len = (*j)*m_xsize + (*i);
810 *i = m_xmap_d2i[len];
811 *j = m_ymap_d2i[len];
823CKinectSkeleton::CKinectSkeleton(
void)
830void CKinectSkeleton::free(
void)