#include "WinBaseLib.h" #include "KinectWin.h" #include "NiJointsTool.h" #include "Graph.h" #include "buffer.h" using namespace jbxl; #ifndef DISABLE_KINECT_SDK using namespace jbxwl; CKinectWin::CKinectWin(void) { device = NULL; m_err_mesg = _T(""); m_scale = 2; m_is_detected = FALSE; m_is_tracking = FALSE; m_is_mirroring = TRUE; m_use_image = TRUE; m_use_led = FALSE; m_use_motor = FALSE; m_enable_motor = TRUE; // m_use_knct_smth = TRUE; m_confidence = 0.5; // 現時点では 0.0より大きければ何でも良い. m_ground_level = NI_DEFAULT_GROUND_LEVEL; pViewData = NULL; hasDepthData = FALSE; // smoothParam.fSmoothing = 0.5f; smoothParam.fCorrection = 0.5f; smoothParam.fPrediction = 0.5f; smoothParam.fJitterRadius = 0.05f; smoothParam.fMaxDeviationRadius = 0.04f; clearAvatarDetected(); } BOOL CKinectWin::init(void) { device = new CKinectDevice(); DWORD mode = NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON; BOOL ret = device->init(mode, m_use_image); if (!ret) { m_err_mesg = device->m_err_mesg; } // setDevState(NI_STATE_DETECT_STOPPED); return ret; } void CKinectWin::free(void) { freeRingBuffer(); deleteDevice(); // free_Buffer(&m_err_mesg); return; } void CKinectWin::deleteDevice(void) { if (device!=NULL) { delete(device); device = NULL; } return; } void CKinectWin::clearJointsData(void) { for (int i=0; i)); rotRing[j].init(NI_RING_BUFFER_SIZE, sizeof(Quaternion)); if (posRing[j].state<0 || rotRing[j].state<0) { freeRingBuffer(); m_err_mesg = _T("CKinectWin::initRingBuffer WARNING: Out of Memory."); ret = FALSE; break; } } return ret; } void CKinectWin::freeRingBuffer(void) { for (int j=0; j=m_confidence) posRing[j].put(&posVect[j]); if (rotQuat[j].c>=m_confidence) rotRing[j].put(&rotQuat[j]); } } ///////////////////////////////////////////////////////////////////////////////////////////////// // // // void CKinectWin::clearAvatarDetected(void) { clearJointsData(); clearRingBuffer(); startPos = Vector(0.0, 0.0, 0.0); m_is_detected = FALSE; m_ground_level= NI_DEFAULT_GROUND_LEVEL; return; } // // // BOOL CKinectWin::checkAvatarDetected(void) { if (posVect[NI_JNT_PELVIS].c>m_confidence && crdVect[NI_JNT_PELVIS].c>0.0) { startPos = posVect[NI_JNT_PELVIS]; m_is_detected = TRUE; // m_ground_level is not used now if (posVect[NI_JNT_R_ANKLE].c>m_confidence && crdVect[NI_JNT_R_ANKLE].c>0.0) { m_ground_level = posVect[NI_JNT_R_ANKLE].z - startPos.z; if (posVect[NI_JNT_L_FOOT].c>m_confidence && crdVect[NI_JNT_L_FOOT].c>0.0) { m_ground_level = Min(m_ground_level, posVect[NI_JNT_L_ANKLE].z - startPos.z); } } else if (posVect[NI_JNT_L_FOOT].c>m_confidence && crdVect[NI_JNT_L_FOOT].c>0.0) { m_ground_level = posVect[NI_JNT_L_ANKLE].z - startPos.z; } } return m_is_detected; } void CKinectWin::setTiltMotor(int ang) { if (m_use_motor && m_enable_motor) { m_enable_motor = FALSE; NuiCameraElevationSetAngle(ang); Sleep(1000); m_enable_motor = TRUE; } } void CKinectWin::setMirroring(BOOL mirror) { m_is_mirroring = mirror; } void CKinectWin::makeDisplayImage() { int index; int label = 0; uByte* src = NULL; uByte* ptr; if (pViewData==NULL) return; if (device->image!=NULL && m_use_image) { src = (uByte*)device->image->m_data; } // if (src!=NULL) { for (int j=0; jysize; j++) { int li; int lj = j*m_scale; int ls = lj*device->m_xsize; for (int i=0; ixsize; i++) { // Mirroring if (m_is_mirroring) li = i*m_scale; else li = (pViewData->xsize-i-1)*m_scale; ptr = &(pViewData->point(i, j)); index = (ls + li)*4; ptr[0] = src[index]; // B ptr[1] = src[index+1]; // G ptr[2] = src[index+2]; // R ptr[3] = src[index+3]; // X // User Label if (getDevState()==NI_STATE_DETECT_EXEC && hasDepthData) { //label = device->depth->get_user(li/2, lj/2); // 320x240 label = device->depth->get_user_index(li, lj); // 640x480 if (label>0) NiSetUserColor(label, ptr); } } } } // No Camera Image else { for (int j=0; jysize; j++) { int li; int lj = j*m_scale; int ls = lj*device->m_xsize; for (int i=0; ixsize; i++) { // Mirror if (m_is_mirroring) li = i*m_scale; else li = (pViewData->xsize-i-1)*m_scale; ptr = &(pViewData->point(i, j)); ptr[0] = ptr[1] = ptr[2] = 224; // Gray Background ptr[3] = 0; // User Label if (getDevState()==NI_STATE_DETECT_EXEC && hasDepthData) { //label = device->depth->get_user(li/2, lj/2); // 320x240 label = device->depth->get_user_index(li, lj); // 640x480 if (label>0) NiSetUserColor(label, ptr, FALSE); } } } } } BOOL CKinectWin::startDetection(void) { clearJointsData(); device->smoothParam = NULL; if (m_use_knct_smth) device->smoothParam = &smoothParam; BOOL ret = device->start_Detection(); if (ret) setLEDColor(NI_LED_BLINK_ORANGE); else m_err_mesg = device->m_err_mesg; return ret; } BOOL CKinectWin::stopDetection(void) { BOOL ret = device->stop_Detection(); setLEDColor(NI_LED_GREEN); if (!ret) m_err_mesg = device->m_err_mesg; return ret; } Vector4 CKinectWin::jointPositionData(int j) { Vector4 vect; if (j>=0 && jjointPosData[j]; } else { memset(&vect, 0, sizeof(Vector4)); } return vect; } // // Tracking Main // BOOL CKinectWin::trackingJoints(void) { m_is_tracking = FALSE; if (hasDepthData) { // int tuser = device->tracking_user; // BOOL ret = device->wait_Skeleton(); if (tuser!=device->tracking_user) { m_is_detected = FALSE; if (device->tracking_user==0) { clearAvatarDetected(); lostTrackingUser(tuser); // virtual } else { clearJointsData(); detectTrackingUser(device->tracking_user); // virtual } } // Tracking if (ret && device->tracking_user>0 && device->tracking_user<=KINECT_MAX_USERS_NUM) { // getJointsPosData(); // if (m_is_detected) { convertJointsData(); // backup2RingBuffer(); saveJointsData(); logingJointsData(); // drawSkeleton(NiGetSkeletonColor(device->tracking_user)); } m_is_tracking = TRUE; } } else { m_is_detected = FALSE; } return m_is_tracking; } // // Calculate posVect, crdVect // void CKinectWin::getJointsPosData(void) { if (!m_is_mirroring) { for (int j=0; jjointPosData[j].x = - device->jointPosData[j].x; } } // for (int j=0; jjointPosData[j]; int n = j; if (m_is_mirroring) n = NiSDKMirrorJointNum(j, NiSDK_Kinect); posVect[n].set(-pos.z, pos.x, pos.y); } posVect[NI_JNT_TORSO] = 2.0*posVect[NI_JNT_PELVIS] - (posVect[NI_JNT_R_HIP] + posVect[NI_JNT_L_HIP])/2.0; // set2DCoordinate(); checkBoneLength(); if (!m_is_detected) checkAvatarDetected(); // for (int j=0; j vp; vp.xs = pViewData->xsize; vp.ys = pViewData->ysize; vp.zs = 1; vp.gp = (unsigned int*)pViewData->grptr; if (crdVect[j1].c>0.0 && crdVect[j2].c>0.0) { MSGraph_Line(vp, crdVect[j1].x, crdVect[j1].y, crdVect[j2].x, crdVect[j2].y, col); } } void CKinectWin::set2DCoordinate(void) { for (int j=0; jdepth!=NULL && pViewData!=NULL) { // for (int j=0; jjointPosData[j], &x, &y, NUI_IMAGE_RESOLUTION_640x480); int xs = (int)x; int ys = (int)y; if (xs>0 && ys>0) { // 縁(0)は除外 device->depth->get_image_coord(&xs, &ys); if (xs>=0) xs = xs/m_scale; if (ys>=0) ys = ys/m_scale; crdVect[j].x = xs; crdVect[j].y = ys; if (xs>0 && xsxsize-1 && ys>0 && ysxsize-1) { // 縁は除外 crdVect[j].c = 1.0; } } } } return; } #endif // DISABLE_KINECT_SDK