Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * usertracker_thread.cpp - OpenNI user tracker thread 00004 * 00005 * Created: Sun Feb 27 17:53:38 2011 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "usertracker_thread.h" 00024 #include "utils/setup.h" 00025 00026 #include <core/threading/mutex_locker.h> 00027 #include <interfaces/HumanSkeletonInterface.h> 00028 #include <interfaces/HumanSkeletonProjectionInterface.h> 00029 #include <fvutils/ipc/shm_image.h> 00030 00031 #include <memory> 00032 00033 using namespace fawkes; 00034 using namespace firevision; 00035 00036 /** @class OpenNiUserTrackerThread "usertracker_thread.h" 00037 * OpenNI User Tracker Thread. 00038 * This thread requests a user tracker node from OpenNI and publishes the 00039 * retrieved information via the blackboard. 00040 * 00041 * @author Tim Niemueller 00042 */ 00043 00044 /** Constructor. */ 00045 OpenNiUserTrackerThread::OpenNiUserTrackerThread() 00046 : Thread("OpenNiUserTrackerThread", Thread::OPMODE_WAITFORWAKEUP), 00047 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS) 00048 { 00049 } 00050 00051 00052 /** Destructor. */ 00053 OpenNiUserTrackerThread::~OpenNiUserTrackerThread() 00054 { 00055 } 00056 00057 00058 static void XN_CALLBACK_TYPE 00059 cb_new_user(xn::UserGenerator &generator, XnUserID id, void *cookie) 00060 { 00061 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00062 t->new_user(id); 00063 } 00064 00065 static void XN_CALLBACK_TYPE 00066 cb_lost_user(xn::UserGenerator &generator, XnUserID id, void *cookie) 00067 { 00068 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00069 t->lost_user(id); 00070 } 00071 00072 static void XN_CALLBACK_TYPE 00073 cb_pose_start(xn::PoseDetectionCapability &capability, 00074 const XnChar *pose_name, XnUserID id, void* cookie) 00075 { 00076 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00077 t->pose_start(id, pose_name); 00078 } 00079 00080 static void XN_CALLBACK_TYPE 00081 cb_pose_end(xn::PoseDetectionCapability &capability, 00082 const XnChar *pose_name, XnUserID id, void* cookie) 00083 { 00084 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00085 t->pose_end(id, pose_name); 00086 } 00087 00088 static void XN_CALLBACK_TYPE 00089 cb_calibration_start(xn::SkeletonCapability &capability, XnUserID id, void *cookie) 00090 { 00091 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00092 t->calibration_start(id); 00093 } 00094 00095 #if XN_VERSION_GE(1,3,2,0) 00096 static void XN_CALLBACK_TYPE 00097 cb_calibration_complete(xn::SkeletonCapability &capability, XnUserID id, 00098 XnCalibrationStatus status, void *cookie) 00099 { 00100 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00101 t->calibration_end(id, status == XN_CALIBRATION_STATUS_OK); 00102 } 00103 #else 00104 static void XN_CALLBACK_TYPE 00105 cb_calibration_end(xn::SkeletonCapability &capability, XnUserID id, 00106 XnBool success, void *cookie) 00107 { 00108 OpenNiUserTrackerThread *t = static_cast<OpenNiUserTrackerThread *>(cookie); 00109 t->calibration_end(id, success); 00110 } 00111 #endif 00112 00113 00114 void 00115 OpenNiUserTrackerThread::init() 00116 { 00117 MutexLocker lock(openni.objmutex_ptr()); 00118 00119 __user_gen = new xn::UserGenerator(); 00120 std::auto_ptr<xn::UserGenerator> usergen_autoptr(__user_gen); 00121 00122 __depth_gen = new xn::DepthGenerator(); 00123 std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen); 00124 00125 XnStatus st; 00126 00127 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen); 00128 fawkes::openni::setup_map_generator(*__depth_gen, config); 00129 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_USER, __user_gen); 00130 00131 if (!__user_gen->IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { 00132 throw Exception("User generator does not support skeleton capability"); 00133 } 00134 00135 __scene_md = new xn::SceneMetaData(); 00136 std::auto_ptr<xn::SceneMetaData> scenemd_autoptr(__scene_md); 00137 if ((st = __user_gen->GetUserPixels(0, *__scene_md)) != XN_STATUS_OK) { 00138 throw Exception("Failed to get scene meta data (%s)", xnGetStatusString(st)); 00139 } 00140 00141 st = __user_gen->RegisterUserCallbacks(cb_new_user, cb_lost_user, 00142 this, __user_cb_handle); 00143 if (st != XN_STATUS_OK) { 00144 throw Exception("Failed to register user callbacks (%s)", 00145 xnGetStatusString(st)); 00146 } 00147 00148 __skelcap = new xn::SkeletonCapability(__user_gen->GetSkeletonCap()); 00149 00150 #if XN_VERSION_GE(1,3,2,0) 00151 st = __skelcap->RegisterToCalibrationStart(cb_calibration_start, 00152 this, __calib_start_cb_handle); 00153 if (st != XN_STATUS_OK) { 00154 throw Exception("Failed to register calibration start event (%s)", 00155 xnGetStatusString(st)); 00156 } 00157 st = __skelcap->RegisterToCalibrationComplete(cb_calibration_complete, 00158 this, __calib_complete_cb_handle); 00159 #else 00160 st = __skelcap->RegisterCalibrationCallbacks(cb_calibration_start, 00161 cb_calibration_end, 00162 this, __calib_cb_handle); 00163 #endif 00164 00165 if (st != XN_STATUS_OK) { 00166 throw Exception("Failed to register calibration callback (%s)", 00167 xnGetStatusString(st)); 00168 } 00169 00170 __skel_need_calib_pose = __skelcap->NeedPoseForCalibration(); 00171 00172 if (__skel_need_calib_pose) { 00173 if (! __user_gen->IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { 00174 throw Exception("Calibration requires pose, but not supported by node"); 00175 } 00176 __skelcap->GetCalibrationPose(__calib_pose_name); 00177 00178 xn::PoseDetectionCapability posecap = __user_gen->GetPoseDetectionCap(); 00179 00180 #if XN_VERSION_GE(1,3,2,0) 00181 st = posecap.RegisterToPoseDetected(cb_pose_start, 00182 this, __pose_start_cb_handle); 00183 if (st != XN_STATUS_OK) { 00184 throw Exception("Failed to register pose detect event (%s)", 00185 xnGetStatusString(st)); 00186 } 00187 st = posecap.RegisterToOutOfPose(cb_pose_end, 00188 this, __pose_end_cb_handle); 00189 #else 00190 st = posecap.RegisterToPoseCallbacks(cb_pose_start, cb_pose_end, 00191 this, __pose_cb_handle); 00192 #endif 00193 if (st != XN_STATUS_OK) { 00194 throw Exception("Failed to register pose callbacks (%s)", xnGetStatusString(st)); 00195 } 00196 } 00197 00198 __skelcap->SetSkeletonProfile(XN_SKEL_PROFILE_ALL); 00199 00200 __depth_gen->StartGenerating(); 00201 __user_gen->StartGenerating(); 00202 00203 __label_buf = new SharedMemoryImageBuffer("openni-labels", RAW16, 00204 __scene_md->XRes(), 00205 __scene_md->YRes()); 00206 __label_bufsize = colorspace_buffer_size(RAW16, 00207 __scene_md->XRes(), __scene_md->YRes()); 00208 00209 usergen_autoptr.release(); 00210 depthgen_autoptr.release(); 00211 scenemd_autoptr.release(); 00212 } 00213 00214 00215 void 00216 OpenNiUserTrackerThread::finalize() 00217 { 00218 // we do not stop generating, we don't know if there is no other plugin 00219 // using the node. 00220 delete __user_gen; 00221 delete __scene_md; 00222 delete __skelcap; 00223 delete __label_buf; 00224 00225 UserMap::iterator i; 00226 for (i = __users.begin(); i != __users.end(); ++i) { 00227 blackboard->close(i->second.skel_if); 00228 blackboard->close(i->second.proj_if); 00229 } 00230 } 00231 00232 00233 void 00234 OpenNiUserTrackerThread::loop() 00235 { 00236 // we do not lock here, we are only operating on our user generator copy 00237 // and the update happens in a different main loop hook 00238 00239 if (! __user_gen->IsDataNew()) return; 00240 00241 UserMap::iterator i; 00242 for (i = __users.begin(); i != __users.end(); ++i) { 00243 00244 if (!i->second.valid) continue; 00245 00246 bool needs_write = false; 00247 00248 HumanSkeletonInterface::State new_state = i->second.skel_if->state(); 00249 if (__skelcap->IsTracking(i->first)) { 00250 new_state = HumanSkeletonInterface::STATE_TRACKING; 00251 } else if (__skelcap->IsCalibrating(i->first)) { 00252 new_state = HumanSkeletonInterface::STATE_CALIBRATING; 00253 } else { 00254 new_state = HumanSkeletonInterface::STATE_DETECTING_POSE; 00255 } 00256 00257 if (new_state != i->second.skel_if->state()) { 00258 i->second.skel_if->set_state(new_state); 00259 needs_write = true; 00260 } 00261 00262 if (new_state == HumanSkeletonInterface::STATE_TRACKING) { 00263 // update skeleton information 00264 try { 00265 update_user(i->first, i->second); 00266 update_com(i->first, i->second); 00267 needs_write = true; 00268 } catch (Exception &e) { 00269 logger->log_warn(name(), "Failed to update skeleton data for %u, " 00270 "exception follows", i->first); 00271 logger->log_warn(name(), e); 00272 } 00273 } else if (new_state == HumanSkeletonInterface::STATE_DETECTING_POSE) { 00274 update_com(i->first, i->second); 00275 needs_write = true; 00276 } else if (new_state == HumanSkeletonInterface::STATE_CALIBRATING) { 00277 update_com(i->first, i->second); 00278 needs_write = true; 00279 } 00280 00281 if (needs_write) { 00282 i->second.skel_if->write(); 00283 i->second.proj_if->write(); 00284 } 00285 } 00286 00287 if (__label_buf->num_attached() > 1) { 00288 memcpy(__label_buf->buffer(), __scene_md->Data(), __label_bufsize); 00289 } 00290 00291 } 00292 00293 00294 00295 00296 // Very noisy when added to st != XN_STATUS_OK case 00297 //logger->log_warn(name(), "Failed to get joint transformation for " 00298 // "%s joint (%s)", 00299 // joint_name, xnGetStatusString(st)); 00300 00301 00302 // change from mm to m 00303 // translating to Fawkes coordinates, empirically verified 00304 // permute ori columns to match our coordinate system, empirically verified 00305 #define SET_JTF(id, joint, joint_name, bbfield) \ 00306 st = __skelcap->GetSkeletonJoint(id, joint, jtf); \ 00307 if (st != XN_STATUS_OK) { \ 00308 ori[0] = ori[1] = ori[2] = ori[3] = ori[4] = ori[5] = 0.; \ 00309 ori[6] = ori[7] = ori[8] = ori_confidence = pos_confidence = 0.; \ 00310 proj[0] = proj[1] = 0; \ 00311 } else { \ 00312 pos[0] = jtf.position.position.Z * 0.001; \ 00313 pos[1] = -jtf.position.position.X * 0.001; \ 00314 pos[2] = jtf.position.position.Y * 0.001; \ 00315 pos_confidence = jtf.position.fConfidence; \ 00316 \ 00317 ori[0] = jtf.orientation.orientation.elements[2]; \ 00318 ori[1] = -jtf.orientation.orientation.elements[0]; \ 00319 ori[2] = jtf.orientation.orientation.elements[1]; \ 00320 ori[3] = jtf.orientation.orientation.elements[5]; \ 00321 ori[4] = -jtf.orientation.orientation.elements[3]; \ 00322 ori[5] = jtf.orientation.orientation.elements[4]; \ 00323 ori[6] = jtf.orientation.orientation.elements[8]; \ 00324 ori[7] = -jtf.orientation.orientation.elements[6]; \ 00325 ori[8] = jtf.orientation.orientation.elements[7]; \ 00326 ori_confidence = jtf.orientation.fConfidence; \ 00327 \ 00328 XnPoint3D pt; \ 00329 pt = jtf.position.position; \ 00330 __depth_gen->ConvertRealWorldToProjective(1, &pt, &pt); \ 00331 proj[0] = pt.X; \ 00332 proj[1] = pt.Y; \ 00333 } \ 00334 user.skel_if->set_pos_##bbfield(pos); \ 00335 user.skel_if->set_pos_##bbfield##_confidence(pos_confidence); \ 00336 user.skel_if->set_ori_##bbfield(ori); \ 00337 user.skel_if->set_ori_##bbfield##_confidence(ori_confidence); \ 00338 \ 00339 user.proj_if->set_proj_##bbfield(proj); 00340 00341 00342 00343 void 00344 OpenNiUserTrackerThread::update_user(XnUserID id, UserInfo &user) 00345 { 00346 XnSkeletonJointTransformation jtf; 00347 XnStatus st; 00348 00349 float pos[3], ori[9], proj[2], pos_confidence, ori_confidence; 00350 00351 SET_JTF(id, XN_SKEL_HEAD, "head", head); 00352 SET_JTF(id, XN_SKEL_NECK, "neck", neck); 00353 SET_JTF(id, XN_SKEL_TORSO, "torso", torso); 00354 SET_JTF(id, XN_SKEL_WAIST, "waist", waist); 00355 SET_JTF(id, XN_SKEL_LEFT_COLLAR, "left collar", left_collar); 00356 SET_JTF(id, XN_SKEL_LEFT_SHOULDER, "left shoulder", left_shoulder); 00357 SET_JTF(id, XN_SKEL_LEFT_ELBOW, "left elbow", left_elbow); 00358 SET_JTF(id, XN_SKEL_LEFT_WRIST, "left wrist", left_wrist); 00359 SET_JTF(id, XN_SKEL_LEFT_HAND, "left hand", left_hand); 00360 SET_JTF(id, XN_SKEL_LEFT_FINGERTIP, "left finger tip", left_fingertip); 00361 SET_JTF(id, XN_SKEL_RIGHT_COLLAR, "right collar", right_collar); 00362 SET_JTF(id, XN_SKEL_RIGHT_SHOULDER, "right shoulder", right_shoulder); 00363 SET_JTF(id, XN_SKEL_RIGHT_ELBOW, "right elbow", right_elbow); 00364 SET_JTF(id, XN_SKEL_RIGHT_WRIST, "right wrist", right_wrist); 00365 SET_JTF(id, XN_SKEL_RIGHT_HAND, "right hand", right_hand); 00366 SET_JTF(id, XN_SKEL_RIGHT_FINGERTIP, "right finger tip", right_fingertip); 00367 SET_JTF(id, XN_SKEL_LEFT_HIP, "left hip", left_hip); 00368 SET_JTF(id, XN_SKEL_LEFT_KNEE, "left knee", left_knee); 00369 SET_JTF(id, XN_SKEL_LEFT_ANKLE, "left ankle", left_ankle); 00370 SET_JTF(id, XN_SKEL_LEFT_FOOT, "left foot", left_foot); 00371 SET_JTF(id, XN_SKEL_RIGHT_HIP, "right hip", right_hip); 00372 SET_JTF(id, XN_SKEL_RIGHT_KNEE, "right knee", right_knee); 00373 SET_JTF(id, XN_SKEL_RIGHT_ANKLE, "right ankle", right_ankle); 00374 SET_JTF(id, XN_SKEL_RIGHT_FOOT, "right foot", right_foot); 00375 00376 } 00377 00378 00379 void 00380 OpenNiUserTrackerThread::update_com(XnUserID id, UserInfo &user) 00381 { 00382 XnPoint3D compt, compt_proj; 00383 XnStatus st; 00384 float com[3], com_proj[2]; 00385 com[0] = com[1] = com[2] = com_proj[0] = com_proj[1] = 0.; 00386 if ((st = __user_gen->GetCoM(id, compt)) == XN_STATUS_OK) { 00387 00388 // translating to Fawkes coordinates, empirically verified 00389 com[0] = compt.Z * 0.001; 00390 com[1] = -compt.X * 0.001; 00391 com[2] = compt.Y * 0.001; 00392 00393 __depth_gen->ConvertRealWorldToProjective(1, &compt, &compt_proj); 00394 com_proj[0] = compt_proj.X; 00395 com_proj[1] = compt_proj.Y; 00396 } else { 00397 logger->log_warn(name(), "GetCoM failed: %s", xnGetStatusString(st)); 00398 } 00399 00400 user.skel_if->set_com(com); 00401 user.proj_if->set_proj_com(com_proj); 00402 00403 int current_vishist = user.skel_if->visibility_history(); 00404 if ((com[0] == 0.) && (com[1] == 0.) && (com[2] == 0.)) { 00405 if (current_vishist < 0) { 00406 user.skel_if->set_visibility_history(--current_vishist); 00407 } else { 00408 user.skel_if->set_visibility_history(-1); 00409 } 00410 } else { 00411 if (current_vishist > 0) { 00412 user.skel_if->set_visibility_history(++current_vishist); 00413 } else { 00414 user.skel_if->set_visibility_history(1); 00415 } 00416 } 00417 } 00418 00419 /** Notify of new user. 00420 * This is called by the OpenNI callback when a new user has been detected. 00421 * @param id new user's ID 00422 */ 00423 void 00424 OpenNiUserTrackerThread::new_user(XnUserID id) 00425 { 00426 if (__users.find(id) != __users.end()) { 00427 logger->log_error(name(), "New user ID %u, interface already exists", id); 00428 } else { 00429 char *ifid; 00430 if (asprintf(&ifid, "OpenNI Human %u", id) == -1) { 00431 logger->log_warn(name(), "New user ID %u, but cannot generate " 00432 "interface ID", id); 00433 return; 00434 } 00435 try { 00436 logger->log_debug(name(), "Opening interface 'HumanSkeletonInterface::%s'", ifid); 00437 __users[id].skel_if = blackboard->open_for_writing<HumanSkeletonInterface>(ifid); 00438 __users[id].skel_if->set_user_id(id); 00439 __users[id].skel_if->write(); 00440 } catch (Exception &e) { 00441 logger->log_warn(name(), "Failed to open interface, exception follows"); 00442 logger->log_warn(name(), e); 00443 } 00444 00445 try { 00446 logger->log_debug(name(), "Opening interface 'HumanSkeletonProjectionInterface::%s'", ifid); 00447 __users[id].proj_if = blackboard->open_for_writing<HumanSkeletonProjectionInterface>(ifid); 00448 XnFieldOfView fov; 00449 XnStatus st; 00450 if ((st = __depth_gen->GetFieldOfView(fov)) != XN_STATUS_OK) { 00451 logger->log_error(name(), "Failed to get field of view, ignoring. (%s)", 00452 xnGetStatusString(st)); 00453 } else { 00454 __users[id].proj_if->set_horizontal_fov(fov.fHFOV); 00455 __users[id].proj_if->set_vertical_fov(fov.fVFOV); 00456 } 00457 00458 xn::DepthMetaData dmd; 00459 __depth_gen->GetMetaData(dmd); 00460 __users[id].proj_if->set_res_x(dmd.XRes()); 00461 __users[id].proj_if->set_res_y(dmd.YRes()); 00462 __users[id].proj_if->set_max_depth(__depth_gen->GetDeviceMaxDepth()); 00463 __users[id].proj_if->write(); 00464 } catch (Exception &e) { 00465 blackboard->close(__users[id].proj_if); 00466 __users.erase(id); 00467 logger->log_warn(name(), "Failed to open interface, exception follows"); 00468 logger->log_warn(name(), e); 00469 } 00470 00471 free(ifid); 00472 } 00473 00474 __users[id].valid = true; 00475 00476 if (__skel_need_calib_pose) { 00477 __user_gen->GetPoseDetectionCap().StartPoseDetection(__calib_pose_name, id); 00478 } else { 00479 __user_gen->GetSkeletonCap().RequestCalibration(id, TRUE); 00480 } 00481 } 00482 00483 00484 /** Notify of lost user. 00485 * This is called by the OpenNI callback when a user has been lost, 00486 * i.e. it has not been visible for some time. 00487 * @param id lost user's ID 00488 */ 00489 void 00490 OpenNiUserTrackerThread::lost_user(XnUserID id) 00491 { 00492 if (__users.find(id) == __users.end()) { 00493 logger->log_error(name(), "Lost user ID %u, but interface does not exist", id); 00494 return; 00495 } 00496 00497 logger->log_error(name(), "Lost user ID %u, setting interface '%s' to invalid", 00498 id, __users[id].skel_if->uid()); 00499 // write invalid, a reader might still be open 00500 __users[id].skel_if->set_state(HumanSkeletonInterface::STATE_INVALID); 00501 __users[id].skel_if->write(); 00502 __users[id].valid = false; 00503 //blackboard->close(__users[id].skel_if); 00504 //blackboard->close(__users[id].proj_if); 00505 //__users.erase(id); 00506 } 00507 00508 00509 /** Notify of detected pose. 00510 * This is called if a pose has been detected. 00511 * @param id ID of user who is in the pose 00512 * @param pose_name name of the detected pose 00513 */ 00514 void 00515 OpenNiUserTrackerThread::pose_start(XnUserID id, const char *pose_name) 00516 { 00517 if (__users.find(id) == __users.end()) { 00518 logger->log_error(name(), "Pose start for user ID %u, " 00519 "but interface does not exist", id); 00520 return; 00521 } 00522 00523 logger->log_info(name(), "Pose %s detected for user %u", pose_name, id); 00524 00525 __users[id].skel_if->set_pose(pose_name); 00526 __user_gen->GetPoseDetectionCap().StopPoseDetection(id); 00527 __user_gen->GetSkeletonCap().RequestCalibration(id, TRUE); 00528 } 00529 00530 /** Notify of pose detection end. 00531 * This is called if a pose is no longer detected. The NITE middleware seems 00532 * not to call this. 00533 * @param id ID of user who is in the pose 00534 * @param pose_name name of the no longer detected pose 00535 */ 00536 void 00537 OpenNiUserTrackerThread::pose_end(XnUserID id, const char *pose_name) 00538 { 00539 if (__users.find(id) == __users.end()) { 00540 logger->log_error(name(), "Pose end for user ID %u, " 00541 "but interface does not exist", id); 00542 return; 00543 } 00544 00545 __users[id].skel_if->set_pose(""); 00546 } 00547 00548 /** Notify of calibration start. 00549 * This is called when tracking for a user has been started. 00550 * @param id ID of user who is being calibrated. 00551 */ 00552 void 00553 OpenNiUserTrackerThread::calibration_start(XnUserID id) 00554 { 00555 if (__users.find(id) == __users.end()) { 00556 logger->log_error(name(), "Pose end for user ID %u, " 00557 "but interface does not exist", id); 00558 return; 00559 } 00560 00561 logger->log_info(name(), "Calibration started for user %u", id); 00562 } 00563 00564 00565 /** Notify of calibration end. 00566 * This is called when tracking for a user has finished. 00567 * @param id ID of user who was being calibrated 00568 * @param success true if the calibration was successful, false otherwise 00569 */ 00570 void 00571 OpenNiUserTrackerThread::calibration_end(XnUserID id, bool success) 00572 { 00573 if (__users.find(id) == __users.end()) { 00574 logger->log_error(name(), "Pose end for user ID %u, " 00575 "but interface does not exist", id); 00576 return; 00577 } 00578 00579 __users[id].skel_if->set_pose(""); 00580 00581 if (success) { 00582 logger->log_info(name(), "Calibration successful for user %u, " 00583 "starting tracking", id); 00584 __user_gen->GetSkeletonCap().StartTracking(id); 00585 } else { 00586 logger->log_info(name(), "Calibration failed for user %u, restarting", id); 00587 if (__skel_need_calib_pose) { 00588 __user_gen->GetPoseDetectionCap().StartPoseDetection(__calib_pose_name, id); 00589 } else { 00590 __user_gen->GetSkeletonCap().RequestCalibration(id, TRUE); 00591 } 00592 } 00593 }