MRPT  2.0.4
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-2020, 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([[maybe_unused]] 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  return false;
255 #endif // MRPT_HAS_OPENNI2
256 }
257 
258 void COpenNI2Generic::open([[maybe_unused]] unsigned sensor_id)
259 {
260 #if MRPT_HAS_OPENNI2
261  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
262  // Sensor index validation.
263  if (!getNumDevices())
264  {
265  THROW_EXCEPTION("No OpenNI2 devices found.");
266  }
267  if ((int)sensor_id >= getNumDevices())
268  {
270  "Sensor index is higher than the number of connected devices.");
271  }
272  showLog(mrpt::format("[%s]\n", __FUNCTION__));
273  showLog(mrpt::format(" open[%d] ...\n", sensor_id));
274 
275  if (isOpen(sensor_id))
276  {
277  showLog(
278  mrpt::format(" The sensor [%d] is already opened\n", sensor_id));
279  return;
280  }
281  if (m_verbose)
282  printf(
283  "[COpenNI2Generic] DBG: [%s] about to call vDevices[%d]->open()\n",
284  __FUNCTION__, sensor_id);
285  vDevices[sensor_id]->open(m_width, m_height, m_fps);
286  showLog(vDevices[sensor_id]->getLog() + "\n");
287  showLog(mrpt::format(" Device [%d] ", sensor_id));
288  if (vDevices[sensor_id]->isOpen())
289  {
290  showLog(" open successfully.\n");
291  }
292  else
293  {
294  showLog(" open failed.\n");
295  }
296  std::this_thread::sleep_for(1000ms); // Sleep
297 #else
298  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
299 #endif // MRPT_HAS_OPENNI2
300 }
301 
303  [maybe_unused]] const std::set<unsigned>& serial_required)
304 {
305 #if MRPT_HAS_OPENNI2
306  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
307  showLog(mrpt::format("[%s]\n", __FUNCTION__));
308  unsigned num_open_dev = 0;
309  for (unsigned sensor_id = 0; sensor_id < vDevices.size(); sensor_id++)
310  {
311  unsigned int serialNum;
312  if (vDevices[sensor_id]->getSerialNumber(serialNum) == false)
313  {
314  showLog(vDevices[sensor_id]->getLog());
315  continue;
316  }
317  if (m_verbose)
318  printf(
319  "[COpenNI2Generic::openDevicesBySerialNum] checking device "
320  "with serial '%d'\n",
321  serialNum);
322 
323  if (serial_required.find(serialNum) == serial_required.end())
324  {
325  vDevices[sensor_id]->close();
326  continue;
327  }
328  if (vDevices[sensor_id]->isOpen())
329  {
330  num_open_dev++;
331  continue;
332  }
333  int width = m_width;
334  int height = m_height;
335  if (m_verbose)
336  printf(
337  "[COpenNI2Generic] DBG: [%s] about to call "
338  "vDevices[%d]->open(%d,%d,%d)\n",
339  __FUNCTION__, sensor_id, width, height, (int)m_fps);
340  if (vDevices[sensor_id]->open(width, height, m_fps) == false)
341  {
342  showLog(vDevices[sensor_id]->getLog());
343  continue;
344  }
345  num_open_dev++;
346  if (m_verbose)
347  printf(
348  "[COpenNI2Generic] DBG: [%s] now has %d devices open\n",
349  __FUNCTION__, num_open_dev);
350  }
351  return num_open_dev;
352 #else
353  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
354 #endif // MRPT_HAS_OPENNI2
355 }
356 
358  const unsigned int SerialRequired)
359 {
360  std::set<unsigned> serial_required;
361  serial_required.insert(SerialRequired);
362  return openDevicesBySerialNum(serial_required);
363 }
364 
366  [[maybe_unused]] const unsigned int SerialRequired,
367  [[maybe_unused]] int& sensor_id) const
368 {
369 #if MRPT_HAS_OPENNI2
370  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
371  for (size_t i = 0, i_end = vDevices.size(); i < i_end; ++i)
372  {
373  unsigned int sn;
374  if (vDevices[i]->getSerialNumber(sn) == false)
375  {
376  continue;
377  }
378  if (sn == SerialRequired)
379  {
380  sensor_id = (int)i;
381  return true;
382  }
383  }
384  return false;
385 #else
386  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
387 #endif // MRPT_HAS_OPENNI2
388 }
389 
390 void COpenNI2Generic::close([[maybe_unused]] unsigned sensor_id)
391 {
392 #if MRPT_HAS_OPENNI2
393  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
394  // Sensor index validation.
395  if (!getNumDevices())
396  {
397  THROW_EXCEPTION("No OpenNI2 devices found.");
398  }
399  if ((int)sensor_id >= getNumDevices())
400  {
402  "Sensor index is higher than the number of connected devices.");
403  }
404  vDevices[sensor_id]->close();
405 #else
406  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
407 #endif // MRPT_HAS_OPENNI2
408 }
409 
410 /** The main data retrieving function, to be called after calling loadConfig()
411  * and initialize().
412  * \param out_obs The output retrieved observation (only if there_is_obs=true).
413  * \param timestamp The timestamp of the capture (only if there_is_obs=true).
414  * \param there_is_obs If set to false, there was no new observation.
415  * \param hardware_error True on hardware/comms error.
416  * \param sensor_id The index of the sensor accessed.
417  *
418  */
420  [[maybe_unused]] mrpt::img::CImage& rgb_img,
421  [[maybe_unused]] mrpt::system::TTimeStamp& timestamp,
422  [[maybe_unused]] bool& there_is_obs, [[maybe_unused]] bool& hardware_error,
423  [[maybe_unused]] unsigned sensor_id)
424 {
425 #if MRPT_HAS_OPENNI2
426  // Sensor index validation.
427  if (!getNumDevices())
428  {
429  THROW_EXCEPTION("No OpenNI2 devices found.");
430  }
431  if ((int)sensor_id >= getNumDevices())
432  {
434  "Sensor index is higher than the number of connected devices.");
435  }
436  if (vDevices[sensor_id]->getNextFrameRGB(
437  rgb_img, timestamp, there_is_obs, hardware_error) == false)
438  {
439  showLog(mrpt::format("[%s]\n", __FUNCTION__));
440  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
441  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
442  }
443 #else
444  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
445 #endif // MRPT_HAS_OPENNI2
446 }
447 
449  [[maybe_unused]] mrpt::math::CMatrix_u16& depth_img_mm,
450  [[maybe_unused]] mrpt::system::TTimeStamp& timestamp,
451  [[maybe_unused]] bool& there_is_obs, [[maybe_unused]] bool& hardware_error,
452  [[maybe_unused]] unsigned sensor_id)
453 {
454 #if MRPT_HAS_OPENNI2
455  // Sensor index validation.
456  if (getNumDevices() == 0)
457  {
458  THROW_EXCEPTION("No OpenNI2 devices found.");
459  }
460  if ((int)sensor_id >= getNumDevices())
461  {
463  "Sensor index is higher than the number of connected devices.");
464  }
465  if (vDevices[sensor_id]->getNextFrameD(
466  depth_img_mm, timestamp, there_is_obs, hardware_error) == false)
467  {
468  showLog(mrpt::format("[%s]\n", __FUNCTION__));
469  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
470  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
471  }
472 #else
473  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
474 #endif // MRPT_HAS_OPENNI2
475 }
476 
477 /** The main data retrieving function, to be called after calling loadConfig()
478  * and initialize().
479  * \param out_obs The output retrieved observation (only if there_is_obs=true).
480  * \param there_is_obs If set to false, there was no new observation.
481  * \param hardware_error True on hardware/comms error.
482  * \param sensor_id The index of the sensor accessed.
483  *
484  */
486  [[maybe_unused]] mrpt::obs::CObservation3DRangeScan& out_obs,
487  [[maybe_unused]] bool& there_is_obs, [[maybe_unused]] bool& hardware_error,
488  [[maybe_unused]] unsigned sensor_id)
489 {
490 #if MRPT_HAS_OPENNI2
491  // Sensor index validation.
492  if (!getNumDevices())
493  {
494  THROW_EXCEPTION("No OpenNI2 devices found.");
495  }
496  if ((int)sensor_id >= getNumDevices())
497  {
499  "Sensor index is higher than the number of connected devices.");
500  }
501  if (vDevices[sensor_id]->getNextFrameRGBD(
502  out_obs, there_is_obs, hardware_error) == false)
503  {
504  showLog(mrpt::format("[%s]\n", __FUNCTION__));
505  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
506  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
507  }
508 #else
509  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
510 #endif // MRPT_HAS_OPENNI2
511 }
512 
514  [[maybe_unused]] mrpt::img::TCamera& param,
515  [[maybe_unused]] unsigned sensor_id) const
516 {
517 #if MRPT_HAS_OPENNI2
518  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
519  if (isOpen(sensor_id) == false)
520  {
521  return false;
522  }
523  return vDevices[sensor_id]->getCameraParam(CDevice::COLOR_STREAM, param);
524 #else
525  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
526 #endif // MRPT_HAS_OPENNI2
527 }
528 
530  [[maybe_unused]] mrpt::img::TCamera& param,
531  [[maybe_unused]] unsigned sensor_id) const
532 {
533 #if MRPT_HAS_OPENNI2
534  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
535  if (isOpen(sensor_id) == false)
536  {
537  return false;
538  }
539  return vDevices[sensor_id]->getCameraParam(CDevice::DEPTH_STREAM, param);
540 #else
541  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
542 #endif // MRPT_HAS_OPENNI2
543 }
544 
545 #if MRPT_HAS_OPENNI2
546 /*
547 void openni::VideoMode::setResolution()
548 Setter function for the resolution of this VideoMode. Application use of this
549 function is not recommended.
550 Instead, use SensorInfo::getSupportedVideoModes() to obtain a list of valid
551 video modes
552 
553 -- cited from OpenNI2 help. setResolution() is not recommended.
554 */
555 bool setONI2StreamMode(
556  openni::VideoStream& stream, int w, int h, int fps,
557  openni::PixelFormat format)
558 {
559  // std::cout << "[COpenNI2Generic] Ask mode: " << w << "x" << h << " " <<
560  // fps << " fps. format " << format << std::endl;
561  bool found = false;
562  const openni::Array<openni::VideoMode>& modes =
563  stream.getSensorInfo().getSupportedVideoModes();
564  for (int i = 0, i_end = modes.getSize(); i < i_end; ++i)
565  {
566  // if (m_verbose) std::cout << "[COpenNI2Generic] Mode: " <<
567  // modes[i].getResolutionX() << "x" << modes[i].getResolutionY() << " "
568  // << modes[i].getFps() << " fps. format " << modes[i].getPixelFormat()
569  // << std::endl;
570  if (modes[i].getResolutionX() != w)
571  {
572  continue;
573  }
574  if (modes[i].getResolutionY() != h)
575  {
576  continue;
577  }
578  if (modes[i].getFps() != fps)
579  {
580  continue;
581  }
582  if (modes[i].getPixelFormat() != format)
583  {
584  continue;
585  }
586  openni::Status rc = stream.setVideoMode(modes[i]);
587  if (rc != openni::STATUS_OK)
588  {
589  return false;
590  }
591  return true;
592  }
593  return false;
594 }
595 
596 std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab)
597 {
598  std::stringstream sst;
599  std::string space;
600  for (int i = 0; i < tab; ++i)
601  {
602  space += " ";
603  }
604  sst << space << "name=" << info.getName() << std::endl;
605  sst << space << "uri=" << info.getUri() << std::endl;
606  sst << space << "vendor=" << info.getVendor() << std::endl;
607  sst << space << "product=" << info.getUsbProductId();
608  return sst.str();
609 }
610 
611 bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2)
612 {
613  return (strcmp(i1.getUri(), i2.getUri()) == 0);
614 }
615 //
616 COpenNI2Generic::CDevice::CDevice(
617  const openni::DeviceInfo& info, openni::PixelFormat rgb,
618  openni::PixelFormat depth, bool verbose)
619  : m_info(info), m_mirror(true), m_verbose(verbose)
620 {
621  m_streams[COLOR_STREAM] =
622  CStream::create(m_device, openni::SENSOR_COLOR, rgb, m_log, m_verbose);
623  m_streams[IR_STREAM] =
624  CStream::create(m_device, openni::SENSOR_IR, rgb, m_log, m_verbose);
625  m_streams[DEPTH_STREAM] = CStream::create(
626  m_device, openni::SENSOR_DEPTH, depth, m_log, m_verbose);
627 }
628 
629 COpenNI2Generic::CDevice::~CDevice() { close(); }
630 bool COpenNI2Generic::CDevice::synchMirrorMode()
631 {
632  m_mirror = false;
633  // Check whether both stream support mirroring.
634  for (auto& m_stream : m_streams)
635  {
636  if (!m_stream) continue;
637  bool mirror_support;
638  try
639  {
640  mirror_support = m_stream->isMirrorSupported();
641  }
642  catch (std::logic_error& e)
643  {
644  throw(e);
645  }
646  if (mirror_support == false)
647  {
648  m_log << "[" << __FUNCTION__ << "]" << std::endl;
649  m_log << " openni::STREAM_PROPERTY_MIRRORING is not supported on "
650  << m_stream->getName() << "." << std::endl;
651  m_log << " We assume this is MS Kinect and taken images are "
652  "inverted to right and left."
653  << std::endl;
654  // In this case, getMirroringEnabled() method always returns false.
655  // So we cannot confirm whether the images are inverted or not.
656  m_mirror = true;
657  break;
658  }
659  }
660  // Set both stream to same mirror mode.
661  for (auto& m_stream : m_streams)
662  {
663  if (!m_stream) continue;
664  if (m_stream->isMirrorSupported() == false)
665  {
666  break;
667  }
668  if (m_stream->setMirror(m_mirror) == false)
669  {
670  return false;
671  }
672  }
673  return true;
674 }
675 
676 bool COpenNI2Generic::CDevice::startStreams()
677 {
678  MRPT_START
679  int num_ok = 0;
680  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
681  {
682  if (!m_streams[i]) continue;
683  if (m_verbose)
684  printf(" [%s] calling m_streams[%d]->start()\n", __FUNCTION__, i);
685  if (m_streams[i]->start() == false)
686  {
687  if (m_verbose)
688  printf(
689  " [%s] m_streams[%d]->start() returned FALSE!\n",
690  __FUNCTION__, i);
691  }
692  else
693  {
694  num_ok++;
695  }
696  if (m_verbose)
697  printf(
698  " [%s] m_streams[%d]->start() returned TRUE\n", __FUNCTION__,
699  i);
700  }
701  if (m_verbose)
702  printf(
703  " [COpenNI2Generic::CDevice::startStreams()] %d streams were "
704  "started.\n",
705  num_ok);
706  return num_ok > 0;
707  MRPT_END
708 }
709 
710 bool COpenNI2Generic::CDevice::isOpen() const
711 {
712  return (m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid()) ||
713  (m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid());
714 }
715 
716 void COpenNI2Generic::CDevice::close()
717 {
718  for (auto& m_stream : m_streams)
719  {
720  if (!m_stream) continue;
721  m_stream->destroy();
722  }
723  m_device.close();
724 }
725 
726 bool COpenNI2Generic::CDevice::open(int w, int h, int fps)
727 {
728  MRPT_START
729  if (m_verbose)
730  printf(
731  " [COpenNI2Generic::CDevice::open()] Called with w=%i h=%i "
732  "fps=%i\n",
733  w, h, fps);
734  clearLog();
735  close();
736  openni::Status rc = m_device.open(getInfo().getUri());
737  if (rc != openni::STATUS_OK)
738  {
739  m_log << "[" << __FUNCTION__ << "]" << std::endl
740  << " Failed to open device " << getInfo().getUri() << " "
741  << openni::OpenNI::getExtendedError() << std::endl;
742  return false;
743  }
744  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
745  {
746  if (!m_streams[i]) continue;
747  if (m_verbose)
748  printf(" [%s] calling m_streams[%d]->open()\n", __FUNCTION__, i);
749 
750  if (m_streams[i]->open(w, h, fps) == false)
751  {
752  if (m_verbose)
753  printf(
754  " [%s] m_streams[%d]->open() returned FALSE\n",
755  __FUNCTION__, i);
756  return false;
757  }
758  if (m_verbose)
759  printf(
760  " [%s] m_streams[%d]->open() returned OK\n", __FUNCTION__, i);
761  }
762 
763  if (synchMirrorMode() == false)
764  {
765  close();
766  return false;
767  }
768 
769  if (m_streams[DEPTH_STREAM])
770  {
771  int CloseRange = 0;
772  m_streams[DEPTH_STREAM]->setCloseRange(CloseRange);
773  m_log << " Close range: " << (CloseRange ? "On" : "Off") << std::endl;
774  }
775 
776  if (m_verbose)
777  printf(" DBG: checking if imageRegistrationMode is supported\n");
778  if (m_device.isImageRegistrationModeSupported(
779  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) &&
780  m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid() &&
781  m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid())
782  {
783  // SEB
784  // if(m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF)
785  // != openni::STATUS_OK){
786  if (m_device.setImageRegistrationMode(
787  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) != openni::STATUS_OK)
788  {
789  m_log << " setImageRegistrationMode() Failed:"
790  << openni::OpenNI::getExtendedError() << endl;
791  }
792  else
793  {
794  m_log << " setImageRegistrationMode() Success" << endl;
795  }
796  }
797  else
798  {
799  m_log << " Device doesn't do image registration!" << endl;
800  }
801 
802  if (false) // hasColor())
803  { // printf("DBG: hasColor() returned TRUE\n");
804  m_streams[COLOR_STREAM]->disableAutoExposure();
805  printf("DBG: returned from disableAutoExposure()\n");
806  }
807 
808  if (startStreams() == false)
809  {
810  close();
811  return false;
812  }
813  return true;
814  MRPT_END
815 }
816 
817 bool COpenNI2Generic::CDevice::getNextFrameRGB(
819  bool& there_is_obs, bool& hardware_error)
820 {
821  MRPT_START
822  if (!hasColor())
823  {
824  THROW_EXCEPTION("This OpenNI2 device does not support color imaging");
825  }
826  openni::VideoFrameRef frame;
827  if (m_streams[COLOR_STREAM]->getFrame(
828  frame, timestamp, there_is_obs, hardware_error) == false)
829  {
830  return false;
831  }
832  copyFrame<openni::RGB888Pixel, mrpt::img::CImage>(frame, img);
833 
834  return true;
835  MRPT_END
836 }
837 
838 bool COpenNI2Generic::CDevice::getNextFrameD(
840  bool& there_is_obs, bool& hardware_error)
841 {
842  MRPT_START
843  if (!hasDepth())
844  {
845  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging");
846  }
847  openni::VideoFrameRef frame;
848  if (m_streams[DEPTH_STREAM]->getFrame(
849  frame, timestamp, there_is_obs, hardware_error) == false)
850  {
851  return false;
852  }
853  copyFrame<openni::DepthPixel, mrpt::math::CMatrix_u16>(frame, depth_mm);
854 
855  return true;
856  MRPT_END
857 }
858 
859 bool COpenNI2Generic::CDevice::getNextFrameRGBD(
860  mrpt::obs::CObservation3DRangeScan& obs, bool& there_is_obs,
861  bool& hardware_error)
862 {
863  MRPT_START
864  clearLog();
865  there_is_obs = false;
866  hardware_error = false;
867 
868  if (!hasColor())
869  {
870  THROW_EXCEPTION("This OpenNI2 device does not support color imaging");
871  }
872  if (!hasDepth())
873  {
874  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging");
875  }
876  // Read a frame (depth + rgb)
878  openni::VideoFrameRef frame[STREAM_TYPE_SIZE];
879  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
880  {
881  if (!m_streams[i] || !m_streams[i]->isValid()) continue;
882  if (m_streams[i]->getFrame(
883  frame[i], tm, there_is_obs, hardware_error) == false)
884  {
885  return false;
886  }
887  if (there_is_obs == false || hardware_error == true)
888  {
889  return false;
890  }
891  }
892 
893  const int width = frame[COLOR_STREAM].getWidth();
894  const int height = frame[COLOR_STREAM].getHeight();
895  if ((frame[DEPTH_STREAM].getWidth() != width) ||
896  (frame[DEPTH_STREAM].getHeight() != height))
897  {
898  m_log << "[" << __FUNCTION__ << "]" << std::endl
899  << " Both frames don't have the same size." << std::endl;
900  return false;
901  }
902  there_is_obs = true;
903  obs.hasConfidenceImage = false;
904  obs.hasIntensityImage = true;
905  obs.hasRangeImage = true;
906  obs.range_is_depth = true;
907  obs.hasPoints3D = false;
909  resize(obs, width, height);
910 
911  const char* data[STREAM_TYPE_SIZE] = {
912  (const char*)frame[COLOR_STREAM].getData(),
913  (const char*)frame[DEPTH_STREAM].getData()};
914  const int step[STREAM_TYPE_SIZE] = {frame[COLOR_STREAM].getStrideInBytes(),
915  frame[DEPTH_STREAM].getStrideInBytes()};
916 
917  for (int yc = 0; yc < height; ++yc)
918  {
919  const auto* pRgb = (const openni::RGB888Pixel*)data[COLOR_STREAM];
920  const auto* pDepth = (const openni::DepthPixel*)data[DEPTH_STREAM];
921  for (int xc = 0; xc < width; ++xc, ++pDepth, ++pRgb)
922  {
923  int x = xc;
924  if (isMirrorMode())
925  {
926  x = width - x - 1;
927  }
928  setPixel(*pRgb, obs.intensityImage, x, yc);
929  setPixel(*pDepth, obs.rangeImage, x, yc);
930  }
931  data[COLOR_STREAM] += step[COLOR_STREAM];
932  data[DEPTH_STREAM] += step[DEPTH_STREAM];
933  }
934 
935  return true;
936  MRPT_END
937 }
938 
939 COpenNI2Generic::CDevice::Ptr COpenNI2Generic::CDevice::create(
940  const openni::DeviceInfo& info, openni::PixelFormat rgb,
941  openni::PixelFormat depth, bool verbose)
942 {
943  return std::make_shared<CDevice>(info, rgb, depth, verbose);
944 }
945 
946 bool COpenNI2Generic::CDevice::getSerialNumber(std::string& sn)
947 {
948  clearLog();
949  openni::Status rc;
950  bool isOpened = isOpen();
951  if (isOpened == false)
952  {
953  rc = m_device.open(getInfo().getUri());
954  if (rc != openni::STATUS_OK)
955  {
956  m_log << "[" << __FUNCTION__ << "]" << std::endl
957  << " Failed to open device " << getInfo().getUri() << " "
958  << openni::OpenNI::getExtendedError() << std::endl;
959  return false;
960  }
961  }
962  char serialNumber[16];
963  rc = m_device.getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER, &serialNumber);
964  if (rc != openni::STATUS_OK)
965  {
966  m_log << "[" << __FUNCTION__ << "]" << std::endl
967  << " Failed to getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER) "
968  << getInfo().getUri() << " " << openni::OpenNI::getExtendedError()
969  << std::endl;
970  return false;
971  }
972  sn = std::string(serialNumber);
973  if (isOpened == false)
974  {
975  m_device.close();
976  }
977  return true;
978 }
979 
980 bool COpenNI2Generic::CDevice::getSerialNumber(unsigned int& sn)
981 {
982  std::string str;
983  if (getSerialNumber(str) == false)
984  {
985  return false;
986  }
987  std::stringstream sst;
988  sst.str(str);
989  sst >> sn;
990  return !sst.fail();
991 }
992 //
994  openni::Device& device, openni::SensorType type, openni::PixelFormat format,
995  std::ostream& log, bool verbose)
996  : m_log(log),
997  m_device(device),
998  m_strName("Unknown"),
999  m_type(type),
1000  m_format(format),
1002 {
1003  if (m_type == openni::SENSOR_COLOR)
1004  {
1005  m_strName = "openni::SENSOR_COLOR";
1006  }
1007  else if (m_type == openni::SENSOR_DEPTH)
1008  {
1009  m_strName = "openni::SENSOR_DEPTH";
1010  }
1011  else if (m_type == openni::SENSOR_IR)
1012  {
1013  m_strName = "openni::SENSOR_IR";
1014  }
1015  else
1016  {
1017  m_log << "[" << __FUNCTION__ << "]" << std::endl
1018  << " Unknown SensorType -> " << m_type << std::endl;
1019  }
1020 }
1021 
1022 COpenNI2Generic::CDevice::CStream::~CStream() { destroy(); }
1023 bool COpenNI2Generic::CDevice::CStream::isMirrorSupported() const
1024 {
1025  if (isValid() == false)
1026  {
1027  THROW_EXCEPTION(getName() + " is not opened.");
1028  }
1029  return m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING);
1030 }
1031 
1032 bool COpenNI2Generic::CDevice::CStream::setMirror(bool flag)
1033 {
1034  if (isValid() == false)
1035  {
1036  m_log << "[" << __FUNCTION__ << "]" << std::endl
1037  << " " << getName() << " is not opened." << std::endl;
1038  return false;
1039  }
1040  if (m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING) ==
1041  false)
1042  {
1043  return false;
1044  }
1045  if (m_stream.setMirroringEnabled(flag) != openni::STATUS_OK)
1046  {
1047  m_log << "[" << __FUNCTION__ << "]" << std::endl
1048  << " setMirroringEnabled() failed: "
1049  << openni::OpenNI::getExtendedError() << std::endl;
1050  return false;
1051  }
1052  return true;
1053 }
1054 
1055 bool COpenNI2Generic::CDevice::CStream::isValid() const
1056 {
1057  return m_stream.isValid();
1058 }
1059 
1060 void COpenNI2Generic::CDevice::CStream::destroy() { m_stream.destroy(); }
1061 void COpenNI2Generic::CDevice::CStream::setCloseRange(int& value)
1062 {
1063  if (m_verbose)
1064  printf(
1065  " [CDevice::CStream::setCloseRange] entry with value=%d\n",
1066  value);
1067  m_stream.setProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, value);
1068  if (m_verbose)
1069  printf(
1070  " [CDevice::CStream::setCloseRange] returned from "
1071  "mstream.setProperty()\n");
1072  m_stream.getProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, &value);
1073  if (m_verbose)
1074  printf(
1075  " [CDevice::CStream::setCloseRange] returned from "
1076  "mstream.getProperty() ... value %d\n",
1077  value);
1078 }
1079 
1080 bool COpenNI2Generic::CDevice::CStream::open(int w, int h, int fps)
1081 {
1082  destroy();
1083  if (m_type != openni::SENSOR_COLOR && m_type != openni::SENSOR_DEPTH &&
1084  m_type != openni::SENSOR_IR)
1085  { // SEB added IR
1086  m_log << "[" << __FUNCTION__ << "]" << std::endl
1087  << " Unknown SensorType -> " << m_type << std::endl;
1088  return false;
1089  }
1090  if (m_verbose)
1091  printf(
1092  " [COpenNI2Generic::CDevice::CStream::open] opening sensor "
1093  "stream with m_type == %d\n",
1094  (int)m_type);
1095  openni::Status rc = openni::STATUS_OK;
1096  // if(m_type == openni::SENSOR_COLOR) {
1097  // m_type = openni::SENSOR_IR;
1098  // m_strName="openni::SENSOR_IR"; // SEB added
1099  // if (m_verbose) printf("DBG: changing type to SENSOR_IR
1100  //(%d)\n",(int)m_type);
1101  // } // added whole if stmt
1102  rc = m_stream.create(m_device, m_type);
1103  if (rc != openni::STATUS_OK)
1104  {
1105  m_log << "[" << __FUNCTION__ << "]" << std::endl
1106  << " Couldn't find sensor " << m_strName << ":"
1107  << openni::OpenNI::getExtendedError() << std::endl;
1108  if (m_type == openni::SENSOR_COLOR)
1109  {
1110  m_type = openni::SENSOR_IR;
1111  m_strName = "openni::SENSOR_IR"; // SEB added
1112  if (m_verbose)
1113  printf("DBG: changing type to SENSOR_IR (%d)\n", (int)m_type);
1114  rc = m_stream.create(m_device, m_type);
1115  } // SEB added whole if stmt
1116  else
1117  return false;
1118  }
1119  if (m_verbose) printf("returned OK from stream.create()\n");
1120  openni::VideoMode options = m_stream.getVideoMode();
1121  m_log << "[" << __FUNCTION__ << "]" << std::endl;
1122  m_log << " " << m_strName << std::endl;
1123  m_log << " "
1124  << mrpt::format(
1125  "Initial resolution (%d, %d) FPS %d Format %d",
1126  options.getResolutionX(), options.getResolutionY(),
1127  options.getFps(), options.getPixelFormat())
1128  << std::endl;
1129  if (m_verbose) printf("DBG: calling setONI2StreamMode()\n");
1130  if (setONI2StreamMode(m_stream, w, h, fps, m_format) == false)
1131  {
1132  m_log << " Can't find desired mode in the " << getName() << std::endl;
1133  destroy();
1134  return false;
1135  }
1136  if (m_verbose) printf("DBG: returned OK from setONI2StreamMode()\n");
1137  if (m_verbose) printf("DBG: calling stream.getVideoMode()\n");
1138  options = m_stream.getVideoMode();
1139  m_log << " "
1140  << mrpt::format(
1141  "-> (%d, %d) FPS %d Format %d", options.getResolutionX(),
1142  options.getResolutionY(), options.getFps(),
1143  options.getPixelFormat())
1144  << std::endl;
1145  if (m_verbose)
1146  printf(
1147  " [COpenNI2Generic::CDevice::CStream::open] returning TRUE\n");
1148  return true;
1149 }
1150 
1151 bool COpenNI2Generic::CDevice::CStream::start()
1152 {
1153  if (isValid() == false)
1154  {
1155  m_log << "[" << __FUNCTION__ << "]" << std::endl
1156  << " " << getName() << " is not opened." << std::endl;
1157  return false;
1158  }
1159  if (m_stream.start() != openni::STATUS_OK)
1160  {
1161  m_log << "[" << __FUNCTION__ << "]" << std::endl
1162  << " Couldn't start " << getName()
1163  << " stream:" << openni::OpenNI::getExtendedError() << std::endl;
1164  this->destroy();
1165  return false;
1166  }
1167  return true;
1168 }
1169 
1170 COpenNI2Generic::CDevice::CStream::Ptr
1171  COpenNI2Generic::CDevice::CStream::create(
1172  openni::Device& device, openni::SensorType type,
1173  openni::PixelFormat format, std::ostream& log, bool verbose)
1174 {
1175  return std::make_shared<CStream>(device, type, format, log, verbose);
1176 }
1177 
1178 bool COpenNI2Generic::CDevice::CStream::getFrame(
1179  openni::VideoFrameRef& frame, mrpt::system::TTimeStamp& timestamp,
1180  bool& there_is_obs, bool& hardware_error)
1181 {
1182  there_is_obs = false;
1183  hardware_error = false;
1184  if (isValid() == false)
1185  {
1186  return false;
1187  }
1188  openni::Status rc = m_stream.readFrame(&frame);
1189  if (rc != openni::STATUS_OK)
1190  {
1191  hardware_error = true;
1192  std::string message =
1193  mrpt::format("Failed to grab frame from %s", getName().c_str());
1194  THROW_EXCEPTION(message);
1195  }
1196  there_is_obs = true;
1197  timestamp = mrpt::system::getCurrentTime();
1198  return true;
1199 }
1200 
1201 #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().
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
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:26
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:148
bool hasConfidenceImage
true means the field confidenceImage contains valid data
static struct FontData data
Definition: gltext.cpp:144



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: c4d914402 Wed Jun 3 12:47:17 2020 +0200 at miƩ jun 3 13:00:13 CEST 2020