MRPT  1.9.9
COpenNI2Generic.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled header
11 
15 
16 // Universal include for all versions of OpenCV
17 #include <mrpt/3rdparty/do_opencv_includes.h>
18 
19 #include <atomic>
20 #include <mutex>
21 #include <thread>
22 
23 #if MRPT_HAS_OPENNI2
24 
25 // This seems to be assumed by OpenNI.h and undefined for some reason in
26 // GCC/Ubuntu
27 #if !defined(_WIN32)
28 #define linux 1
29 #endif
30 
31 #include <OpenNI.h>
32 #include <PS1080.h>
33 #endif
34 
35 using namespace mrpt::hwdrivers;
36 using namespace mrpt::system;
37 using namespace mrpt::obs;
38 using namespace std;
39 
40 // Initialize static member
41 std::vector<std::shared_ptr<COpenNI2Generic::CDevice>> vDevices;
42 std::recursive_mutex vDevices_mx;
43 std::atomic<int> numInstances(0);
44 
45 #if MRPT_HAS_OPENNI2
46 bool setONI2StreamMode(
47  openni::VideoStream& stream, int w, int h, int fps,
48  openni::PixelFormat format);
49 std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab = 0);
50 bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2);
51 #endif // MRPT_HAS_OPENNI2
52 
54 
56 {
57  return numInstances; // get atomic
58 }
59 
60 /*-------------------------------------------------------------
61 ctor
62 -------------------------------------------------------------*/
64 #if MRPT_HAS_OPENNI2
65  : m_rgb_format(openni::PIXEL_FORMAT_RGB888),
66  m_depth_format(openni::PIXEL_FORMAT_DEPTH_1_MM)
67 #endif
68 {
69  const char* sVerbose = getenv("MRPT_HWDRIVERS_VERBOSE");
70  m_verbose = (sVerbose != nullptr) && atoi(sVerbose) != 0;
71  // Start automatically:
72  if (!this->start())
73  {
74 #if MRPT_HAS_OPENNI2
76  "After initialization:\n %s\n",
77  openni::OpenNI::getExtendedError()));
78 #endif
79  }
80 }
81 
83  int width, int height, float fps, bool open_streams_now)
84  : m_width(width),
85  m_height(height),
86  m_fps(fps),
87 #if MRPT_HAS_OPENNI2
88  m_rgb_format(openni::PIXEL_FORMAT_RGB888),
89  m_depth_format(openni::PIXEL_FORMAT_DEPTH_1_MM),
90 #endif // MRPT_HAS_OPENNI2
91  m_verbose(false),
92  m_grab_image(true),
93  m_grab_depth(true),
94  m_grab_3D_points(true)
95 {
96  const char* sVerbose = getenv("MRPT_HWDRIVERS_VERBOSE");
97  m_verbose = (sVerbose != nullptr) && atoi(sVerbose) != 0;
98  // Open?
99  if (open_streams_now)
100  {
101  if (!this->start())
102  {
103 #if MRPT_HAS_OPENNI2
105  "After initialization:\n %s\n",
106  openni::OpenNI::getExtendedError()));
107 #endif
108  }
109  }
110 }
111 
113 {
114 #if MRPT_HAS_OPENNI2
115  if (numInstances == 0)
116  {
117  if (openni::OpenNI::initialize() != openni::STATUS_OK)
118  {
119  return false;
120  }
121  else
122  {
123  std::cerr << "[" << __FUNCTION__ << "]" << std::endl
124  << " Initialized OpenNI2." << std::endl;
125  }
126  }
127  numInstances++;
128  return true;
129 #else
130  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
131 #endif // MRPT_HAS_OPENNI2
132 }
133 
134 /*-------------------------------------------------------------
135 dtor
136 -------------------------------------------------------------*/
138 {
139  numInstances--;
140  if (numInstances == 0)
141  {
142  kill();
143  }
144 }
145 
147 {
148  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
149  return vDevices.size();
150 }
152 bool COpenNI2Generic::isVerbose() const { return m_verbose; }
153 void COpenNI2Generic::showLog(const std::string& message) const
154 {
155  if (isVerbose() == false)
156  {
157  return;
158  }
159  std::cerr << message;
160 }
161 /** This method can or cannot be implemented in the derived class, depending on
162  * the need for it.
163  * \exception This method must throw an exception with a descriptive message if
164  * some critical error is found.
165  */
167 {
168 #if MRPT_HAS_OPENNI2
169  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
170  // Get devices list
171  openni::Array<openni::DeviceInfo> oni2InfoArray;
172  openni::OpenNI::enumerateDevices(&oni2InfoArray);
173 
174  const size_t numDevices = oni2InfoArray.getSize();
175  showLog(mrpt::format("[%s]\n", __FUNCTION__));
177  " Get device list. %d devices connected.\n", (int)numDevices));
178 
179  // Search new devices.
180  std::set<int> newDevices;
181  for (unsigned i = 0; i < numDevices; i++)
182  {
183  const openni::DeviceInfo& info = oni2InfoArray[i];
184  showLog(mrpt::format(" Device[%d]\n", i));
185  showLog(oni2DevInfoStr(info, 3) + "\n");
186 
187  bool isExist = false;
188  for (unsigned int j = 0, j_end = vDevices.size();
189  j < j_end && isExist == false; ++j)
190  {
191  if (cmpONI2Device(info, vDevices[j]->getInfo()))
192  {
193  isExist = true;
194  }
195  }
196  if (isExist == false)
197  {
198  newDevices.insert(i);
199  }
200  }
201  // Add new devices to device list(static member).
202  for (int newDevice : newDevices)
203  {
204  const openni::DeviceInfo& info = oni2InfoArray[newDevice];
205  CDevice::Ptr device = CDevice::create(
206  info, (openni::PixelFormat)m_rgb_format,
207  (openni::PixelFormat)m_depth_format, m_verbose);
208  vDevices.push_back(device);
209  {
210  unsigned int sn;
211  if (device->getSerialNumber(sn))
212  {
214  "Device[%d]: serial number: `%u`\n", newDevice, sn));
215  }
216  }
217  }
218 
219  if (getNumDevices() == 0)
220  {
221  showLog(" No devices connected -> EXIT\n");
222  }
223  else
224  {
225  showLog(mrpt::format(" %d devices were found.\n", getNumDevices()));
226  }
227  return getNumDevices();
228 #else
229  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
230 #endif // MRPT_HAS_OPENNI2
231 }
232 
234 {
235  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
236 #if MRPT_HAS_OPENNI2
237  vDevices.clear();
238  openni::OpenNI::shutdown();
239 #else
240  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
241 #endif // MRPT_HAS_OPENNI2
242 }
243 
244 bool COpenNI2Generic::isOpen(const unsigned sensor_id) const
245 {
246 #if MRPT_HAS_OPENNI2
247  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
248  if ((int)sensor_id >= getNumDevices())
249  {
250  return false;
251  }
252  return vDevices[sensor_id]->isOpen();
253 #else
254  MRPT_UNUSED_PARAM(sensor_id);
255  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
256 #endif // MRPT_HAS_OPENNI2
257 }
258 
259 void COpenNI2Generic::open(unsigned sensor_id)
260 {
261 #if MRPT_HAS_OPENNI2
262  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
263  // Sensor index validation.
264  if (!getNumDevices())
265  {
266  THROW_EXCEPTION("No OpenNI2 devices found.");
267  }
268  if ((int)sensor_id >= getNumDevices())
269  {
271  "Sensor index is higher than the number of connected devices.");
272  }
273  showLog(mrpt::format("[%s]\n", __FUNCTION__));
274  showLog(mrpt::format(" open[%d] ...\n", sensor_id));
275 
276  if (isOpen(sensor_id))
277  {
278  showLog(
279  mrpt::format(" The sensor [%d] is already opened\n", sensor_id));
280  return;
281  }
282  if (m_verbose)
283  printf(
284  "[COpenNI2Generic] DBG: [%s] about to call vDevices[%d]->open()\n",
285  __FUNCTION__, sensor_id);
286  vDevices[sensor_id]->open(m_width, m_height, m_fps);
287  showLog(vDevices[sensor_id]->getLog() + "\n");
288  showLog(mrpt::format(" Device [%d] ", sensor_id));
289  if (vDevices[sensor_id]->isOpen())
290  {
291  showLog(" open successfully.\n");
292  }
293  else
294  {
295  showLog(" open failed.\n");
296  }
297  std::this_thread::sleep_for(1000ms); // Sleep
298 #else
299  MRPT_UNUSED_PARAM(sensor_id);
300  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
301 #endif // MRPT_HAS_OPENNI2
302 }
303 
305  const std::set<unsigned>& serial_required)
306 {
307 #if MRPT_HAS_OPENNI2
308  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
309  showLog(mrpt::format("[%s]\n", __FUNCTION__));
310  unsigned num_open_dev = 0;
311  for (unsigned sensor_id = 0; sensor_id < vDevices.size(); sensor_id++)
312  {
313  unsigned int serialNum;
314  if (vDevices[sensor_id]->getSerialNumber(serialNum) == false)
315  {
316  showLog(vDevices[sensor_id]->getLog());
317  continue;
318  }
319  if (m_verbose)
320  printf(
321  "[COpenNI2Generic::openDevicesBySerialNum] checking device "
322  "with serial '%d'\n",
323  serialNum);
324 
325  if (serial_required.find(serialNum) == serial_required.end())
326  {
327  vDevices[sensor_id]->close();
328  continue;
329  }
330  if (vDevices[sensor_id]->isOpen())
331  {
332  num_open_dev++;
333  continue;
334  }
335  int width = m_width;
336  int height = m_height;
337  if (m_verbose)
338  printf(
339  "[COpenNI2Generic] DBG: [%s] about to call "
340  "vDevices[%d]->open(%d,%d,%d)\n",
341  __FUNCTION__, sensor_id, width, height, (int)m_fps);
342  if (vDevices[sensor_id]->open(width, height, m_fps) == false)
343  {
344  showLog(vDevices[sensor_id]->getLog());
345  continue;
346  }
347  num_open_dev++;
348  if (m_verbose)
349  printf(
350  "[COpenNI2Generic] DBG: [%s] now has %d devices open\n",
351  __FUNCTION__, num_open_dev);
352  }
353  return num_open_dev;
354 #else
355  MRPT_UNUSED_PARAM(serial_required);
356  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
357 #endif // MRPT_HAS_OPENNI2
358 }
359 
361  const unsigned int SerialRequired)
362 {
363  std::set<unsigned> serial_required;
364  serial_required.insert(SerialRequired);
365  return openDevicesBySerialNum(serial_required);
366 }
367 
369  const unsigned int SerialRequired, int& sensor_id) const
370 {
371 #if MRPT_HAS_OPENNI2
372  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
373  for (size_t i = 0, i_end = vDevices.size(); i < i_end; ++i)
374  {
375  unsigned int sn;
376  if (vDevices[i]->getSerialNumber(sn) == false)
377  {
378  continue;
379  }
380  if (sn == SerialRequired)
381  {
382  sensor_id = (int)i;
383  return true;
384  }
385  }
386  return false;
387 #else
388  MRPT_UNUSED_PARAM(SerialRequired);
389  MRPT_UNUSED_PARAM(sensor_id);
390  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
391 #endif // MRPT_HAS_OPENNI2
392 }
393 
394 void COpenNI2Generic::close(unsigned sensor_id)
395 {
396 #if MRPT_HAS_OPENNI2
397  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
398  // Sensor index validation.
399  if (!getNumDevices())
400  {
401  THROW_EXCEPTION("No OpenNI2 devices found.");
402  }
403  if ((int)sensor_id >= getNumDevices())
404  {
406  "Sensor index is higher than the number of connected devices.");
407  }
408  vDevices[sensor_id]->close();
409 #else
410  MRPT_UNUSED_PARAM(sensor_id);
411  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
412 #endif // MRPT_HAS_OPENNI2
413 }
414 
415 /** The main data retrieving function, to be called after calling loadConfig()
416  * and initialize().
417  * \param out_obs The output retrieved observation (only if there_is_obs=true).
418  * \param timestamp The timestamp of the capture (only if there_is_obs=true).
419  * \param there_is_obs If set to false, there was no new observation.
420  * \param hardware_error True on hardware/comms error.
421  * \param sensor_id The index of the sensor accessed.
422  *
423  */
425  mrpt::img::CImage& rgb_img, mrpt::system::TTimeStamp& timestamp,
426  bool& there_is_obs, bool& hardware_error, unsigned sensor_id)
427 {
428 #if MRPT_HAS_OPENNI2
429  // Sensor index validation.
430  if (!getNumDevices())
431  {
432  THROW_EXCEPTION("No OpenNI2 devices found.");
433  }
434  if ((int)sensor_id >= getNumDevices())
435  {
437  "Sensor index is higher than the number of connected devices.");
438  }
439  if (vDevices[sensor_id]->getNextFrameRGB(
440  rgb_img, timestamp, there_is_obs, hardware_error) == false)
441  {
442  showLog(mrpt::format("[%s]\n", __FUNCTION__));
443  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
444  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
445  }
446 #else
447  MRPT_UNUSED_PARAM(rgb_img);
448  MRPT_UNUSED_PARAM(timestamp);
449  MRPT_UNUSED_PARAM(there_is_obs);
450  MRPT_UNUSED_PARAM(hardware_error);
451  MRPT_UNUSED_PARAM(sensor_id);
452  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
453 #endif // MRPT_HAS_OPENNI2
454 }
455 
457  mrpt::math::CMatrix_u16& depth_img_mm, mrpt::system::TTimeStamp& timestamp,
458  bool& there_is_obs, bool& hardware_error, unsigned sensor_id)
459 {
460 #if MRPT_HAS_OPENNI2
461  // Sensor index validation.
462  if (getNumDevices() == 0)
463  {
464  THROW_EXCEPTION("No OpenNI2 devices found.");
465  }
466  if ((int)sensor_id >= getNumDevices())
467  {
469  "Sensor index is higher than the number of connected devices.");
470  }
471  if (vDevices[sensor_id]->getNextFrameD(
472  depth_img_mm, timestamp, there_is_obs, hardware_error) == false)
473  {
474  showLog(mrpt::format("[%s]\n", __FUNCTION__));
475  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
476  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
477  }
478 #else
479  MRPT_UNUSED_PARAM(depth_img_mm);
480  MRPT_UNUSED_PARAM(timestamp);
481  MRPT_UNUSED_PARAM(there_is_obs);
482  MRPT_UNUSED_PARAM(hardware_error);
483  MRPT_UNUSED_PARAM(sensor_id);
484  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
485 #endif // MRPT_HAS_OPENNI2
486 }
487 
488 /** The main data retrieving function, to be called after calling loadConfig()
489  * and initialize().
490  * \param out_obs The output retrieved observation (only if there_is_obs=true).
491  * \param there_is_obs If set to false, there was no new observation.
492  * \param hardware_error True on hardware/comms error.
493  * \param sensor_id The index of the sensor accessed.
494  *
495  */
497  mrpt::obs::CObservation3DRangeScan& out_obs, bool& there_is_obs,
498  bool& hardware_error, unsigned sensor_id)
499 {
500 #if MRPT_HAS_OPENNI2
501  // Sensor index validation.
502  if (!getNumDevices())
503  {
504  THROW_EXCEPTION("No OpenNI2 devices found.");
505  }
506  if ((int)sensor_id >= getNumDevices())
507  {
509  "Sensor index is higher than the number of connected devices.");
510  }
511  if (vDevices[sensor_id]->getNextFrameRGBD(
512  out_obs, there_is_obs, hardware_error) == false)
513  {
514  showLog(mrpt::format("[%s]\n", __FUNCTION__));
515  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
516  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
517  }
518 #else
519  MRPT_UNUSED_PARAM(out_obs);
520  MRPT_UNUSED_PARAM(there_is_obs);
521  MRPT_UNUSED_PARAM(hardware_error);
522  MRPT_UNUSED_PARAM(sensor_id);
523  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
524 #endif // MRPT_HAS_OPENNI2
525 }
526 
528  mrpt::img::TCamera& param, unsigned sensor_id) const
529 {
530 #if MRPT_HAS_OPENNI2
531  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
532  if (isOpen(sensor_id) == false)
533  {
534  return false;
535  }
536  return vDevices[sensor_id]->getCameraParam(CDevice::COLOR_STREAM, param);
537 #else
538  MRPT_UNUSED_PARAM(param);
539  MRPT_UNUSED_PARAM(sensor_id);
540  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
541 #endif // MRPT_HAS_OPENNI2
542 }
543 
545  mrpt::img::TCamera& param, unsigned sensor_id) const
546 {
547 #if MRPT_HAS_OPENNI2
548  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
549  if (isOpen(sensor_id) == false)
550  {
551  return false;
552  }
553  return vDevices[sensor_id]->getCameraParam(CDevice::DEPTH_STREAM, param);
554 #else
555  MRPT_UNUSED_PARAM(param);
556  MRPT_UNUSED_PARAM(sensor_id);
557  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
558 #endif // MRPT_HAS_OPENNI2
559 }
560 
561 #if MRPT_HAS_OPENNI2
562 /*
563 void openni::VideoMode::setResolution()
564 Setter function for the resolution of this VideoMode. Application use of this
565 function is not recommended.
566 Instead, use SensorInfo::getSupportedVideoModes() to obtain a list of valid
567 video modes
568 
569 -- cited from OpenNI2 help. setResolution() is not recommended.
570 */
571 bool setONI2StreamMode(
572  openni::VideoStream& stream, int w, int h, int fps,
573  openni::PixelFormat format)
574 {
575  // std::cout << "[COpenNI2Generic] Ask mode: " << w << "x" << h << " " <<
576  // fps << " fps. format " << format << std::endl;
577  bool found = false;
578  const openni::Array<openni::VideoMode>& modes =
579  stream.getSensorInfo().getSupportedVideoModes();
580  for (int i = 0, i_end = modes.getSize(); i < i_end; ++i)
581  {
582  // if (m_verbose) std::cout << "[COpenNI2Generic] Mode: " <<
583  // modes[i].getResolutionX() << "x" << modes[i].getResolutionY() << " "
584  // << modes[i].getFps() << " fps. format " << modes[i].getPixelFormat()
585  // << std::endl;
586  if (modes[i].getResolutionX() != w)
587  {
588  continue;
589  }
590  if (modes[i].getResolutionY() != h)
591  {
592  continue;
593  }
594  if (modes[i].getFps() != fps)
595  {
596  continue;
597  }
598  if (modes[i].getPixelFormat() != format)
599  {
600  continue;
601  }
602  openni::Status rc = stream.setVideoMode(modes[i]);
603  if (rc != openni::STATUS_OK)
604  {
605  return false;
606  }
607  return true;
608  }
609  return false;
610 }
611 
612 std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab)
613 {
614  std::stringstream sst;
615  std::string space;
616  for (int i = 0; i < tab; ++i)
617  {
618  space += " ";
619  }
620  sst << space << "name=" << info.getName() << std::endl;
621  sst << space << "uri=" << info.getUri() << std::endl;
622  sst << space << "vendor=" << info.getVendor() << std::endl;
623  sst << space << "product=" << info.getUsbProductId();
624  return sst.str();
625 }
626 
627 bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2)
628 {
629  return (strcmp(i1.getUri(), i2.getUri()) == 0);
630 }
631 //
632 COpenNI2Generic::CDevice::CDevice(
633  const openni::DeviceInfo& info, openni::PixelFormat rgb,
634  openni::PixelFormat depth, bool verbose)
635  : m_info(info), m_mirror(true), m_verbose(verbose)
636 {
637  m_streams[COLOR_STREAM] =
638  CStream::create(m_device, openni::SENSOR_COLOR, rgb, m_log, m_verbose);
639  m_streams[IR_STREAM] =
640  CStream::create(m_device, openni::SENSOR_IR, rgb, m_log, m_verbose);
641  m_streams[DEPTH_STREAM] = CStream::create(
642  m_device, openni::SENSOR_DEPTH, depth, m_log, m_verbose);
643 }
644 
645 COpenNI2Generic::CDevice::~CDevice() { close(); }
646 bool COpenNI2Generic::CDevice::synchMirrorMode()
647 {
648  m_mirror = false;
649  // Check whether both stream support mirroring.
650  for (auto& m_stream : m_streams)
651  {
652  if (!m_stream) continue;
653  bool mirror_support;
654  try
655  {
656  mirror_support = m_stream->isMirrorSupported();
657  }
658  catch (std::logic_error& e)
659  {
660  throw(e);
661  }
662  if (mirror_support == false)
663  {
664  m_log << "[" << __FUNCTION__ << "]" << std::endl;
665  m_log << " openni::STREAM_PROPERTY_MIRRORING is not supported on "
666  << m_stream->getName() << "." << std::endl;
667  m_log << " We assume this is MS Kinect and taken images are "
668  "inverted to right and left."
669  << std::endl;
670  // In this case, getMirroringEnabled() method always returns false.
671  // So we cannot confirm whether the images are inverted or not.
672  m_mirror = true;
673  break;
674  }
675  }
676  // Set both stream to same mirror mode.
677  for (auto& m_stream : m_streams)
678  {
679  if (!m_stream) continue;
680  if (m_stream->isMirrorSupported() == false)
681  {
682  break;
683  }
684  if (m_stream->setMirror(m_mirror) == false)
685  {
686  return false;
687  }
688  }
689  return true;
690 }
691 
692 bool COpenNI2Generic::CDevice::startStreams()
693 {
694  MRPT_START
695  int num_ok = 0;
696  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
697  {
698  if (!m_streams[i]) continue;
699  if (m_verbose)
700  printf(" [%s] calling m_streams[%d]->start()\n", __FUNCTION__, i);
701  if (m_streams[i]->start() == false)
702  {
703  if (m_verbose)
704  printf(
705  " [%s] m_streams[%d]->start() returned FALSE!\n",
706  __FUNCTION__, i);
707  }
708  else
709  {
710  num_ok++;
711  }
712  if (m_verbose)
713  printf(
714  " [%s] m_streams[%d]->start() returned TRUE\n", __FUNCTION__,
715  i);
716  }
717  if (m_verbose)
718  printf(
719  " [COpenNI2Generic::CDevice::startStreams()] %d streams were "
720  "started.\n",
721  num_ok);
722  return num_ok > 0;
723  MRPT_END
724 }
725 
726 bool COpenNI2Generic::CDevice::isOpen() const
727 {
728  return (m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid()) ||
729  (m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid());
730 }
731 
732 void COpenNI2Generic::CDevice::close()
733 {
734  for (auto& m_stream : m_streams)
735  {
736  if (!m_stream) continue;
737  m_stream->destroy();
738  }
739  m_device.close();
740 }
741 
742 bool COpenNI2Generic::CDevice::open(int w, int h, int fps)
743 {
744  MRPT_START
745  if (m_verbose)
746  printf(
747  " [COpenNI2Generic::CDevice::open()] Called with w=%i h=%i "
748  "fps=%i\n",
749  w, h, fps);
750  clearLog();
751  close();
752  openni::Status rc = m_device.open(getInfo().getUri());
753  if (rc != openni::STATUS_OK)
754  {
755  m_log << "[" << __FUNCTION__ << "]" << std::endl
756  << " Failed to open device " << getInfo().getUri() << " "
757  << openni::OpenNI::getExtendedError() << std::endl;
758  return false;
759  }
760  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
761  {
762  if (!m_streams[i]) continue;
763  if (m_verbose)
764  printf(" [%s] calling m_streams[%d]->open()\n", __FUNCTION__, i);
765 
766  if (m_streams[i]->open(w, h, fps) == false)
767  {
768  if (m_verbose)
769  printf(
770  " [%s] m_streams[%d]->open() returned FALSE\n",
771  __FUNCTION__, i);
772  return false;
773  }
774  if (m_verbose)
775  printf(
776  " [%s] m_streams[%d]->open() returned OK\n", __FUNCTION__, i);
777  }
778 
779  if (synchMirrorMode() == false)
780  {
781  close();
782  return false;
783  }
784 
785  if (m_streams[DEPTH_STREAM])
786  {
787  int CloseRange = 0;
788  m_streams[DEPTH_STREAM]->setCloseRange(CloseRange);
789  m_log << " Close range: " << (CloseRange ? "On" : "Off") << std::endl;
790  }
791 
792  if (m_verbose)
793  printf(" DBG: checking if imageRegistrationMode is supported\n");
794  if (m_device.isImageRegistrationModeSupported(
795  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) &&
796  m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid() &&
797  m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid())
798  {
799  // SEB
800  // if(m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF)
801  // != openni::STATUS_OK){
802  if (m_device.setImageRegistrationMode(
803  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) != openni::STATUS_OK)
804  {
805  m_log << " setImageRegistrationMode() Failed:"
806  << openni::OpenNI::getExtendedError() << endl;
807  }
808  else
809  {
810  m_log << " setImageRegistrationMode() Success" << endl;
811  }
812  }
813  else
814  {
815  m_log << " Device doesn't do image registration!" << endl;
816  }
817 
818  if (false) // hasColor())
819  { // printf("DBG: hasColor() returned TRUE\n");
820  m_streams[COLOR_STREAM]->disableAutoExposure();
821  printf("DBG: returned from disableAutoExposure()\n");
822  }
823 
824  if (startStreams() == false)
825  {
826  close();
827  return false;
828  }
829  return true;
830  MRPT_END
831 }
832 
833 bool COpenNI2Generic::CDevice::getNextFrameRGB(
835  bool& there_is_obs, bool& hardware_error)
836 {
837  MRPT_START
838  if (!hasColor())
839  {
840  THROW_EXCEPTION("This OpenNI2 device does not support color imaging");
841  }
842  openni::VideoFrameRef frame;
843  if (m_streams[COLOR_STREAM]->getFrame(
844  frame, timestamp, there_is_obs, hardware_error) == false)
845  {
846  return false;
847  }
848  copyFrame<openni::RGB888Pixel, mrpt::img::CImage>(frame, img);
849 
850  return true;
851  MRPT_END
852 }
853 
854 bool COpenNI2Generic::CDevice::getNextFrameD(
856  bool& there_is_obs, bool& hardware_error)
857 {
858  MRPT_START
859  if (!hasDepth())
860  {
861  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging");
862  }
863  openni::VideoFrameRef frame;
864  if (m_streams[DEPTH_STREAM]->getFrame(
865  frame, timestamp, there_is_obs, hardware_error) == false)
866  {
867  return false;
868  }
869  copyFrame<openni::DepthPixel, mrpt::math::CMatrix_u16>(frame, depth_mm);
870 
871  return true;
872  MRPT_END
873 }
874 
875 bool COpenNI2Generic::CDevice::getNextFrameRGBD(
876  mrpt::obs::CObservation3DRangeScan& obs, bool& there_is_obs,
877  bool& hardware_error)
878 {
879  MRPT_START
880  clearLog();
881  there_is_obs = false;
882  hardware_error = false;
883 
884  if (!hasColor())
885  {
886  THROW_EXCEPTION("This OpenNI2 device does not support color imaging");
887  }
888  if (!hasDepth())
889  {
890  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging");
891  }
892  // Read a frame (depth + rgb)
894  openni::VideoFrameRef frame[STREAM_TYPE_SIZE];
895  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
896  {
897  if (!m_streams[i] || !m_streams[i]->isValid()) continue;
898  if (m_streams[i]->getFrame(
899  frame[i], tm, there_is_obs, hardware_error) == false)
900  {
901  return false;
902  }
903  if (there_is_obs == false || hardware_error == true)
904  {
905  return false;
906  }
907  }
908 
909  const int width = frame[COLOR_STREAM].getWidth();
910  const int height = frame[COLOR_STREAM].getHeight();
911  if ((frame[DEPTH_STREAM].getWidth() != width) ||
912  (frame[DEPTH_STREAM].getHeight() != height))
913  {
914  m_log << "[" << __FUNCTION__ << "]" << std::endl
915  << " Both frames don't have the same size." << std::endl;
916  return false;
917  }
918  there_is_obs = true;
919  obs.hasConfidenceImage = false;
920  obs.hasIntensityImage = true;
921  obs.hasRangeImage = true;
922  obs.range_is_depth = true;
923  obs.hasPoints3D = false;
925  resize(obs, width, height);
926 
927  const char* data[STREAM_TYPE_SIZE] = {
928  (const char*)frame[COLOR_STREAM].getData(),
929  (const char*)frame[DEPTH_STREAM].getData()};
930  const int step[STREAM_TYPE_SIZE] = {frame[COLOR_STREAM].getStrideInBytes(),
931  frame[DEPTH_STREAM].getStrideInBytes()};
932 
933  for (int yc = 0; yc < height; ++yc)
934  {
935  const auto* pRgb = (const openni::RGB888Pixel*)data[COLOR_STREAM];
936  const auto* pDepth = (const openni::DepthPixel*)data[DEPTH_STREAM];
937  for (int xc = 0; xc < width; ++xc, ++pDepth, ++pRgb)
938  {
939  int x = xc;
940  if (isMirrorMode())
941  {
942  x = width - x - 1;
943  }
944  setPixel(*pRgb, obs.intensityImage, x, yc);
945  setPixel(*pDepth, obs.rangeImage, x, yc);
946  }
947  data[COLOR_STREAM] += step[COLOR_STREAM];
948  data[DEPTH_STREAM] += step[DEPTH_STREAM];
949  }
950 
951  return true;
952  MRPT_END
953 }
954 
955 COpenNI2Generic::CDevice::Ptr COpenNI2Generic::CDevice::create(
956  const openni::DeviceInfo& info, openni::PixelFormat rgb,
957  openni::PixelFormat depth, bool verbose)
958 {
959  return std::make_shared<CDevice>(info, rgb, depth, verbose);
960 }
961 
962 bool COpenNI2Generic::CDevice::getSerialNumber(std::string& sn)
963 {
964  clearLog();
965  openni::Status rc;
966  bool isOpened = isOpen();
967  if (isOpened == false)
968  {
969  rc = m_device.open(getInfo().getUri());
970  if (rc != openni::STATUS_OK)
971  {
972  m_log << "[" << __FUNCTION__ << "]" << std::endl
973  << " Failed to open device " << getInfo().getUri() << " "
974  << openni::OpenNI::getExtendedError() << std::endl;
975  return false;
976  }
977  }
978  char serialNumber[16];
979  rc = m_device.getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER, &serialNumber);
980  if (rc != openni::STATUS_OK)
981  {
982  m_log << "[" << __FUNCTION__ << "]" << std::endl
983  << " Failed to getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER) "
984  << getInfo().getUri() << " " << openni::OpenNI::getExtendedError()
985  << std::endl;
986  return false;
987  }
988  sn = std::string(serialNumber);
989  if (isOpened == false)
990  {
991  m_device.close();
992  }
993  return true;
994 }
995 
996 bool COpenNI2Generic::CDevice::getSerialNumber(unsigned int& sn)
997 {
998  std::string str;
999  if (getSerialNumber(str) == false)
1000  {
1001  return false;
1002  }
1003  std::stringstream sst;
1004  sst.str(str);
1005  sst >> sn;
1006  return !sst.fail();
1007 }
1008 //
1010  openni::Device& device, openni::SensorType type, openni::PixelFormat format,
1011  std::ostream& log, bool verbose)
1012  : m_log(log),
1013  m_device(device),
1014  m_strName("Unknown"),
1015  m_type(type),
1016  m_format(format),
1018 {
1019  if (m_type == openni::SENSOR_COLOR)
1020  {
1021  m_strName = "openni::SENSOR_COLOR";
1022  }
1023  else if (m_type == openni::SENSOR_DEPTH)
1024  {
1025  m_strName = "openni::SENSOR_DEPTH";
1026  }
1027  else if (m_type == openni::SENSOR_IR)
1028  {
1029  m_strName = "openni::SENSOR_IR";
1030  }
1031  else
1032  {
1033  m_log << "[" << __FUNCTION__ << "]" << std::endl
1034  << " Unknown SensorType -> " << m_type << std::endl;
1035  }
1036 }
1037 
1038 COpenNI2Generic::CDevice::CStream::~CStream() { destroy(); }
1039 bool COpenNI2Generic::CDevice::CStream::isMirrorSupported() const
1040 {
1041  if (isValid() == false)
1042  {
1043  THROW_EXCEPTION(getName() + " is not opened.");
1044  }
1045  return m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING);
1046 }
1047 
1048 bool COpenNI2Generic::CDevice::CStream::setMirror(bool flag)
1049 {
1050  if (isValid() == false)
1051  {
1052  m_log << "[" << __FUNCTION__ << "]" << std::endl
1053  << " " << getName() << " is not opened." << std::endl;
1054  return false;
1055  }
1056  if (m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING) ==
1057  false)
1058  {
1059  return false;
1060  }
1061  if (m_stream.setMirroringEnabled(flag) != openni::STATUS_OK)
1062  {
1063  m_log << "[" << __FUNCTION__ << "]" << std::endl
1064  << " setMirroringEnabled() failed: "
1065  << openni::OpenNI::getExtendedError() << std::endl;
1066  return false;
1067  }
1068  return true;
1069 }
1070 
1071 bool COpenNI2Generic::CDevice::CStream::isValid() const
1072 {
1073  return m_stream.isValid();
1074 }
1075 
1076 void COpenNI2Generic::CDevice::CStream::destroy() { m_stream.destroy(); }
1077 void COpenNI2Generic::CDevice::CStream::setCloseRange(int& value)
1078 {
1079  if (m_verbose)
1080  printf(
1081  " [CDevice::CStream::setCloseRange] entry with value=%d\n",
1082  value);
1083  m_stream.setProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, value);
1084  if (m_verbose)
1085  printf(
1086  " [CDevice::CStream::setCloseRange] returned from "
1087  "mstream.setProperty()\n");
1088  m_stream.getProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, &value);
1089  if (m_verbose)
1090  printf(
1091  " [CDevice::CStream::setCloseRange] returned from "
1092  "mstream.getProperty() ... value %d\n",
1093  value);
1094 }
1095 
1096 bool COpenNI2Generic::CDevice::CStream::open(int w, int h, int fps)
1097 {
1098  destroy();
1099  if (m_type != openni::SENSOR_COLOR && m_type != openni::SENSOR_DEPTH &&
1100  m_type != openni::SENSOR_IR)
1101  { // SEB added IR
1102  m_log << "[" << __FUNCTION__ << "]" << std::endl
1103  << " Unknown SensorType -> " << m_type << std::endl;
1104  return false;
1105  }
1106  if (m_verbose)
1107  printf(
1108  " [COpenNI2Generic::CDevice::CStream::open] opening sensor "
1109  "stream with m_type == %d\n",
1110  (int)m_type);
1111  openni::Status rc = openni::STATUS_OK;
1112  // if(m_type == openni::SENSOR_COLOR) {
1113  // m_type = openni::SENSOR_IR;
1114  // m_strName="openni::SENSOR_IR"; // SEB added
1115  // if (m_verbose) printf("DBG: changing type to SENSOR_IR
1116  //(%d)\n",(int)m_type);
1117  // } // added whole if stmt
1118  rc = m_stream.create(m_device, m_type);
1119  if (rc != openni::STATUS_OK)
1120  {
1121  m_log << "[" << __FUNCTION__ << "]" << std::endl
1122  << " Couldn't find sensor " << m_strName << ":"
1123  << openni::OpenNI::getExtendedError() << std::endl;
1124  if (m_type == openni::SENSOR_COLOR)
1125  {
1126  m_type = openni::SENSOR_IR;
1127  m_strName = "openni::SENSOR_IR"; // SEB added
1128  if (m_verbose)
1129  printf("DBG: changing type to SENSOR_IR (%d)\n", (int)m_type);
1130  rc = m_stream.create(m_device, m_type);
1131  } // SEB added whole if stmt
1132  else
1133  return false;
1134  }
1135  if (m_verbose) printf("returned OK from stream.create()\n");
1136  openni::VideoMode options = m_stream.getVideoMode();
1137  m_log << "[" << __FUNCTION__ << "]" << std::endl;
1138  m_log << " " << m_strName << std::endl;
1139  m_log << " "
1140  << mrpt::format(
1141  "Initial resolution (%d, %d) FPS %d Format %d",
1142  options.getResolutionX(), options.getResolutionY(),
1143  options.getFps(), options.getPixelFormat())
1144  << std::endl;
1145  if (m_verbose) printf("DBG: calling setONI2StreamMode()\n");
1146  if (setONI2StreamMode(m_stream, w, h, fps, m_format) == false)
1147  {
1148  m_log << " Can't find desired mode in the " << getName() << std::endl;
1149  destroy();
1150  return false;
1151  }
1152  if (m_verbose) printf("DBG: returned OK from setONI2StreamMode()\n");
1153  if (m_verbose) printf("DBG: calling stream.getVideoMode()\n");
1154  options = m_stream.getVideoMode();
1155  m_log << " "
1156  << mrpt::format(
1157  "-> (%d, %d) FPS %d Format %d", options.getResolutionX(),
1158  options.getResolutionY(), options.getFps(),
1159  options.getPixelFormat())
1160  << std::endl;
1161  if (m_verbose)
1162  printf(
1163  " [COpenNI2Generic::CDevice::CStream::open] returning TRUE\n");
1164  return true;
1165 }
1166 
1167 bool COpenNI2Generic::CDevice::CStream::start()
1168 {
1169  if (isValid() == false)
1170  {
1171  m_log << "[" << __FUNCTION__ << "]" << std::endl
1172  << " " << getName() << " is not opened." << std::endl;
1173  return false;
1174  }
1175  if (m_stream.start() != openni::STATUS_OK)
1176  {
1177  m_log << "[" << __FUNCTION__ << "]" << std::endl
1178  << " Couldn't start " << getName()
1179  << " stream:" << openni::OpenNI::getExtendedError() << std::endl;
1180  this->destroy();
1181  return false;
1182  }
1183  return true;
1184 }
1185 
1186 COpenNI2Generic::CDevice::CStream::Ptr
1187  COpenNI2Generic::CDevice::CStream::create(
1188  openni::Device& device, openni::SensorType type,
1189  openni::PixelFormat format, std::ostream& log, bool verbose)
1190 {
1191  return std::make_shared<CStream>(device, type, format, log, verbose);
1192 }
1193 
1194 bool COpenNI2Generic::CDevice::CStream::getFrame(
1195  openni::VideoFrameRef& frame, mrpt::system::TTimeStamp& timestamp,
1196  bool& there_is_obs, bool& hardware_error)
1197 {
1198  there_is_obs = false;
1199  hardware_error = false;
1200  if (isValid() == false)
1201  {
1202  return false;
1203  }
1204  openni::Status rc = m_stream.readFrame(&frame);
1205  if (rc != openni::STATUS_OK)
1206  {
1207  hardware_error = true;
1208  std::string message =
1209  mrpt::format("Failed to grab frame from %s", getName().c_str());
1210  THROW_EXCEPTION(message);
1211  }
1212  there_is_obs = true;
1213  timestamp = mrpt::system::getCurrentTime();
1214  return true;
1215 }
1216 
1217 #endif // MRPT_HAS_OPENNI2
#define MRPT_START
Definition: exceptions.h:241
int getNumDevices() const
The number of available devices at initialization.
bool getDepthSensorParam(mrpt::img::TCamera &param, unsigned sensor_id=0) const
app initialize(argc, argv)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
void getNextFrameRGB(mrpt::img::CImage &rgb_img, mrpt::system::TTimeStamp &timestamp, bool &there_is_obs, bool &hardware_error, unsigned sensor_id=0)
The main data retrieving function, to be called after calling loadConfig() and initialize().
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
void getNextFrameRGBD(mrpt::obs::CObservation3DRangeScan &out_obs, bool &there_is_obs, bool &hardware_error, unsigned sensor_id=0)
The main data retrieving function, to be called after calling loadConfig() and initialize().
Contains classes for various device interfaces.
COpenNI2Generic()
Default ctor (width=640, height=480, fps=30)
STL namespace.
mrpt::io::CStream CStream
Definition: utils/CStream.h:5
int m_width
The same options (width, height and fps) are set for all the sensors.
void kill()
Kill the OpenNI2 driver.
std::vector< std::shared_ptr< COpenNI2Generic::CDevice > > vDevices
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
unsigned int openDeviceBySerial(const unsigned int SerialRequired)
Open a RGBD device specified by its serial number.
bool start()
Open all sensor streams (normally called automatically at constructor, no need to call it manually)...
void getNextFrameD(mrpt::math::CMatrix_u16 &depth_img_mm, mrpt::system::TTimeStamp &timestamp, bool &there_is_obs, bool &hardware_error, unsigned sensor_id=0)
The main data retrieving function, to be called after calling loadConfig() and initialize().
bool getColorSensorParam(mrpt::img::TCamera &param, unsigned sensor_id=0) const
bool isOpen(const unsigned sensor_id) const
Whether there is a working connection to the sensor.
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
static int getNumInstances()
Get the number of OpenNI2 cameras currently open via COpenNI2Generic.
This namespace contains representation of robot actions and observations.
bool hasRangeImage
true means the field rangeImage contains valid data
void close(unsigned sensor_id=0)
Close the connection to the sensor (no need to call it manually unless desired for some reason...
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:25
std::atomic< int > numInstances(0)
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
bool hasPoints3D
true means the field points3D contains valid data.
unsigned int openDevicesBySerialNum(const std::set< unsigned > &vSerialRequired)
Open a set of RGBD devices specified by their serial number.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
bool hasIntensityImage
true means the field intensityImage contains valid data
#define MRPT_END
Definition: exceptions.h:245
std::recursive_mutex vDevices_mx
void open(unsigned sensor_id=0)
Try to open the camera (all the parameters [resolution,fps,...] must be set before calling this) - us...
bool getDeviceIDFromSerialNum(const unsigned int SerialRequired, int &sensor_id) const
Get the ID of the device corresponding to &#39;SerialRequired&#39;.
int getConnectedDevices()
Get a list of the connected OpenNI2 sensors.
void showLog(const std::string &message) const
double getHeight(const TPolygon3D &p, const TPoint3D &c)
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
images resize(NUM_IMGS)
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
bool hasConfidenceImage
true means the field confidenceImage contains valid data
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 338471620 Mon Feb 17 00:12:39 2020 +0100 at lun feb 17 00:15:10 CET 2020