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);
OpenNI User Tracker Thread.
void calibration_end(XnUserID id, bool success)
Notify of calibration end.
virtual void init()
Initialize the thread.
void new_user(XnUserID id)
Notify of new user.
virtual ~OpenNiUserTrackerThread()
Destructor.
virtual void loop()
Code to execute in the thread.
void pose_end(XnUserID id, const char *pose_name)
Notify of pose detection end.
OpenNiUserTrackerThread()
Constructor.
void pose_start(XnUserID id, const char *pose_name)
Notify of detected pose.
void calibration_start(XnUserID id)
Notify of calibration start.
void lost_user(XnUserID id)
Notify of lost user.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Base class for exceptions in Fawkes.
HumanSkeletonInterface Fawkes BlackBoard Interface.
State
Current tracking state for the skeleton.
HumanSkeletonProjectionInterface Fawkes BlackBoard Interface.
Mutex * objmutex_ptr() const
Get object mutex.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
LockPtr< xn::Context > openni
Central OpenNI context.
unsigned int num_attached() const
Get number of attached processes.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Shared memory image buffer.
unsigned char * buffer() const
Get image buffer.
Fawkes library namespace.