23 #include "usertracker_thread.h"
24 #include "utils/setup.h"
26 #include <core/threading/mutex_locker.h>
27 #include <interfaces/HumanSkeletonInterface.h>
28 #include <interfaces/HumanSkeletonProjectionInterface.h>
29 #include <fvutils/ipc/shm_image.h>
46 :
Thread(
"OpenNiUserTrackerThread",
Thread::OPMODE_WAITFORWAKEUP),
58 static void XN_CALLBACK_TYPE
59 cb_new_user(xn::UserGenerator &generator, XnUserID
id,
void *cookie)
65 static void XN_CALLBACK_TYPE
66 cb_lost_user(xn::UserGenerator &generator, XnUserID
id,
void *cookie)
72 static void XN_CALLBACK_TYPE
73 cb_pose_start(xn::PoseDetectionCapability &capability,
74 const XnChar *pose_name, XnUserID
id,
void* cookie)
80 static void XN_CALLBACK_TYPE
81 cb_pose_end(xn::PoseDetectionCapability &capability,
82 const XnChar *pose_name, XnUserID
id,
void* cookie)
88 static void XN_CALLBACK_TYPE
89 cb_calibration_start(xn::SkeletonCapability &capability, XnUserID
id,
void *cookie)
95 #if XN_VERSION_GE(1,3,2,0)
96 static void XN_CALLBACK_TYPE
97 cb_calibration_complete(xn::SkeletonCapability &capability, XnUserID
id,
98 XnCalibrationStatus status,
void *cookie)
104 static void XN_CALLBACK_TYPE
105 cb_calibration_end(xn::SkeletonCapability &capability, XnUserID
id,
106 XnBool success,
void *cookie)
119 __user_gen =
new xn::UserGenerator();
120 std::auto_ptr<xn::UserGenerator> usergen_autoptr(__user_gen);
122 __depth_gen =
new xn::DepthGenerator();
123 std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
127 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_DEPTH, __depth_gen);
128 fawkes::openni::setup_map_generator(*__depth_gen,
config);
129 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_USER, __user_gen);
131 if (!__user_gen->IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
132 throw Exception(
"User generator does not support skeleton capability");
135 __scene_md =
new xn::SceneMetaData();
136 std::auto_ptr<xn::SceneMetaData> scenemd_autoptr(__scene_md);
137 if ((st = __user_gen->GetUserPixels(0, *__scene_md)) != XN_STATUS_OK) {
138 throw Exception(
"Failed to get scene meta data (%s)", xnGetStatusString(st));
141 st = __user_gen->RegisterUserCallbacks(cb_new_user, cb_lost_user,
142 this, __user_cb_handle);
143 if (st != XN_STATUS_OK) {
144 throw Exception(
"Failed to register user callbacks (%s)",
145 xnGetStatusString(st));
148 __skelcap =
new xn::SkeletonCapability(__user_gen->GetSkeletonCap());
150 #if XN_VERSION_GE(1,3,2,0)
151 st = __skelcap->RegisterToCalibrationStart(cb_calibration_start,
152 this, __calib_start_cb_handle);
153 if (st != XN_STATUS_OK) {
154 throw Exception(
"Failed to register calibration start event (%s)",
155 xnGetStatusString(st));
157 st = __skelcap->RegisterToCalibrationComplete(cb_calibration_complete,
158 this, __calib_complete_cb_handle);
160 st = __skelcap->RegisterCalibrationCallbacks(cb_calibration_start,
162 this, __calib_cb_handle);
165 if (st != XN_STATUS_OK) {
166 throw Exception(
"Failed to register calibration callback (%s)",
167 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,
182 this, __pose_start_cb_handle);
183 if (st != XN_STATUS_OK) {
184 throw Exception(
"Failed to register pose detect event (%s)",
185 xnGetStatusString(st));
187 st = posecap.RegisterToOutOfPose(cb_pose_end,
188 this, __pose_end_cb_handle);
190 st = posecap.RegisterToPoseCallbacks(cb_pose_start, cb_pose_end,
191 this, __pose_cb_handle);
193 if (st != XN_STATUS_OK) {
194 throw Exception(
"Failed to register pose callbacks (%s)", xnGetStatusString(st));
198 __skelcap->SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
200 __depth_gen->StartGenerating();
201 __user_gen->StartGenerating();
206 __label_bufsize = colorspace_buffer_size(RAW16,
207 __scene_md->XRes(), __scene_md->YRes());
209 usergen_autoptr.release();
210 depthgen_autoptr.release();
211 scenemd_autoptr.release();
226 for (i = __users.begin(); i != __users.end(); ++i) {
239 if (! __user_gen->IsDataNew())
return;
242 for (i = __users.begin(); i != __users.end(); ++i) {
244 if (!i->second.valid)
continue;
246 bool needs_write =
false;
249 if (__skelcap->IsTracking(i->first)) {
250 new_state = HumanSkeletonInterface::STATE_TRACKING;
251 }
else if (__skelcap->IsCalibrating(i->first)) {
252 new_state = HumanSkeletonInterface::STATE_CALIBRATING;
254 new_state = HumanSkeletonInterface::STATE_DETECTING_POSE;
257 if (new_state != i->second.skel_if->state()) {
258 i->second.skel_if->set_state(new_state);
262 if (new_state == HumanSkeletonInterface::STATE_TRACKING) {
265 update_user(i->first, i->second);
266 update_com(i->first, i->second);
270 "exception follows", i->first);
273 }
else if (new_state == HumanSkeletonInterface::STATE_DETECTING_POSE) {
274 update_com(i->first, i->second);
276 }
else if (new_state == HumanSkeletonInterface::STATE_CALIBRATING) {
277 update_com(i->first, i->second);
282 i->second.skel_if->write();
283 i->second.proj_if->write();
288 memcpy(__label_buf->
buffer(), __scene_md->Data(), __label_bufsize);
305 #define SET_JTF(id, joint, joint_name, bbfield) \
306 st = __skelcap->GetSkeletonJoint(id, joint, jtf); \
307 if (st != XN_STATUS_OK) { \
308 ori[0] = ori[1] = ori[2] = ori[3] = ori[4] = ori[5] = 0.; \
309 ori[6] = ori[7] = ori[8] = ori_confidence = pos_confidence = 0.; \
310 proj[0] = proj[1] = 0; \
312 pos[0] = jtf.position.position.Z * 0.001; \
313 pos[1] = -jtf.position.position.X * 0.001; \
314 pos[2] = jtf.position.position.Y * 0.001; \
315 pos_confidence = jtf.position.fConfidence; \
317 ori[0] = jtf.orientation.orientation.elements[2]; \
318 ori[1] = -jtf.orientation.orientation.elements[0]; \
319 ori[2] = jtf.orientation.orientation.elements[1]; \
320 ori[3] = jtf.orientation.orientation.elements[5]; \
321 ori[4] = -jtf.orientation.orientation.elements[3]; \
322 ori[5] = jtf.orientation.orientation.elements[4]; \
323 ori[6] = jtf.orientation.orientation.elements[8]; \
324 ori[7] = -jtf.orientation.orientation.elements[6]; \
325 ori[8] = jtf.orientation.orientation.elements[7]; \
326 ori_confidence = jtf.orientation.fConfidence; \
329 pt = jtf.position.position; \
330 __depth_gen->ConvertRealWorldToProjective(1, &pt, &pt); \
334 user.skel_if->set_pos_##bbfield(pos); \
335 user.skel_if->set_pos_##bbfield##_confidence(pos_confidence); \
336 user.skel_if->set_ori_##bbfield(ori); \
337 user.skel_if->set_ori_##bbfield##_confidence(ori_confidence); \
339 user.proj_if->set_proj_##bbfield(proj);
344 OpenNiUserTrackerThread::update_user(XnUserID
id, UserInfo &user)
346 XnSkeletonJointTransformation jtf;
349 float pos[3], ori[9], proj[2], pos_confidence, ori_confidence;
351 SET_JTF(
id, XN_SKEL_HEAD,
"head", head);
352 SET_JTF(
id, XN_SKEL_NECK,
"neck", neck);
353 SET_JTF(
id, XN_SKEL_TORSO,
"torso", torso);
354 SET_JTF(
id, XN_SKEL_WAIST,
"waist", waist);
355 SET_JTF(
id, XN_SKEL_LEFT_COLLAR,
"left collar", left_collar);
356 SET_JTF(
id, XN_SKEL_LEFT_SHOULDER,
"left shoulder", left_shoulder);
357 SET_JTF(
id, XN_SKEL_LEFT_ELBOW,
"left elbow", left_elbow);
358 SET_JTF(
id, XN_SKEL_LEFT_WRIST,
"left wrist", left_wrist);
359 SET_JTF(
id, XN_SKEL_LEFT_HAND,
"left hand", left_hand);
360 SET_JTF(
id, XN_SKEL_LEFT_FINGERTIP,
"left finger tip", left_fingertip);
361 SET_JTF(
id, XN_SKEL_RIGHT_COLLAR,
"right collar", right_collar);
362 SET_JTF(
id, XN_SKEL_RIGHT_SHOULDER,
"right shoulder", right_shoulder);
363 SET_JTF(
id, XN_SKEL_RIGHT_ELBOW,
"right elbow", right_elbow);
364 SET_JTF(
id, XN_SKEL_RIGHT_WRIST,
"right wrist", right_wrist);
365 SET_JTF(
id, XN_SKEL_RIGHT_HAND,
"right hand", right_hand);
366 SET_JTF(
id, XN_SKEL_RIGHT_FINGERTIP,
"right finger tip", right_fingertip);
367 SET_JTF(
id, XN_SKEL_LEFT_HIP,
"left hip", left_hip);
368 SET_JTF(
id, XN_SKEL_LEFT_KNEE,
"left knee", left_knee);
369 SET_JTF(
id, XN_SKEL_LEFT_ANKLE,
"left ankle", left_ankle);
370 SET_JTF(
id, XN_SKEL_LEFT_FOOT,
"left foot", left_foot);
371 SET_JTF(
id, XN_SKEL_RIGHT_HIP,
"right hip", right_hip);
372 SET_JTF(
id, XN_SKEL_RIGHT_KNEE,
"right knee", right_knee);
373 SET_JTF(
id, XN_SKEL_RIGHT_ANKLE,
"right ankle", right_ankle);
374 SET_JTF(
id, XN_SKEL_RIGHT_FOOT,
"right foot", right_foot);
380 OpenNiUserTrackerThread::update_com(XnUserID
id, UserInfo &user)
382 XnPoint3D compt, compt_proj;
384 float com[3], com_proj[2];
385 com[0] = com[1] = com[2] = com_proj[0] = com_proj[1] = 0.;
386 if ((st = __user_gen->GetCoM(
id, compt)) == XN_STATUS_OK) {
389 com[0] = compt.Z * 0.001;
390 com[1] = -compt.X * 0.001;
391 com[2] = compt.Y * 0.001;
393 __depth_gen->ConvertRealWorldToProjective(1, &compt, &compt_proj);
394 com_proj[0] = compt_proj.X;
395 com_proj[1] = compt_proj.Y;
400 user.skel_if->set_com(com);
401 user.proj_if->set_proj_com(com_proj);
403 int current_vishist = user.skel_if->visibility_history();
404 if ((com[0] == 0.) && (com[1] == 0.) && (com[2] == 0.)) {
405 if (current_vishist < 0) {
406 user.skel_if->set_visibility_history(--current_vishist);
408 user.skel_if->set_visibility_history(-1);
411 if (current_vishist > 0) {
412 user.skel_if->set_visibility_history(++current_vishist);
414 user.skel_if->set_visibility_history(1);
426 if (__users.find(
id) != __users.end()) {
430 if (asprintf(&ifid,
"OpenNI Human %u",
id) == -1) {
438 __users[id].skel_if->set_user_id(
id);
439 __users[id].skel_if->write();
446 logger->
log_debug(
name(),
"Opening interface 'HumanSkeletonProjectionInterface::%s'", ifid);
450 if ((st = __depth_gen->GetFieldOfView(fov)) != XN_STATUS_OK) {
452 xnGetStatusString(st));
454 __users[id].proj_if->set_horizontal_fov(fov.fHFOV);
455 __users[id].proj_if->set_vertical_fov(fov.fVFOV);
458 xn::DepthMetaData dmd;
459 __depth_gen->GetMetaData(dmd);
460 __users[id].proj_if->set_res_x(dmd.XRes());
461 __users[id].proj_if->set_res_y(dmd.YRes());
462 __users[id].proj_if->set_max_depth(__depth_gen->GetDeviceMaxDepth());
463 __users[id].proj_if->write();
474 __users[id].valid =
true;
476 if (__skel_need_calib_pose) {
477 __user_gen->GetPoseDetectionCap().StartPoseDetection(__calib_pose_name,
id);
479 __user_gen->GetSkeletonCap().RequestCalibration(
id, TRUE);
492 if (__users.find(
id) == __users.end()) {
498 id, __users[
id].skel_if->uid());
500 __users[id].skel_if->set_state(HumanSkeletonInterface::STATE_INVALID);
501 __users[id].skel_if->write();
502 __users[id].valid =
false;
517 if (__users.find(
id) == __users.end()) {
519 "but interface does not exist",
id);
525 __users[id].skel_if->set_pose(pose_name);
526 __user_gen->GetPoseDetectionCap().StopPoseDetection(
id);
527 __user_gen->GetSkeletonCap().RequestCalibration(
id, TRUE);
539 if (__users.find(
id) == __users.end()) {
541 "but interface does not exist",
id);
545 __users[id].skel_if->set_pose(
"");
555 if (__users.find(
id) == __users.end()) {
557 "but interface does not exist",
id);
573 if (__users.find(
id) == __users.end()) {
575 "but interface does not exist",
id);
579 __users[id].skel_if->set_pose(
"");
583 "starting tracking",
id);
584 __user_gen->GetSkeletonCap().StartTracking(
id);
587 if (__skel_need_calib_pose) {
588 __user_gen->GetPoseDetectionCap().StartPoseDetection(__calib_pose_name,
id);
590 __user_gen->GetSkeletonCap().RequestCalibration(
id, TRUE);
LockPtr< xn::Context > openni
Central OpenNI context.
HumanSkeletonProjectionInterface Fawkes BlackBoard Interface.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual ~OpenNiUserTrackerThread()
Destructor.
void new_user(XnUserID id)
Notify of new user.
Fawkes library namespace.
Mutex * objmutex_ptr() const
Get object mutex.
Thread class encapsulation of pthreads.
void calibration_end(XnUserID id, bool success)
Notify of calibration end.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
void pose_end(XnUserID id, const char *pose_name)
Notify of pose detection end.
virtual void init()
Initialize the thread.
State
Current tracking state for the skeleton.
Thread aspect to use blocked timing.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
OpenNiUserTrackerThread()
Constructor.
Shared memory image buffer.
unsigned char * buffer() const
Get image buffer.
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 loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
void lost_user(XnUserID id)
Notify of lost user.
unsigned int num_attached() const
Get number of attached processes.
OpenNI User Tracker Thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
HumanSkeletonInterface Fawkes BlackBoard Interface.
void pose_start(XnUserID id, const char *pose_name)
Notify of detected pose.
void calibration_start(XnUserID id)
Notify of calibration start.
Configuration * config
This is the Configuration member used to access the configuration.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.