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>
33 using namespace fawkes;
34 using namespace firevision;
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);