// Callback: New user was detected void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { XnUInt32 epochTime = 0; xnOSGetEpochTime(&epochTime); ROS_INFO("%d New User %d\n", epochTime, nId); // New user found if (g_bNeedPose) { g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId); } else { g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); } publishUserDetected(nId); }
// Save calibration to file void SaveCalibration() { XnUserID aUserIDs[20] = {0}; XnUInt16 nUsers = 20; g_UserGenerator.GetUsers(aUserIDs, nUsers); for (int i = 0; i < nUsers; ++i) { // Find a user who is already calibrated if (g_UserGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i])) { // Save user's calibration to file g_UserGenerator.GetSkeletonCap().SaveCalibrationDataToFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME); break; } } }
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { printf("New User: %d\n", nId); g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(POSE_TO_USE, nId); }
float RLeg::getAnklePitch(XnUserID user, xn::UserGenerator guser) { XnSkeletonJointTransformation knee, ankle, foot; RLeg finalAngle; guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_KNEE,knee); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_ANKLE,ankle); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_FOOT,foot); return (finalAngle.getAngle(knee.position.position, ankle.position.position, foot.position.position)); }
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) { if (bSuccess) { ROS_INFO("Calibration complete, start tracking user %d", nId); g_UserGenerator.GetSkeletonCap().StartTracking(nId); } else { ROS_INFO("Calibration failed for user %d", nId); if (g_bNeedPose) g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId); else g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); } }
float RLeg::getKneePitch(XnUserID user, xn::UserGenerator guser) { XnSkeletonJointTransformation hip, knee, ankle; RLeg finalAngle; guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_HIP,hip); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_KNEE,knee); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_FOOT,ankle); return (finalAngle.getAngle(knee.position.position, hip.position.position, ankle.position.position)); }
///Obtiene el angulo real dado por el kinect del movimiento de la cadera float RLeg::getHipRoll(XnUserID user, xn::UserGenerator guser) { XnSkeletonJointTransformation lhip, rhip, knee; RLeg finalAngle; guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_HIP,lhip); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_HIP,rhip); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_KNEE,knee); return (finalAngle.getAngle(lhip.position.position, rhip.position.position, knee.position.position)); }
void processUsers(uint32_t frameId) { // Process users XnUserID aUsers[20]; XnUInt16 nUsers = 20; g_UserGenerator.GetUsers(aUsers, nUsers); std::vector<XnUserID> users; for (int i = 0; i < nUsers; ++i) { if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { users.push_back(aUsers[i]); } } processAndPublish(users, frameId); }
// this function is called each frame void glutDisplay (void) { glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); glDisable(GL_TEXTURE_2D); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } // Process the data g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); DrawDepthMap(depthMD, sceneMD); glutSwapBuffers(); }
void publishTransforms(const std::string& frame_id) { XnUserID users[15]; XnUInt16 users_count = 15; XnUserID user = 0; g_UserGenerator.GetUsers(users, users_count); for (int i = 0; i <= users_count; ++i) { user = users[i]; if (!g_UserGenerator.GetSkeletonCap().IsTracking(user)) { continue; } publishTransform(user, XN_SKEL_HEAD, frame_id, "head"); publishTransform(user, XN_SKEL_NECK, frame_id, "neck"); publishTransform(user, XN_SKEL_TORSO, frame_id, "torso"); // publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder"); // publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow"); // publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand"); publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "right_shoulder"); publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "right_elbow"); publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "right_hand"); // publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder"); // publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow"); // publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand"); publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "left_shoulder"); publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "left_elbow"); publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "left_hand"); // publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip"); // publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee"); // publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot"); publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "right_hip"); publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "right_knee"); publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "right_foot"); // publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip"); // publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee"); // publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot"); publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "left_hip"); publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "left_knee"); publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "left_foot"); } }
///Obtiene el angulo real dado por el kinect del movimiento frontal de la cabeza float Head::getHeadPitch(XnUserID user, xn::UserGenerator guser) { XnSkeletonJointTransformation head, neck, torso; Head finalAngle; guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_HEAD,head); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_NECK,neck); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_TORSO,torso); return (finalAngle.getAngle(head.position.position, neck.position.position, torso.position.position)); }
//---------------------------------------- void XN_CALLBACK_TYPE ofxOpenNITracker::User_LostUser(xn::UserGenerator& userGenerator, XnUserID nID, void* pCookie){ ofLogVerbose(LOG_NAME) << "Lost user" << nID; ofxOpenNITracker* tracker = static_cast<ofxOpenNITracker*>(pCookie); userGenerator.GetSkeletonCap().Reset(nID); tracker->users[nID]; tracker->setUserState(nID, ofxOpenNIUser::Lost); }
// track two users // take down users' joint positions // calculate joint angles and etc. void CheckPosture(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, FILE *fp) { XnUserID aUsers[2]; XnUInt16 nUsers = 2; g_UserGenerator.GetUsers(aUsers, nUsers); for(int i = 0; i < nUsers; ++i) { // if the user is being tracked // take down his/her joint positions if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // check user posture RecordAngle(aUsers[i], fp); } } }
// Callback: New user was detected void XN_CALLBACK_TYPE new_user(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { printf("New User %d\n", nId); userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); if (kitchenMode) return; lo_send(addr, "/new_user","i",(int)nId); }
void XN_CALLBACK_TYPE whu_NewUser(xn::UserGenerator &generator, XnUserID user, void* pCookie) { cout<<"New User Identified:"<<user<<endl; //generator.GetSkeletonCap().RequestCalibration(user,true);//假如不需要做出投降姿势才开始的话,把此句解注释 generator.GetPoseDetectionCap().StartPoseDetection("Psi", user);//做出[ 。]姿势以开始用户认证// }
void hiKinect::getPosition(XnSkeletonJoint joint, XnSkeletonJointPosition &pos){ XnUserID id = getTrackedUserID(); if (id != 0) { g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(id, joint, pos); }else{ return; } }
XnBool AssignPlayer(XnUserID user) { if (g_nPlayer != 0) return FALSE; XnPoint3D com; g_UserGenerator.GetCoM(user, com); if (com.Z == 0) return FALSE; printf("Matching for existing calibration\n"); g_UserGenerator.GetSkeletonCap().LoadCalibrationData(user, 0); g_UserGenerator.GetSkeletonCap().StartTracking(user); g_nPlayer = user; return TRUE; }
float CalculateJointAngle(XnUserID player, XnSkeletonJoint eJointLeft, XnSkeletonJoint eJointMid, XnSkeletonJoint eJointRight) { if (!g_UserGenerator.GetSkeletonCap().IsTracking(player)) { printf("not tracked!\n"); return -1; } if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJointLeft)|| !g_UserGenerator.GetSkeletonCap().IsJointActive(eJointMid) || !g_UserGenerator.GetSkeletonCap().IsJointActive(eJointRight)) { return -1; } XnSkeletonJointPosition jointLeft, jointMid, jointRight; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJointLeft, jointLeft); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJointMid, jointMid); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJointRight, jointRight); if (jointLeft.fConfidence < 0.5 || jointMid.fConfidence < 0.5 || jointRight.fConfidence < 0.5) { return -1; } return CalculateAngleInTriangle(jointLeft.position, jointMid.position, jointRight.position); }
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) { if (bSuccess) { ROS_INFO("Calibration complete, start tracking user %d", nId); g_UserGenerator.GetSkeletonCap().StartTracking(nId); sound_play::SoundClient soundhandle; soundhandle.say("hello sir what can i do for you "); ROS_INFO("sayed..."); } else { ROS_INFO("Calibration failed for user %d", nId); if (g_bNeedPose) g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId); else g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); } }
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { ROS_INFO("New User %d.", nId); //se non ha ancora fatto calibrazione la carica da file e ?la salva su calibrationData? if(calibrationData == NULL) { g_UserGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(nId, genericUserCalibrationFileName.c_str()); g_UserGenerator.GetSkeletonCap().SaveCalibrationData(nId, calibrationData); } else //se no carica la calibration data??? { g_UserGenerator.GetSkeletonCap().LoadCalibrationData(nId, calibrationData); } //inizia il tracking g_UserGenerator.GetSkeletonCap().StartTracking(nId); ROS_INFO("Start tracking user: %d.", nId); }
// Callback: Detected a pose void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie) { ROS_INFO("[bk_skeletal_tracker] Pose (%s) detected for user %d", strPose, nId); g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId); // If we already calibrated on a user, just load that calibration if(g_bhasCal) { //g_UserGenerator.GetSkeletonCap().LoadCalibrationData(nId, 0); g_UserGenerator.GetSkeletonCap().LoadCalibrationData(first_calibrated_user_, 0); g_UserGenerator.GetSkeletonCap().StartTracking(nId); ROS_INFO("[bk_skeletal_tracker] Loaded previous calibration of user %d", first_calibrated_user_); } // Detected pose of first user: start calibration else { g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, true); } }
void CleanupExit(){ g_scriptNode.Release(); g_DepthGenerator.Release(); g_UserGenerator.Release(); g_Player.Release(); g_Context.Release(); exit (1); }
void xn_init(char *argv) { xn_call_and_check(gContext.InitFromXmlFile("../conf/SamplesConfig.xml"), "init from xml"); xn::ImageGenerator image; xn_call_and_check(gContext.FindExistingNode(XN_NODE_TYPE_IMAGE, image), "find image node"); xn_call_and_check(gContext.FindExistingNode(XN_NODE_TYPE_DEPTH, gDepthGenerator), "find depth node"); xn_call_and_check(gContext.FindExistingNode(XN_NODE_TYPE_USER, gUserGenerator), "find user node"); XnCallbackHandle userCB, poseCB, calibrationCB; gUserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); gUserGenerator.RegisterUserCallbacks(xn_onFoundUser, xn_onLostUser, NULL, userCB); gUserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(NULL, xn_onCalibrated, NULL, calibrationCB); gUserGenerator.GetSkeletonCap().GetCalibrationPose(gPose); gUserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(xn_onPoseDetected, NULL, NULL, poseCB); gImageDrawer = new ImageDrawer(image); gImageDrawer->setDrawRegion(WIN_SIZE_X/15, WIN_SIZE_Y/15, WIN_SIZE_X/3, WIN_SIZE_Y/3); gDepthDrawer = new DepthDrawer(gDepthGenerator, gUserGenerator); gDepthDrawer->setDrawRegion(0, 0, WIN_SIZE_X, WIN_SIZE_Y); gSkeletonDrawer = new SkeletonDrawer(gUserGenerator, gDepthGenerator); gSkeletonDrawer->setDrawRegion(0, 0, WIN_SIZE_X, WIN_SIZE_Y); capture = new Capture(&gContext); capture->setFileName(argv); XnSkeletonJoint joints[] = { XN_SKEL_HEAD, XN_SKEL_NECK, XN_SKEL_TORSO, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, }; for(int i = 0; i < sizeof(joints)/sizeof(XnSkeletonJoint); i++){ capture->addSkeletonJoint(joints[i]); } }
information() { RC(context.Init(), "Context Intialized"); XnMapOutputMode mode; mode.nXRes = XN_VGA_X_RES; mode.nYRes = XN_VGA_Y_RES; mode.nFPS = 30; RC(image.Create(context), "Create image buffer"); RC(image.SetMapOutputMode(mode), "Set image mode"); RC(depth.Create(context), "Create depth buffer"); RC(depth.SetMapOutputMode(mode), "Set depth mode"); xn::Query q; RC(q.AddSupportedCapability(XN_CAPABILITY_SKELETON), "Request skeleton"); try { RC(context.FindExistingNode(XN_NODE_TYPE_USER, user), "User generator"); } catch (...) { RC(user.Create(context), "Get skeleton!!!"); } // RC(user.Create(context, &q), "Get skeleton!!!"); // // xn::NodeInfoList il; // RC(context.EnumerateProductionTrees(XN_NODE_TYPE_USER, &q, il, NULL), // "Enumerate nodes"); // // xn::NodeInfo i = *il.Begin(); // RC(context.CreateProductionTree(i), "Create skeleton node"); // RC(i.GetInstance(user), "Get skeleton"); user.RegisterUserCallbacks(User_NewUser, NULL, NULL, hUserCallbacks); user.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, &user, hCalibrationCallbacks); if (user.GetSkeletonCap().NeedPoseForCalibration()) { if (!user.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { post("Pose required, but not supported\n"); } else { user.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, &user, hPoseCallbacks); user.GetSkeletonCap().GetCalibrationPose(g_strPose); user.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); } } RC(context.StartGeneratingAll(), "Start generating data"); post("Kinect initialized!\n"); }
// Callback: An existing user was lost void XN_CALLBACK_TYPE User_LostUser( xn::UserGenerator& rGenerator ,XnUserID nID ,void* pCookie ) { printf("Lost user %d\n", nID); rGenerator.GetSkeletonCap().Reset(nID); }
void XN_CALLBACK_TYPE UserFoundCB( xn::UserGenerator& gen, XnUserID id, void* /*cookie*/ ) { LOG(logDEBUG) << "UserFoundCB()"; gen.GetPoseDetectionCap().StartPoseDetection("Psi", id); }
// Callback: New user was detected void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { printf("New User %d\n", nId); // New user found // if (g_bNeedPose) // { // g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId); // } // else{ if(g_bhasCal){ g_UserGenerator.GetSkeletonCap().LoadCalibrationData(nId, 0); g_UserGenerator.GetSkeletonCap().StartTracking(nId); } else{ //never gotten calibration before // g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId); } // } }
// Callback: Finished calibration void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) { if (bSuccess) { // Calibration succeeded - save this first calibration and start tracking the user ROS_INFO("[bk_skeletal_tracker] Calibration complete, now tracking user %d", nId); g_bhasCal=true; first_calibrated_user_ = nId; g_UserGenerator.GetSkeletonCap().SaveCalibrationData(nId, 0); g_UserGenerator.GetSkeletonCap().StartTracking(nId); // Save the user's calibration // Get mask of this user xn::SceneMetaData sceneMD; cv::Mat label_image; g_UserGenerator.GetUserPixels(0, sceneMD); getUserLabelImage(sceneMD, label_image); label_image = (label_image == nId); xn::ImageMetaData imageMD; //g_ImageGenerator.GetMetaData(imageMD); cv::Mat rgb; rgb = getRGB(imageMD); original_cal_.init(rgb, label_image); user_cal_ = original_cal_; } else { // Calibration failed ROS_INFO("[bk_skeletal_tracker] Calibration failed for user %d", nId); if (g_bNeedPose) g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId); else g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, true); } }
void XN_CALLBACK_TYPE CalibrationEnded(xn::SkeletonCapability& skeleton, XnUserID user, XnBool bSuccess, void* cxt) { printf("Calibration done [%d] %ssuccessfully\n", user, bSuccess?"":"un"); if (bSuccess) { if (!g_bCalibrated) { g_UserGenerator.GetSkeletonCap().SaveCalibrationData(user, 0); g_nPlayer = user; g_UserGenerator.GetSkeletonCap().StartTracking(user); g_bCalibrated = TRUE; } XnUserID aUsers[10]; XnUInt16 nUsers = 10; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(aUsers[i]); } }
// Callback: Detected a pose void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& /*capability*/, const XnChar* strPose, XnUserID nId, void* /*pCookie*/){ //TODO : archivage du squelette, puis mise à jours du squelette. // Si la distance est trop éloignée, reprendre le squelette précédent (peut être faire une transition entre les deux ?) Skeleton new_s(nId); float distance = m_skeleton.find(nId)->second.distance(new_s); if(distance < SEUIL_DISTANCE) m_skeleton.find(nId)->second = new_s; // else // possibilité de détecter la pose courante et voir si elle n'a pas déjà été enregistrée (à un delta près), probablement il faut revoir la fonction distance du squelette // si on perd le personnage mais qu'on en trouve un nouveau dans une pose connue, on peu essayer de faire une transition. //TODO export du squelette XnUInt32 epochTime = 0; xnOSGetEpochTime(&epochTime); printf("%d Pose %s detected for user %d\n", epochTime, strPose, nId); g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId); g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); }