Fawkes API  Fawkes Development Version
usertracker_thread.cpp
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 }