Main MRPT website > C++ reference for MRPT 1.9.9
CSkeletonTracker.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/hwdrivers/CSkeletonTracker.h>
13 
14 // opengl includes
15 #include <mrpt/opengl/CSphere.h>
16 #include <mrpt/opengl/CSimpleLine.h>
17 #include <mrpt/opengl/CGridPlaneXZ.h>
18 #include <mrpt/opengl/CSetOfLines.h>
19 #include <mrpt/opengl/CBox.h>
20 #include <mrpt/opengl/CText.h>
21 #include <mrpt/opengl/CCylinder.h>
22 #include <mrpt/opengl/CTexturedPlane.h>
23 #include <mrpt/opengl/stock_objects.h>
24 #include <mrpt/poses/CPoint3D.h>
25 
26 IMPLEMENTS_GENERIC_SENSOR(CSkeletonTracker, mrpt::hwdrivers)
27 
28 using namespace mrpt::hwdrivers;
29 using namespace mrpt::poses;
30 using namespace mrpt::obs;
31 using namespace mrpt::img;
32 using namespace std;
33 
34 #define skl_states (static_cast<nite::SkeletonState*>(m_skeletons_ptr))
35 #define user_tracker (static_cast<nite::UserTracker*>(m_userTracker_ptr))
36 #define MAX_USERS 10
37 #define FILL_JOINT_DATA(_J1, _J2) \
38  obs->_J1.x = user.getSkeleton().getJoint(_J2).getPosition().x; \
39  obs->_J1.y = user.getSkeleton().getJoint(_J2).getPosition().y; \
40  obs->_J1.z = user.getSkeleton().getJoint(_J2).getPosition().z; \
41  obs->_J1.conf = user.getSkeleton().getJoint(_J2).getPositionConfidence();
42 
43 #if MRPT_HAS_NITE2
44 #include "NiTE.h"
45 #pragma comment(lib, "NiTE2.lib")
46 #endif
47 
48 string jointNames[] = {
49  "head", "neck", "torso", "left_shoulder", "left_elbow",
50  "left_hand", "left_hip", "left_knee", "left_foot", "right_shoulder",
51  "right_elbow", "right_hand", "right_hip", "right_knee", "right_foot"};
52 
53 /*-------------------------------------------------------------
54  CSkeletonTracker
55 -------------------------------------------------------------*/
56 CSkeletonTracker::CSkeletonTracker()
57  : m_skeletons_ptr(nullptr),
58  m_userTracker_ptr(nullptr),
59  m_timeStartUI(0),
60  m_timeStartTT(0),
61  m_sensorPose(),
62  m_nUsers(0),
63  m_showPreview(false),
64  m_toutCounter(0)
65 {
66  m_sensorLabel = "skeletonTracker";
67 
68 #if MRPT_HAS_OPENNI2 && MRPT_HAS_NITE2
69  m_skeletons_ptr = new nite::SkeletonState[MAX_USERS];
70  m_userTracker_ptr = new nite::UserTracker;
71  for (int i = 0; i < MAX_USERS; ++i) skl_states[i] = nite::SKELETON_NONE;
72 
73  m_linesToPlot.resize(NUM_LINES);
74  m_joint_theta.resize(NUM_JOINTS);
75  for (int i = 1; i < NUM_JOINTS; ++i)
76  m_joint_theta[i] = (i - 1) * (M_2PI / (NUM_JOINTS - 1));
77 #else
78  THROW_EXCEPTION(
79  "MRPT has been compiled with 'BUILD_OPENNI2'=OFF or 'BUILD_NITE2'=OFF, "
80  "so this class cannot be used.");
81 #endif
82 }
83 
84 /*-------------------------------------------------------------
85  ~CSkeletonTracker
86 -------------------------------------------------------------*/
87 CSkeletonTracker::~CSkeletonTracker()
88 {
89 #if MRPT_HAS_OPENNI2 && MRPT_HAS_NITE2
90  nite::NiTE::shutdown(); // close tracker
91  delete[] skl_states;
92  m_skeletons_ptr = nullptr;
93  delete user_tracker;
94  m_userTracker_ptr = nullptr;
95 #endif
96  if (m_win) m_win.reset();
97 }
98 
99 /*-------------------------------------------------------------
100  processPreviewNone
101 -------------------------------------------------------------*/
102 void CSkeletonTracker::processPreviewNone()
103 {
104  using namespace mrpt::opengl;
105 
106  // show skeleton data
107  if (m_showPreview)
108  {
109  if (!m_win)
110  {
111  string caption = string("Preview of ") + m_sensorLabel;
112  m_win = mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow3D>(
113  caption, 800, 600);
114 
115  COpenGLScene::Ptr& scene = m_win->get3DSceneAndLock();
116  scene->insert(
117  mrpt::make_aligned_shared<CGridPlaneXZ>(-3, 3, 0, 5, -1.5));
118 
119  // set camera parameters
120  m_win->setCameraElevationDeg(-90);
121  m_win->setCameraAzimuthDeg(90);
122  m_win->setCameraZoom(4);
123  m_win->setCameraPointingToPoint(0, 0, 0);
124 
125  // insert initial body
126  CSetOfObjects::Ptr body =
127  mrpt::make_aligned_shared<CSetOfObjects>();
128  body->setName("body");
129  for (int i = 0; i < NUM_JOINTS; ++i)
130  {
131  CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.03f);
132  sph->setColor(0, 1, 0);
133  sph->setName(jointNames[i]);
134  body->insert(sph);
135  }
136 
137  // insert initial lines
138  CSetOfLines::Ptr lines = mrpt::make_aligned_shared<CSetOfLines>();
139  lines->setName("lines");
140  lines->setColor(0, 0, 1);
141  body->insert(lines);
142 
143  scene->insert(body);
144  m_win->unlockAccess3DScene();
145  }
146 
147  if (m_win && m_win->isOpen())
148  {
149  COpenGLScene::Ptr& scene = m_win->get3DSceneAndLock();
150  {
151  m_win->addTextMessage(
152  0.35, 0.9, "Please, adopt this position", TColorf(1, 1, 1),
153  "mono", 10, mrpt::opengl::FILL, 0);
154 
155  // insert translucid dummy and help text (it will go away when
156  // measurements are taken)
157  if (!scene->getByName("dummy"))
158  {
159  const double SCALE = 0.8;
160  const double BODY_RADIUS = 0.22 * SCALE;
161  const double BODY_LENGTH = 0.8 * SCALE;
162  const double ARM_RADIUS = 0.05 * SCALE;
163  const double ARM_LENGTH = 0.4 * SCALE;
164  const double LEG_RADIUS = 0.1 * SCALE;
165  const double LEG_LENGTH = 0.8 * SCALE;
166  const double HEAD_RADIUS = 0.15 * SCALE;
167  const double ALPHA_CH = 0.8;
168 
169  CSetOfObjects::Ptr dummy =
170  mrpt::make_aligned_shared<CSetOfObjects>();
171  dummy->setName("dummy");
172  dummy->setPose(math::TPose3D(0, 0, 0, 0, 0, DEG2RAD(-90)));
173  {
174  // head
175  CSphere::Ptr part =
176  mrpt::make_aligned_shared<CSphere>(HEAD_RADIUS);
177  part->setColor(1, 1, 1, ALPHA_CH);
178  part->setPose(
179  math::TPose3D(
180  0, 0, 0.5 * BODY_LENGTH + HEAD_RADIUS, 0, 0,
181  0));
182  dummy->insert(part);
183  }
184  {
185  // body
186  CCylinder::Ptr part =
187  mrpt::make_aligned_shared<CCylinder>(
188  BODY_RADIUS, BODY_RADIUS, BODY_LENGTH);
189  part->setColor(1, 1, 1, ALPHA_CH);
190  part->setPose(
191  math::TPose3D(0, 0, -BODY_LENGTH / 2, 0, 0, 0));
192  dummy->insert(part);
193  }
194  {
195  // left arm 0
196  CCylinder::Ptr part =
197  mrpt::make_aligned_shared<CCylinder>(
198  ARM_RADIUS, ARM_RADIUS, ARM_LENGTH);
199  part->setColor(1, 1, 1, ALPHA_CH);
200  part->setPose(
201  math::TPose3D(
202  -BODY_RADIUS, 0, 0.5 * BODY_LENGTH - ARM_RADIUS,
203  0, DEG2RAD(-90), 0));
204  dummy->insert(part);
205  }
206  {
207  // left arm 1
208  CCylinder::Ptr part =
209  mrpt::make_aligned_shared<CCylinder>(
210  ARM_RADIUS, ARM_RADIUS, ARM_LENGTH);
211  part->setColor(1, 1, 1, ALPHA_CH);
212  part->setPose(
213  math::TPose3D(
214  -BODY_RADIUS - ARM_LENGTH + ARM_RADIUS, 0,
215  0.5 * BODY_LENGTH - ARM_RADIUS, 0, 0, 0));
216  dummy->insert(part);
217  }
218  {
219  // right arm 0
220  CCylinder::Ptr part =
221  mrpt::make_aligned_shared<CCylinder>(
222  ARM_RADIUS, ARM_RADIUS, ARM_LENGTH);
223  part->setColor(1, 1, 1, ALPHA_CH);
224  part->setPose(
225  math::TPose3D(
226  BODY_RADIUS, 0, 0.5 * BODY_LENGTH - ARM_RADIUS,
227  0, DEG2RAD(90), 0));
228  dummy->insert(part);
229  }
230  {
231  // right arm 1
232  CCylinder::Ptr part =
233  mrpt::make_aligned_shared<CCylinder>(
234  ARM_RADIUS, ARM_RADIUS, ARM_LENGTH);
235  part->setColor(1, 1, 1, ALPHA_CH);
236  part->setPose(
237  math::TPose3D(
238  BODY_RADIUS + ARM_LENGTH - ARM_RADIUS, 0,
239  0.5 * BODY_LENGTH - ARM_RADIUS, 0, 0, 0));
240  dummy->insert(part);
241  }
242  {
243  // left leg
244  CCylinder::Ptr part =
245  mrpt::make_aligned_shared<CCylinder>(
246  LEG_RADIUS, LEG_RADIUS, LEG_LENGTH);
247  part->setColor(1, 1, 1, ALPHA_CH);
248  part->setPose(
249  math::TPose3D(
250  -BODY_RADIUS + LEG_RADIUS, 0,
251  -(0.5 * BODY_LENGTH + LEG_LENGTH), 0, 0, 0));
252  dummy->insert(part);
253  }
254  {
255  // right leg
256  CCylinder::Ptr part =
257  mrpt::make_aligned_shared<CCylinder>(
258  LEG_RADIUS, LEG_RADIUS, LEG_LENGTH);
259  part->setColor(1, 1, 1, ALPHA_CH);
260  part->setPose(
261  math::TPose3D(
262  BODY_RADIUS - LEG_RADIUS, 0,
263  -(0.5 * BODY_LENGTH + LEG_LENGTH), 0, 0, 0));
264  dummy->insert(part);
265  }
266  scene->insert(dummy);
267  } // end-if
268  else
269  {
270  CSetOfObjects::Ptr dummy =
271  std::dynamic_pointer_cast<CSetOfObjects>(
272  scene->getByName("dummy"));
273  dummy->setVisibility(true);
274  }
275 
276  // update joints positions
277  CSetOfObjects::Ptr body =
278  std::dynamic_pointer_cast<CSetOfObjects>(
279  scene->getByName("body"));
280  ASSERT_(body);
281 
282  for (int i = 0; i < NUM_JOINTS; ++i)
283  {
284  CSphere::Ptr s = std::dynamic_pointer_cast<CSphere>(
285  body->getByName(jointNames[i]));
286  CPoint3D sphPos;
287  if (i == 0)
288  sphPos = CPoint3D(0, 0, 0);
289  else
290  {
291  m_joint_theta[i] += M_2PI / (10 * (NUM_JOINTS - 1));
292  sphPos.x(0.5 * cos(m_joint_theta[i]));
293  sphPos.y(0.5 * sin(m_joint_theta[i]));
294  sphPos.z(0.0);
295  }
296  s->setPose(sphPos);
297  s->setColor(1, 0, 0);
298  s->setRadius(i == 0 ? 0.07 : 0.03);
299  } // end-for
300  } // end-get3DSceneAndLock
301  m_win->unlockAccess3DScene();
302  m_win->forceRepaint();
303  } // end if
304  } // end if
305 } // end-processPreviewNone
306 
307 /*-------------------------------------------------------------
308  processPreview
309 -------------------------------------------------------------*/
310 void CSkeletonTracker::processPreview(
311  const mrpt::obs::CObservationSkeleton::Ptr& obs)
312 {
313  using namespace mrpt::opengl;
314 
315  // show skeleton data
316  if (m_showPreview)
317  {
318  if (!m_win)
319  {
320  string caption = string("Preview of ") + m_sensorLabel;
321  m_win = mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow3D>(
322  caption, 800, 600);
323 
324  COpenGLScene::Ptr& scene = m_win->get3DSceneAndLock();
325  scene->insert(
326  mrpt::make_aligned_shared<CGridPlaneXZ>(-3, 3, 0, 5, -1.5));
327 
328  // set camera parameters
329  m_win->setCameraElevationDeg(-90);
330  m_win->setCameraAzimuthDeg(90);
331  m_win->setCameraZoom(4);
332  m_win->setCameraPointingToPoint(0, 0, 0);
333 
334  // insert initial body
335  CSetOfObjects::Ptr body =
336  mrpt::make_aligned_shared<CSetOfObjects>();
337  body->setName("body");
338  for (int i = 0; i < NUM_JOINTS; ++i)
339  {
340  CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.03f);
341  sph->setColor(0, 1, 0);
342  sph->setName(jointNames[i]);
343  body->insert(sph);
344  }
345 
346  // insert initial lines
347  CSetOfLines::Ptr lines = mrpt::make_aligned_shared<CSetOfLines>();
348  lines->setName("lines");
349  lines->setColor(0, 0, 1);
350  body->insert(lines);
351 
352  scene->insert(body);
353  m_win->unlockAccess3DScene();
354  }
355 
356  if (m_win && m_win->isOpen())
357  {
358  COpenGLScene::Ptr& scene = m_win->get3DSceneAndLock();
359 
360  // remove help text and dummy
361  m_win->clearTextMessages();
362  CSetOfObjects::Ptr dummy = std::dynamic_pointer_cast<CSetOfObjects>(
363  scene->getByName("dummy"));
364  if (dummy) dummy->setVisibility(false);
365 
366  {
367  // update joints positions
368  CSetOfObjects::Ptr body =
369  std::dynamic_pointer_cast<CSetOfObjects>(
370  scene->getByName("body"));
371  ASSERT_(body);
372 
373  for (int i = 0; i < NUM_JOINTS; ++i)
374  {
375  CObservationSkeleton::TSkeletonJoint j;
376 
377  switch (i)
378  {
379  case 0:
380  j = obs->head;
381  break;
382  case 1:
383  j = obs->neck;
384  break;
385  case 2:
386  j = obs->torso;
387  break;
388 
389  case 3:
390  j = obs->left_shoulder;
391  break;
392  case 4:
393  j = obs->left_elbow;
394  break;
395  case 5:
396  j = obs->left_hand;
397  break;
398  case 6:
399  j = obs->left_hip;
400  break;
401  case 7:
402  j = obs->left_knee;
403  break;
404  case 8:
405  j = obs->left_foot;
406  break;
407 
408  case 9:
409  j = obs->right_shoulder;
410  break;
411  case 10:
412  j = obs->right_elbow;
413  break;
414  case 11:
415  j = obs->right_hand;
416  break;
417  case 12:
418  j = obs->right_hip;
419  break;
420  case 13:
421  j = obs->right_knee;
422  break;
423  case 14:
424  j = obs->right_foot;
425  break;
426  } // end-switch
427 
428  CSphere::Ptr s = std::dynamic_pointer_cast<CSphere>(
429  body->getByName(jointNames[i]));
430  s->setPose(
431  mrpt::math::TPose3D(
432  j.x * 1e-3, j.y * 1e-3, j.z * 1e-3, 0, 0, 0));
433  s->setColor(
434  std::min(1.0, 2 * (1 - j.conf)),
435  std::min(1.0, 2 * j.conf), 0);
436  s->setRadius(i == 0 ? 0.07 : 0.03);
437  } // end-for
438 
439  // update lines joining joints
440  CSetOfLines::Ptr lines = std::dynamic_pointer_cast<CSetOfLines>(
441  body->getByName("lines"));
442  ASSERT_(lines);
443 
444  lines->clear();
445  for (int i = 0; i < NUM_LINES; ++i)
446  {
447  pair<JOINT, JOINT> pair = m_linesToPlot[i];
448  CSphere::Ptr s0 = dynamic_pointer_cast<CSphere>(
449  body->getByName(jointNames[pair.first]));
450  CSphere::Ptr s1 = dynamic_pointer_cast<CSphere>(
451  body->getByName(jointNames[pair.second]));
452  ASSERT_(s0 && s1);
453 
454  lines->appendLine(
455  s0->getPoseX(), s0->getPoseY(), s0->getPoseZ(),
456  s1->getPoseX(), s1->getPoseY(), s1->getPoseZ());
457  }
458  } // end-get3DSceneAndLock
459  m_win->unlockAccess3DScene();
460  m_win->forceRepaint();
461  } // end if
462  } // end if
463 }
464 
465 /*-------------------------------------------------------------
466  doProcess
467 -------------------------------------------------------------*/
468 void CSkeletonTracker::doProcess()
469 {
470 #if MRPT_HAS_OPENNI2 && MRPT_HAS_NITE2
471  if (m_state == ssError)
472  {
473  std::this_thread::sleep_for(200ms);
474  initialize();
475  }
476 
477  if (m_state == ssError) return;
478 
479  nite::UserTrackerFrameRef userTrackerFrame;
480  nite::Status niteRc = user_tracker->readFrame(&userTrackerFrame);
481 
482  if (niteRc != nite::STATUS_OK)
483  {
484  printf(" [Skeleton tracker] Get next frame failed\n");
485  return;
486  }
487 
488  int n_data_ok = 0;
489  const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers();
490  m_nUsers = users.getSize();
491  for (int i = 0; i < m_nUsers; ++i)
492  {
493  const nite::UserData& user = users[i];
494 
495  // update user state
496  skl_states[user.getId()] = user.getSkeleton().getState();
497 
498  if (user.isNew())
499  {
500  user_tracker->startSkeletonTracking(user.getId());
501  cout << " [Skeleton tracker] New user found" << endl;
502  }
503  else if (user.getSkeleton().getState() == nite::SKELETON_TRACKED)
504  {
505  cout << " [Skeleton tracker] User " << user.getId() << " tracked"
506  << endl;
507  CObservationSkeleton::Ptr obs =
508  mrpt::make_aligned_shared<CObservationSkeleton>();
509 
510  // timestamp
511  const uint64_t nowUI = userTrackerFrame.getTimestamp();
512 
513  uint64_t AtUI = 0;
514  if (m_timeStartUI == 0)
515  {
516  m_timeStartUI = nowUI;
517  m_timeStartTT = mrpt::system::now();
518  }
519  else
520  AtUI = nowUI - m_timeStartUI;
521 
522  mrpt::system::TTimeStamp AtDO = mrpt::system::secondsToTimestamp(
523  AtUI * 1e-6 /* Board time is usec */);
524  mrpt::system::TTimeStamp ts = m_timeStartTT + AtDO;
525 
526  // fill joint data
527  FILL_JOINT_DATA(head, nite::JOINT_HEAD)
528  FILL_JOINT_DATA(neck, nite::JOINT_NECK)
529  FILL_JOINT_DATA(torso, nite::JOINT_TORSO)
530 
531  FILL_JOINT_DATA(left_shoulder, nite::JOINT_LEFT_SHOULDER)
532  FILL_JOINT_DATA(left_elbow, nite::JOINT_LEFT_ELBOW)
533  FILL_JOINT_DATA(left_hand, nite::JOINT_LEFT_HAND)
534  FILL_JOINT_DATA(left_hip, nite::JOINT_LEFT_HIP)
535  FILL_JOINT_DATA(left_knee, nite::JOINT_LEFT_KNEE)
536  FILL_JOINT_DATA(left_foot, nite::JOINT_LEFT_FOOT)
537 
538  FILL_JOINT_DATA(right_shoulder, nite::JOINT_RIGHT_SHOULDER)
539  FILL_JOINT_DATA(right_elbow, nite::JOINT_RIGHT_ELBOW)
540  FILL_JOINT_DATA(right_hand, nite::JOINT_RIGHT_HAND)
541  FILL_JOINT_DATA(right_hip, nite::JOINT_RIGHT_HIP)
542  FILL_JOINT_DATA(right_knee, nite::JOINT_RIGHT_KNEE)
543  FILL_JOINT_DATA(right_foot, nite::JOINT_RIGHT_FOOT)
544 
545  // sensor label:
546  obs->sensorLabel =
547  m_sensorLabel + "_" + std::to_string(user.getId());
548 
549  appendObservation(obs);
550  processPreview(obs);
551 
552  m_toutCounter = 0;
553  n_data_ok++;
554  } // end-else-if
555  } // end-for
556 
557  if (n_data_ok == 0) // none of the sensors yielded data
558  m_toutCounter++;
559 
560  if (m_toutCounter > 0)
561  {
562  processPreviewNone();
563  if ((m_toutCounter % 50) == 0)
564  cout << " [Skeleton tracker] Looking for user..." << endl;
565  }
566 
567  if (m_toutCounter > 2000)
568  {
569  m_toutCounter = 0;
570  m_state = ssError;
571 
572  cout << " [Skeleton tracker] No user found after 2000 attempts ..."
573  << endl;
574  nite::NiTE::shutdown(); // close tracker
575  }
576 #else
577  THROW_EXCEPTION(
578  "MRPT has been compiled with 'BUILD_OPENNI2'=OFF or "
579  "'MRPT_HAS_NITE2'=OFF, so this class cannot be used.");
580 #endif
581 }
582 
583 /*-------------------------------------------------------------
584  initialize
585 -------------------------------------------------------------*/
586 void CSkeletonTracker::initialize()
587 {
588 #if MRPT_HAS_OPENNI2 && MRPT_HAS_NITE2
589 
590  // initialize tracker
591  nite::NiTE::initialize();
592  nite::Status niteRc = user_tracker->create();
593 
594  if (niteRc != nite::STATUS_OK)
595  {
596  printf("Couldn't create user tracker\n");
597  m_state = ssError;
598  }
599  else
600  {
601  printf("Sucessfully created user tracker \n");
602  printf(
603  "Start moving around to get detected...\n(PSI pose may be required "
604  "for skeleton calibration, depending on the configuration)\n");
605  m_state = ssInitializing;
606  }
607  // initialize preview joints and lines
608  if (m_showPreview)
609  {
610  m_linesToPlot[0] = make_pair(NECK, HEAD);
611  m_linesToPlot[1] = make_pair(NECK, TORSO);
612  m_linesToPlot[2] = make_pair(NECK, LEFT_SHOULDER);
613  m_linesToPlot[3] = make_pair(NECK, RIGHT_SHOULDER);
614  m_linesToPlot[4] = make_pair(LEFT_SHOULDER, LEFT_ELBOW);
615  m_linesToPlot[5] = make_pair(LEFT_ELBOW, LEFT_HAND);
616  m_linesToPlot[6] = make_pair(RIGHT_SHOULDER, RIGHT_ELBOW);
617  m_linesToPlot[7] = make_pair(RIGHT_ELBOW, RIGHT_HAND);
618  m_linesToPlot[8] = make_pair(TORSO, LEFT_HIP);
619  m_linesToPlot[9] = make_pair(TORSO, RIGHT_HIP);
620  m_linesToPlot[10] = make_pair(LEFT_HIP, LEFT_KNEE);
621  m_linesToPlot[11] = make_pair(LEFT_KNEE, LEFT_FOOT);
622  m_linesToPlot[12] = make_pair(RIGHT_HIP, RIGHT_KNEE);
623  m_linesToPlot[13] = make_pair(RIGHT_KNEE, RIGHT_FOOT);
624  }
625 #else
626  THROW_EXCEPTION(
627  "MRPT has been compiled with 'BUILD_OPENNI2'=OFF OR "
628  "'MRPT_HAS_NITE2'=OFF, so this class cannot be used.");
629 #endif
630 }
631 
632 /*-------------------------------------------------------------
633  loadConfig_sensorSpecific
634 -------------------------------------------------------------*/
635 void CSkeletonTracker::loadConfig_sensorSpecific(
636  const mrpt::config::CConfigFileBase& configSource,
637  const std::string& iniSection)
638 {
639  m_sensorPose.setFromValues(
640  configSource.read_float(iniSection, "pose_x", 0, false),
641  configSource.read_float(iniSection, "pose_y", 0, false),
642  configSource.read_float(iniSection, "pose_z", 0, false),
643  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
644  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
645  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
646 
647  m_showPreview =
648  configSource.read_bool(iniSection, "showPreview", m_showPreview, false);
649 
650  // dump parameters to console
651  cout << "---------------------------" << endl;
652  cout << "Skeleton Tracker parameters: " << endl;
653  cout << "---------------------------" << endl;
654  cout << m_sensorPose << endl;
655  cout << m_showPreview << endl;
656  cout << "---------------------------" << endl << endl;
657 }
CSetOfLines.h
mrpt::hwdrivers::CSkeletonTracker::CSkeletonTracker
CSkeletonTracker()
Constructor.
Definition: CSkeletonTracker.cpp:56
skl_states
#define skl_states
Definition: CSkeletonTracker.cpp:34
mrpt::hwdrivers::CSkeletonTracker::m_showPreview
bool m_showPreview
Preview window management.
Definition: CSkeletonTracker.h:91
mrpt::opengl::CSetOfLines::Ptr
std::shared_ptr< CSetOfLines > Ptr
Definition: CSetOfLines.h:35
mrpt::system::secondsToTimestamp
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:224
NUM_JOINTS
#define NUM_JOINTS
Definition: CSkeletonTracker.h:72
MAX_USERS
#define MAX_USERS
Definition: CSkeletonTracker.cpp:36
mrpt::hwdrivers::CSkeletonTracker::m_userTracker_ptr
void * m_userTracker_ptr
Definition: CSkeletonTracker.h:79
s
GLdouble s
Definition: glext.h:3676
mrpt::hwdrivers::CSkeletonTracker::RIGHT_HAND
@ RIGHT_HAND
Definition: CSkeletonTracker.h:65
CSimpleLine.h
CSkeletonTracker.h
CSphere.h
user_tracker
#define user_tracker
Definition: CSkeletonTracker.cpp:35
mrpt::hwdrivers::CSkeletonTracker::initialize
void initialize()
Connects to the PrimeSense camera and prepares it to get skeleton data.
Definition: CSkeletonTracker.cpp:586
mrpt::hwdrivers::CSkeletonTracker::HEAD
@ HEAD
Definition: CSkeletonTracker.h:54
mrpt::opengl::CSphere::Ptr
std::shared_ptr< CSphere > Ptr
Definition: CSphere.h:33
mrpt::hwdrivers::CSkeletonTracker::RIGHT_KNEE
@ RIGHT_KNEE
Definition: CSkeletonTracker.h:67
mrpt::hwdrivers::CSkeletonTracker::processPreviewNone
void processPreviewNone()
Definition: CSkeletonTracker.cpp:102
mrpt::to_string
std::string std::string to_string(T v)
Just like std::to_string(), but with an overloaded version for std::string arguments.
Definition: format.h:27
mrpt::poses::CPose3D::setFromValues
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:239
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
mrpt::opengl::CCylinder::Ptr
std::shared_ptr< CCylinder > Ptr
Definition: CCylinder.h:34
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:150
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:22
mrpt::hwdrivers::CGenericSensor::ssError
@ ssError
Definition: CGenericSensor.h:88
mrpt::hwdrivers::CSkeletonTracker::RIGHT_SHOULDER
@ RIGHT_SHOULDER
Definition: CSkeletonTracker.h:63
mrpt::hwdrivers::CSkeletonTracker::m_win
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CSkeletonTracker.h:92
mrpt::hwdrivers::CSkeletonTracker::LEFT_HAND
@ LEFT_HAND
Definition: CSkeletonTracker.h:59
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
stock_objects.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::hwdrivers::CGenericSensor::m_state
TSensorState m_state
Definition: CGenericSensor.h:147
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:31
mrpt::hwdrivers::CSkeletonTracker::doProcess
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
Definition: CSkeletonTracker.cpp:468
mrpt::hwdrivers::CSkeletonTracker::LEFT_HIP
@ LEFT_HIP
Definition: CSkeletonTracker.h:60
mrpt::hwdrivers::CSkeletonTracker::LEFT_SHOULDER
@ LEFT_SHOULDER
Definition: CSkeletonTracker.h:57
mrpt::hwdrivers::CSkeletonTracker::TORSO
@ TORSO
Definition: CSkeletonTracker.h:56
mrpt::hwdrivers::CSkeletonTracker::RIGHT_HIP
@ RIGHT_HIP
Definition: CSkeletonTracker.h:66
mrpt::img
Definition: CCanvas.h:17
mrpt::hwdrivers::CSkeletonTracker::~CSkeletonTracker
virtual ~CSkeletonTracker()
Destructor.
Definition: CSkeletonTracker.cpp:87
mrpt::hwdrivers::CSkeletonTracker::LEFT_KNEE
@ LEFT_KNEE
Definition: CSkeletonTracker.h:61
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:144
Status
enum Status { eInsideTag=0, eOutsideTag } Status
Definition: xmlParser.cpp:804
mrpt::obs::CObservationSkeleton::TSkeletonJoint::x
double x
3D position
Definition: CObservationSkeleton.h:67
mrpt::hwdrivers::CSkeletonTracker::RIGHT_FOOT
@ RIGHT_FOOT
Definition: CSkeletonTracker.h:68
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hwdrivers::CSkeletonTracker::LEFT_FOOT
@ LEFT_FOOT
Definition: CSkeletonTracker.h:62
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::obs::CObservationSkeleton::TSkeletonJoint::y
double y
Definition: CObservationSkeleton.h:67
mrpt::hwdrivers::CSkeletonTracker::m_timeStartUI
uint32_t m_timeStartUI
Timestamp management.
Definition: CSkeletonTracker.h:82
mrpt::img::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
mrpt::hwdrivers::CSkeletonTracker::LEFT_ELBOW
@ LEFT_ELBOW
Definition: CSkeletonTracker.h:58
mrpt::obs::CObservationSkeleton::Ptr
std::shared_ptr< CObservationSkeleton > Ptr
Definition: CObservationSkeleton.h:30
uint64_t
unsigned __int64 uint64_t
Definition: rptypes.h:50
mrpt::hwdrivers::CSkeletonTracker::m_joint_theta
std::vector< double > m_joint_theta
Joint angles when no skeleton has been detected.
Definition: CSkeletonTracker.h:96
mrpt::hwdrivers::CGenericSensor::appendObservation
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
Definition: CGenericSensor.h:179
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:603
mrpt::hwdrivers::CSkeletonTracker::NECK
@ NECK
Definition: CSkeletonTracker.h:55
NUM_LINES
#define NUM_LINES
Definition: CSkeletonTracker.h:73
mrpt::hwdrivers::CGenericSensor::m_sensorLabel
std::string m_sensorLabel
See CGenericSensor.
Definition: CGenericSensor.h:140
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:113
jointNames
string jointNames[]
Definition: CSkeletonTracker.cpp:48
mrpt::hwdrivers::CSkeletonTracker::m_toutCounter
unsigned int m_toutCounter
Timeout counter (for internal use only)
Definition: CSkeletonTracker.h:99
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:30
mrpt::hwdrivers::CSkeletonTracker::m_skeletons_ptr
void * m_skeletons_ptr
Opaque pointers to specific NITE data.
Definition: CSkeletonTracker.h:78
mrpt::opengl::FILL
@ FILL
renders glyphs as filled polygons
Definition: opengl_fonts.h:38
mrpt::hwdrivers::CSkeletonTracker::m_timeStartTT
mrpt::system::TTimeStamp m_timeStartTT
Definition: CSkeletonTracker.h:83
CBox.h
CPoint3D.h
IMPLEMENTS_GENERIC_SENSOR
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Definition: CGenericSensor.h:330
mrpt::hwdrivers::CSkeletonTracker::loadConfig_sensorSpecific
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
Definition: CSkeletonTracker.cpp:635
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::hwdrivers::CSkeletonTracker::RIGHT_ELBOW
@ RIGHT_ELBOW
Definition: CSkeletonTracker.h:64
mrpt::obs::CObservationSkeleton::TSkeletonJoint::conf
double conf
Confidence value [0...1].
Definition: CObservationSkeleton.h:70
mrpt::hwdrivers::CSkeletonTracker::m_sensorPose
mrpt::poses::CPose3D m_sensorPose
Sensor pose.
Definition: CSkeletonTracker.h:86
FILL_JOINT_DATA
#define FILL_JOINT_DATA(_J1, _J2)
Definition: CSkeletonTracker.cpp:37
CCylinder.h
mrpt::hwdrivers::CSkeletonTracker
A class for grabbing mrpt::obs::CObservationSkeleton from a PrimeSense camera.
Definition: CSkeletonTracker.h:50
CGridPlaneXZ.h
M_2PI
#define M_2PI
Definition: common.h:58
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< COpenGLScene > Ptr
Definition: COpenGLScene.h:61
mrpt::obs::CObservationSkeleton::TSkeletonJoint
A generic joint for the skeleton observation.
Definition: CObservationSkeleton.h:62
mrpt::hwdrivers::CGenericSensor::ssInitializing
@ ssInitializing
Definition: CGenericSensor.h:86
mrpt::hwdrivers::CSkeletonTracker::processPreview
void processPreview(const mrpt::obs::CObservationSkeleton::Ptr &obs)
Displays real-time info for the captured skeleton.
Definition: CSkeletonTracker.cpp:310
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:33
mrpt::hwdrivers::CSkeletonTracker::m_nUsers
int m_nUsers
Number of detected users.
Definition: CSkeletonTracker.h:88
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
mrpt::obs::CObservationSkeleton::TSkeletonJoint::z
double z
Definition: CObservationSkeleton.h:67
mrpt::hwdrivers::CSkeletonTracker::m_linesToPlot
std::vector< std::pair< JOINT, JOINT > > m_linesToPlot
Lines between joints.
Definition: CSkeletonTracker.h:94
CTexturedPlane.h
CText.h
hwdrivers-precomp.h
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST