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.
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.
void pose_end(XnUserID id, const char *pose_name)
Notify of pose detection end.
virtual void init()
Initialize the thread.
unsigned char * buffer() const
Get image buffer.
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.
Mutex * objmutex_ptr() const
Get object mutex.
OpenNiUserTrackerThread()
Constructor.
Shared memory image buffer.
unsigned int num_attached() const
Get number of attached processes.
const char * name() const
Get name of thread.
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.
void lost_user(XnUserID id)
Notify of lost user.
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.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.