23 #include "usertracker_thread.h"
25 #include "utils/setup.h"
27 #include <core/threading/mutex_locker.h>
28 #include <fvutils/ipc/shm_image.h>
29 #include <interfaces/HumanSkeletonInterface.h>
30 #include <interfaces/HumanSkeletonProjectionInterface.h>
35 using namespace firevision;
47 :
Thread(
"OpenNiUserTrackerThread",
Thread::OPMODE_WAITFORWAKEUP),
57 static void XN_CALLBACK_TYPE
58 cb_new_user(xn::UserGenerator &generator, XnUserID
id,
void *cookie)
64 static void XN_CALLBACK_TYPE
65 cb_lost_user(xn::UserGenerator &generator, XnUserID
id,
void *cookie)
71 static void XN_CALLBACK_TYPE
72 cb_pose_start(xn::PoseDetectionCapability &capability,
73 const XnChar * pose_name,
81 static void XN_CALLBACK_TYPE
82 cb_pose_end(xn::PoseDetectionCapability &capability,
83 const XnChar * pose_name,
91 static void XN_CALLBACK_TYPE
92 cb_calibration_start(xn::SkeletonCapability &capability, XnUserID
id,
void *cookie)
98 #if XN_VERSION_GE(1, 3, 2, 0)
99 static void XN_CALLBACK_TYPE
100 cb_calibration_complete(xn::SkeletonCapability &capability,
102 XnCalibrationStatus status,
109 static void XN_CALLBACK_TYPE
110 cb_calibration_end(xn::SkeletonCapability &capability, XnUserID
id, XnBool success,
void *cookie)
122 user_gen_ =
new xn::UserGenerator();
123 std::auto_ptr<xn::UserGenerator> usergen_autoptr(user_gen_);
125 depth_gen_ =
new xn::DepthGenerator();
126 std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(depth_gen_);
130 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_DEPTH, depth_gen_);
131 fawkes::openni::setup_map_generator(*depth_gen_,
config);
132 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_USER, user_gen_);
134 if (!user_gen_->IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
135 throw Exception(
"User generator does not support skeleton capability");
138 scene_md_ =
new xn::SceneMetaData();
139 std::auto_ptr<xn::SceneMetaData> scenemd_autoptr(scene_md_);
140 if ((st = user_gen_->GetUserPixels(0, *scene_md_)) != XN_STATUS_OK) {
141 throw Exception(
"Failed to get scene meta data (%s)", xnGetStatusString(st));
144 st = user_gen_->RegisterUserCallbacks(cb_new_user, cb_lost_user,
this, user_cb_handle_);
145 if (st != XN_STATUS_OK) {
146 throw Exception(
"Failed to register user callbacks (%s)", xnGetStatusString(st));
149 skelcap_ =
new xn::SkeletonCapability(user_gen_->GetSkeletonCap());
151 #if XN_VERSION_GE(1, 3, 2, 0)
152 st = skelcap_->RegisterToCalibrationStart(cb_calibration_start,
this, calib_start_cb_handle_);
153 if (st != XN_STATUS_OK) {
154 throw Exception(
"Failed to register calibration start event (%s)", xnGetStatusString(st));
156 st = skelcap_->RegisterToCalibrationComplete(cb_calibration_complete,
158 calib_complete_cb_handle_);
160 st = skelcap_->RegisterCalibrationCallbacks(cb_calibration_start,
166 if (st != XN_STATUS_OK) {
167 throw Exception(
"Failed to register calibration callback (%s)", xnGetStatusString(st));
170 skel_need_calib_pose_ = skelcap_->NeedPoseForCalibration();
172 if (skel_need_calib_pose_) {
173 if (!user_gen_->IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
174 throw Exception(
"Calibration requires pose, but not supported by node");
176 skelcap_->GetCalibrationPose(calib_pose_name_);
178 xn::PoseDetectionCapability posecap = user_gen_->GetPoseDetectionCap();
180 #if XN_VERSION_GE(1, 3, 2, 0)
181 st = posecap.RegisterToPoseDetected(cb_pose_start,
this, pose_start_cb_handle_);
182 if (st != XN_STATUS_OK) {
183 throw Exception(
"Failed to register pose detect event (%s)", xnGetStatusString(st));
185 st = posecap.RegisterToOutOfPose(cb_pose_end,
this, pose_end_cb_handle_);
187 st = posecap.RegisterToPoseCallbacks(cb_pose_start, cb_pose_end,
this, pose_cb_handle_);
189 if (st != XN_STATUS_OK) {
190 throw Exception(
"Failed to register pose callbacks (%s)", xnGetStatusString(st));
194 skelcap_->SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
196 depth_gen_->StartGenerating();
197 user_gen_->StartGenerating();
201 label_bufsize_ = colorspace_buffer_size(RAW16, scene_md_->XRes(), scene_md_->YRes());
203 usergen_autoptr.release();
204 depthgen_autoptr.release();
205 scenemd_autoptr.release();
219 for (i = users_.begin(); i != users_.end(); ++i) {
231 if (!user_gen_->IsDataNew())
235 for (i = users_.begin(); i != users_.end(); ++i) {
236 if (!i->second.valid)
239 bool needs_write =
false;
242 if (skelcap_->IsTracking(i->first)) {
243 new_state = HumanSkeletonInterface::STATE_TRACKING;
244 }
else if (skelcap_->IsCalibrating(i->first)) {
245 new_state = HumanSkeletonInterface::STATE_CALIBRATING;
247 new_state = HumanSkeletonInterface::STATE_DETECTING_POSE;
250 if (new_state != i->second.skel_if->state()) {
251 i->second.skel_if->set_state(new_state);
255 if (new_state == HumanSkeletonInterface::STATE_TRACKING) {
258 update_user(i->first, i->second);
259 update_com(i->first, i->second);
263 "Failed to update skeleton data for %u, "
268 }
else if (new_state == HumanSkeletonInterface::STATE_DETECTING_POSE) {
269 update_com(i->first, i->second);
271 }
else if (new_state == HumanSkeletonInterface::STATE_CALIBRATING) {
272 update_com(i->first, i->second);
277 i->second.skel_if->write();
278 i->second.proj_if->write();
283 memcpy(label_buf_->
buffer(), scene_md_->Data(), label_bufsize_);
295 #define SET_JTF(id, joint, joint_name, bbfield) \
296 st = skelcap_->GetSkeletonJoint(id, joint, jtf); \
297 if (st != XN_STATUS_OK) { \
298 ori[0] = ori[1] = ori[2] = ori[3] = ori[4] = ori[5] = 0.; \
299 ori[6] = ori[7] = ori[8] = ori_confidence = pos_confidence = 0.; \
300 proj[0] = proj[1] = 0; \
302 pos[0] = jtf.position.position.Z * 0.001; \
303 pos[1] = -jtf.position.position.X * 0.001; \
304 pos[2] = jtf.position.position.Y * 0.001; \
305 pos_confidence = jtf.position.fConfidence; \
307 ori[0] = jtf.orientation.orientation.elements[2]; \
308 ori[1] = -jtf.orientation.orientation.elements[0]; \
309 ori[2] = jtf.orientation.orientation.elements[1]; \
310 ori[3] = jtf.orientation.orientation.elements[5]; \
311 ori[4] = -jtf.orientation.orientation.elements[3]; \
312 ori[5] = jtf.orientation.orientation.elements[4]; \
313 ori[6] = jtf.orientation.orientation.elements[8]; \
314 ori[7] = -jtf.orientation.orientation.elements[6]; \
315 ori[8] = jtf.orientation.orientation.elements[7]; \
316 ori_confidence = jtf.orientation.fConfidence; \
319 pt = jtf.position.position; \
320 depth_gen_->ConvertRealWorldToProjective(1, &pt, &pt); \
324 user.skel_if->set_pos_##bbfield(pos); \
325 user.skel_if->set_pos_##bbfield##_confidence(pos_confidence); \
326 user.skel_if->set_ori_##bbfield(ori); \
327 user.skel_if->set_ori_##bbfield##_confidence(ori_confidence); \
329 user.proj_if->set_proj_##bbfield(proj);
332 OpenNiUserTrackerThread::update_user(XnUserID
id, UserInfo &user)
334 XnSkeletonJointTransformation jtf;
337 float pos[3], ori[9], proj[2], pos_confidence, ori_confidence;
339 SET_JTF(
id, XN_SKEL_HEAD,
"head", head);
340 SET_JTF(
id, XN_SKEL_NECK,
"neck", neck);
341 SET_JTF(
id, XN_SKEL_TORSO,
"torso", torso);
342 SET_JTF(
id, XN_SKEL_WAIST,
"waist", waist);
343 SET_JTF(
id, XN_SKEL_LEFT_COLLAR,
"left collar", left_collar);
344 SET_JTF(
id, XN_SKEL_LEFT_SHOULDER,
"left shoulder", left_shoulder);
345 SET_JTF(
id, XN_SKEL_LEFT_ELBOW,
"left elbow", left_elbow);
346 SET_JTF(
id, XN_SKEL_LEFT_WRIST,
"left wrist", left_wrist);
347 SET_JTF(
id, XN_SKEL_LEFT_HAND,
"left hand", left_hand);
348 SET_JTF(
id, XN_SKEL_LEFT_FINGERTIP,
"left finger tip", left_fingertip);
349 SET_JTF(
id, XN_SKEL_RIGHT_COLLAR,
"right collar", right_collar);
350 SET_JTF(
id, XN_SKEL_RIGHT_SHOULDER,
"right shoulder", right_shoulder);
351 SET_JTF(
id, XN_SKEL_RIGHT_ELBOW,
"right elbow", right_elbow);
352 SET_JTF(
id, XN_SKEL_RIGHT_WRIST,
"right wrist", right_wrist);
353 SET_JTF(
id, XN_SKEL_RIGHT_HAND,
"right hand", right_hand);
354 SET_JTF(
id, XN_SKEL_RIGHT_FINGERTIP,
"right finger tip", right_fingertip);
355 SET_JTF(
id, XN_SKEL_LEFT_HIP,
"left hip", left_hip);
356 SET_JTF(
id, XN_SKEL_LEFT_KNEE,
"left knee", left_knee);
357 SET_JTF(
id, XN_SKEL_LEFT_ANKLE,
"left ankle", left_ankle);
358 SET_JTF(
id, XN_SKEL_LEFT_FOOT,
"left foot", left_foot);
359 SET_JTF(
id, XN_SKEL_RIGHT_HIP,
"right hip", right_hip);
360 SET_JTF(
id, XN_SKEL_RIGHT_KNEE,
"right knee", right_knee);
361 SET_JTF(
id, XN_SKEL_RIGHT_ANKLE,
"right ankle", right_ankle);
362 SET_JTF(
id, XN_SKEL_RIGHT_FOOT,
"right foot", right_foot);
366 OpenNiUserTrackerThread::update_com(XnUserID
id, UserInfo &user)
368 XnPoint3D compt, compt_proj;
370 float com[3], com_proj[2];
371 com[0] = com[1] = com[2] = com_proj[0] = com_proj[1] = 0.;
372 if ((st = user_gen_->GetCoM(
id, compt)) == XN_STATUS_OK) {
374 com[0] = compt.Z * 0.001;
375 com[1] = -compt.X * 0.001;
376 com[2] = compt.Y * 0.001;
378 depth_gen_->ConvertRealWorldToProjective(1, &compt, &compt_proj);
379 com_proj[0] = compt_proj.X;
380 com_proj[1] = compt_proj.Y;
385 user.skel_if->set_com(com);
386 user.proj_if->set_proj_com(com_proj);
388 int current_vishist = user.skel_if->visibility_history();
389 if ((com[0] == 0.) && (com[1] == 0.) && (com[2] == 0.)) {
390 if (current_vishist < 0) {
391 user.skel_if->set_visibility_history(--current_vishist);
393 user.skel_if->set_visibility_history(-1);
396 if (current_vishist > 0) {
397 user.skel_if->set_visibility_history(++current_vishist);
399 user.skel_if->set_visibility_history(1);
411 if (users_.find(
id) != users_.end()) {
415 if (asprintf(&ifid,
"OpenNI Human %u",
id) == -1) {
417 "New user ID %u, but cannot generate "
425 users_[id].skel_if->set_user_id(
id);
426 users_[id].skel_if->write();
433 logger->
log_debug(
name(),
"Opening interface 'HumanSkeletonProjectionInterface::%s'", ifid);
437 if ((st = depth_gen_->GetFieldOfView(fov)) != XN_STATUS_OK) {
439 "Failed to get field of view, ignoring. (%s)",
440 xnGetStatusString(st));
442 users_[id].proj_if->set_horizontal_fov(fov.fHFOV);
443 users_[id].proj_if->set_vertical_fov(fov.fVFOV);
446 xn::DepthMetaData dmd;
447 depth_gen_->GetMetaData(dmd);
448 users_[id].proj_if->set_res_x(dmd.XRes());
449 users_[id].proj_if->set_res_y(dmd.YRes());
450 users_[id].proj_if->set_max_depth(depth_gen_->GetDeviceMaxDepth());
451 users_[id].proj_if->write();
462 users_[id].valid =
true;
464 if (skel_need_calib_pose_) {
465 user_gen_->GetPoseDetectionCap().StartPoseDetection(calib_pose_name_,
id);
467 user_gen_->GetSkeletonCap().RequestCalibration(
id, TRUE);
479 if (users_.find(
id) == users_.end()) {
485 "Lost user ID %u, setting interface '%s' to invalid",
487 users_[
id].skel_if->uid());
489 users_[id].skel_if->set_state(HumanSkeletonInterface::STATE_INVALID);
490 users_[id].skel_if->write();
491 users_[id].valid =
false;
505 if (users_.find(
id) == users_.end()) {
507 "Pose start for user ID %u, "
508 "but interface does not exist",
515 users_[id].skel_if->set_pose(pose_name);
516 user_gen_->GetPoseDetectionCap().StopPoseDetection(
id);
517 user_gen_->GetSkeletonCap().RequestCalibration(
id, TRUE);
529 if (users_.find(
id) == users_.end()) {
531 "Pose end for user ID %u, "
532 "but interface does not exist",
537 users_[id].skel_if->set_pose(
"");
547 if (users_.find(
id) == users_.end()) {
549 "Pose end for user ID %u, "
550 "but interface does not exist",
566 if (users_.find(
id) == users_.end()) {
568 "Pose end for user ID %u, "
569 "but interface does not exist",
574 users_[id].skel_if->set_pose(
"");
578 "Calibration successful for user %u, "
581 user_gen_->GetSkeletonCap().StartTracking(
id);
584 if (skel_need_calib_pose_) {
585 user_gen_->GetPoseDetectionCap().StartPoseDetection(calib_pose_name_,
id);
587 user_gen_->GetSkeletonCap().RequestCalibration(
id, TRUE);