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/otherlibs/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 
456 /** The main data retrieving function, to be called after calling loadConfig()
457  * and initialize().
458  * \param depth_img The output retrieved depth image (only if
459  * there_is_obs=true).
460  * \param timestamp The timestamp of the capture (only if there_is_obs=true).
461  * \param there_is_obs If set to false, there was no new observation.
462  * \param hardware_error True on hardware/comms error.
463  * \param sensor_id The index of the sensor accessed.
464  *
465  */
467  mrpt::math::CMatrixF& depth_img, mrpt::system::TTimeStamp& timestamp,
468  bool& there_is_obs, bool& hardware_error, unsigned sensor_id)
469 {
470 #if MRPT_HAS_OPENNI2
471  // Sensor index validation.
472  if (getNumDevices() == 0)
473  {
474  THROW_EXCEPTION("No OpenNI2 devices found.");
475  }
476  if ((int)sensor_id >= getNumDevices())
477  {
479  "Sensor index is higher than the number of connected devices.");
480  }
481  if (vDevices[sensor_id]->getNextFrameD(
482  depth_img, timestamp, there_is_obs, hardware_error) == false)
483  {
484  showLog(mrpt::format("[%s]\n", __FUNCTION__));
485  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
486  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
487  }
488 #else
489  MRPT_UNUSED_PARAM(depth_img);
490  MRPT_UNUSED_PARAM(timestamp);
491  MRPT_UNUSED_PARAM(there_is_obs);
492  MRPT_UNUSED_PARAM(hardware_error);
493  MRPT_UNUSED_PARAM(sensor_id);
494  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
495 #endif // MRPT_HAS_OPENNI2
496 }
497 
498 /** The main data retrieving function, to be called after calling loadConfig()
499  * and initialize().
500  * \param out_obs The output retrieved observation (only if there_is_obs=true).
501  * \param there_is_obs If set to false, there was no new observation.
502  * \param hardware_error True on hardware/comms error.
503  * \param sensor_id The index of the sensor accessed.
504  *
505  */
507  mrpt::obs::CObservation3DRangeScan& out_obs, bool& there_is_obs,
508  bool& hardware_error, unsigned sensor_id)
509 {
510 #if MRPT_HAS_OPENNI2
511  // Sensor index validation.
512  if (!getNumDevices())
513  {
514  THROW_EXCEPTION("No OpenNI2 devices found.");
515  }
516  if ((int)sensor_id >= getNumDevices())
517  {
519  "Sensor index is higher than the number of connected devices.");
520  }
521  if (vDevices[sensor_id]->getNextFrameRGBD(
522  out_obs, there_is_obs, hardware_error) == false)
523  {
524  showLog(mrpt::format("[%s]\n", __FUNCTION__));
525  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
526  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
527  }
528 #else
529  MRPT_UNUSED_PARAM(out_obs);
530  MRPT_UNUSED_PARAM(there_is_obs);
531  MRPT_UNUSED_PARAM(hardware_error);
532  MRPT_UNUSED_PARAM(sensor_id);
533  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
534 #endif // MRPT_HAS_OPENNI2
535 }
536 
538  mrpt::img::TCamera& param, unsigned sensor_id) const
539 {
540 #if MRPT_HAS_OPENNI2
541  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
542  if (isOpen(sensor_id) == false)
543  {
544  return false;
545  }
546  return vDevices[sensor_id]->getCameraParam(CDevice::COLOR_STREAM, param);
547 #else
549  MRPT_UNUSED_PARAM(sensor_id);
550  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
551 #endif // MRPT_HAS_OPENNI2
552 }
553 
555  mrpt::img::TCamera& param, unsigned sensor_id) const
556 {
557 #if MRPT_HAS_OPENNI2
558  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
559  if (isOpen(sensor_id) == false)
560  {
561  return false;
562  }
563  return vDevices[sensor_id]->getCameraParam(CDevice::DEPTH_STREAM, param);
564 #else
566  MRPT_UNUSED_PARAM(sensor_id);
567  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
568 #endif // MRPT_HAS_OPENNI2
569 }
570 
571 #if MRPT_HAS_OPENNI2
572 /*
573 void openni::VideoMode::setResolution()
574 Setter function for the resolution of this VideoMode. Application use of this
575 function is not recommended.
576 Instead, use SensorInfo::getSupportedVideoModes() to obtain a list of valid
577 video modes
578 
579 -- cited from OpenNI2 help. setResolution() is not recommended.
580 */
581 bool setONI2StreamMode(
582  openni::VideoStream& stream, int w, int h, int fps,
583  openni::PixelFormat format)
584 {
585  // std::cout << "[COpenNI2Generic] Ask mode: " << w << "x" << h << " " <<
586  // fps << " fps. format " << format << std::endl;
587  bool found = false;
588  const openni::Array<openni::VideoMode>& modes =
589  stream.getSensorInfo().getSupportedVideoModes();
590  for (int i = 0, i_end = modes.getSize(); i < i_end; ++i)
591  {
592  // if (m_verbose) std::cout << "[COpenNI2Generic] Mode: " <<
593  // modes[i].getResolutionX() << "x" << modes[i].getResolutionY() << " "
594  // << modes[i].getFps() << " fps. format " << modes[i].getPixelFormat()
595  // << std::endl;
596  if (modes[i].getResolutionX() != w)
597  {
598  continue;
599  }
600  if (modes[i].getResolutionY() != h)
601  {
602  continue;
603  }
604  if (modes[i].getFps() != fps)
605  {
606  continue;
607  }
608  if (modes[i].getPixelFormat() != format)
609  {
610  continue;
611  }
612  openni::Status rc = stream.setVideoMode(modes[i]);
613  if (rc != openni::STATUS_OK)
614  {
615  return false;
616  }
617  return true;
618  }
619  return false;
620 }
621 
622 std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab)
623 {
624  std::stringstream sst;
625  std::string space;
626  for (int i = 0; i < tab; ++i)
627  {
628  space += " ";
629  }
630  sst << space << "name=" << info.getName() << std::endl;
631  sst << space << "uri=" << info.getUri() << std::endl;
632  sst << space << "vendor=" << info.getVendor() << std::endl;
633  sst << space << "product=" << info.getUsbProductId();
634  return sst.str();
635 }
636 
637 bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2)
638 {
639  return (strcmp(i1.getUri(), i2.getUri()) == 0);
640 }
641 //
642 COpenNI2Generic::CDevice::CDevice(
643  const openni::DeviceInfo& info, openni::PixelFormat rgb,
644  openni::PixelFormat depth, bool verbose)
645  : m_info(info), m_mirror(true), m_verbose(verbose)
646 {
647  m_streams[COLOR_STREAM] =
648  CStream::create(m_device, openni::SENSOR_COLOR, rgb, m_log, m_verbose);
649  m_streams[IR_STREAM] =
650  CStream::create(m_device, openni::SENSOR_IR, rgb, m_log, m_verbose);
651  m_streams[DEPTH_STREAM] = CStream::create(
652  m_device, openni::SENSOR_DEPTH, depth, m_log, m_verbose);
653 }
654 
655 COpenNI2Generic::CDevice::~CDevice() { close(); }
656 bool COpenNI2Generic::CDevice::synchMirrorMode()
657 {
658  m_mirror = false;
659  // Check whether both stream support mirroring.
660  for (auto& m_stream : m_streams)
661  {
662  if (!m_stream) continue;
663  bool mirror_support;
664  try
665  {
666  mirror_support = m_stream->isMirrorSupported();
667  }
668  catch (std::logic_error& e)
669  {
670  throw(e);
671  }
672  if (mirror_support == false)
673  {
674  m_log << "[" << __FUNCTION__ << "]" << std::endl;
675  m_log << " openni::STREAM_PROPERTY_MIRRORING is not supported on "
676  << m_stream->getName() << "." << std::endl;
677  m_log << " We assume this is MS Kinect and taken images are "
678  "inverted to right and left."
679  << std::endl;
680  // In this case, getMirroringEnabled() method always returns false.
681  // So we cannot confirm whether the images are inverted or not.
682  m_mirror = true;
683  break;
684  }
685  }
686  // Set both stream to same mirror mode.
687  for (auto& m_stream : m_streams)
688  {
689  if (!m_stream) continue;
690  if (m_stream->isMirrorSupported() == false)
691  {
692  break;
693  }
694  if (m_stream->setMirror(m_mirror) == false)
695  {
696  return false;
697  }
698  }
699  return true;
700 }
701 
702 bool COpenNI2Generic::CDevice::startStreams()
703 {
704  MRPT_START
705  int num_ok = 0;
706  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
707  {
708  if (!m_streams[i]) continue;
709  if (m_verbose)
710  printf(" [%s] calling m_streams[%d]->start()\n", __FUNCTION__, i);
711  if (m_streams[i]->start() == false)
712  {
713  if (m_verbose)
714  printf(
715  " [%s] m_streams[%d]->start() returned FALSE!\n",
716  __FUNCTION__, i);
717  }
718  else
719  {
720  num_ok++;
721  }
722  if (m_verbose)
723  printf(
724  " [%s] m_streams[%d]->start() returned TRUE\n", __FUNCTION__,
725  i);
726  }
727  if (m_verbose)
728  printf(
729  " [COpenNI2Generic::CDevice::startStreams()] %d streams were "
730  "started.\n",
731  num_ok);
732  return num_ok > 0;
733  MRPT_END
734 }
735 
736 bool COpenNI2Generic::CDevice::isOpen() const
737 {
738  return (m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid()) ||
739  (m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid());
740 }
741 
742 void COpenNI2Generic::CDevice::close()
743 {
744  for (auto& m_stream : m_streams)
745  {
746  if (!m_stream) continue;
747  m_stream->destroy();
748  }
749  m_device.close();
750 }
751 
752 bool COpenNI2Generic::CDevice::open(int w, int h, int fps)
753 {
754  MRPT_START
755  if (m_verbose)
756  printf(
757  " [COpenNI2Generic::CDevice::open()] Called with w=%i h=%i "
758  "fps=%i\n",
759  w, h, fps);
760  clearLog();
761  close();
762  openni::Status rc = m_device.open(getInfo().getUri());
763  if (rc != openni::STATUS_OK)
764  {
765  m_log << "[" << __FUNCTION__ << "]" << std::endl
766  << " Failed to open device " << getInfo().getUri() << " "
767  << openni::OpenNI::getExtendedError() << std::endl;
768  return false;
769  }
770  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
771  {
772  if (!m_streams[i]) continue;
773  if (m_verbose)
774  printf(" [%s] calling m_streams[%d]->open()\n", __FUNCTION__, i);
775 
776  if (m_streams[i]->open(w, h, fps) == false)
777  {
778  if (m_verbose)
779  printf(
780  " [%s] m_streams[%d]->open() returned FALSE\n",
781  __FUNCTION__, i);
782  return false;
783  }
784  if (m_verbose)
785  printf(
786  " [%s] m_streams[%d]->open() returned OK\n", __FUNCTION__, i);
787  }
788 
789  if (synchMirrorMode() == false)
790  {
791  close();
792  return false;
793  }
794 
795  if (m_streams[DEPTH_STREAM])
796  {
797  int CloseRange = 0;
798  m_streams[DEPTH_STREAM]->setCloseRange(CloseRange);
799  m_log << " Close range: " << (CloseRange ? "On" : "Off") << std::endl;
800  }
801 
802  if (m_verbose)
803  printf(" DBG: checking if imageRegistrationMode is supported\n");
804  if (m_device.isImageRegistrationModeSupported(
805  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) &&
806  m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid() &&
807  m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid())
808  {
809  // SEB
810  // if(m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF)
811  // != openni::STATUS_OK){
812  if (m_device.setImageRegistrationMode(
813  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) != openni::STATUS_OK)
814  {
815  m_log << " setImageRegistrationMode() Failed:"
816  << openni::OpenNI::getExtendedError() << endl;
817  }
818  else
819  {
820  m_log << " setImageRegistrationMode() Success" << endl;
821  }
822  }
823  else
824  {
825  m_log << " Device doesn't do image registration!" << endl;
826  }
827 
828  if (false) // hasColor())
829  { // printf("DBG: hasColor() returned TRUE\n");
830  m_streams[COLOR_STREAM]->disableAutoExposure();
831  printf("DBG: returned from disableAutoExposure()\n");
832  }
833 
834  if (startStreams() == false)
835  {
836  close();
837  return false;
838  }
839  return true;
840  MRPT_END
841 }
842 
843 bool COpenNI2Generic::CDevice::getNextFrameRGB(
845  bool& there_is_obs, bool& hardware_error)
846 {
847  MRPT_START
848  if (!hasColor())
849  {
850  THROW_EXCEPTION("This OpenNI2 device does not support color imaging");
851  }
852  openni::VideoFrameRef frame;
853  if (m_streams[COLOR_STREAM]->getFrame(
854  frame, timestamp, there_is_obs, hardware_error) == false)
855  {
856  return false;
857  }
858  copyFrame<openni::RGB888Pixel, mrpt::img::CImage>(frame, img);
859 
860  return true;
861  MRPT_END
862 }
863 
864 bool COpenNI2Generic::CDevice::getNextFrameD(
866  bool& there_is_obs, bool& hardware_error)
867 {
868  MRPT_START
869  if (!hasDepth())
870  {
871  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging");
872  }
873  openni::VideoFrameRef frame;
874  if (m_streams[DEPTH_STREAM]->getFrame(
875  frame, timestamp, there_is_obs, hardware_error) == false)
876  {
877  return false;
878  }
879  copyFrame<openni::DepthPixel, mrpt::math::CMatrixF>(frame, img);
880 
881  return true;
882  MRPT_END
883 }
884 
885 bool COpenNI2Generic::CDevice::getNextFrameRGBD(
886  mrpt::obs::CObservation3DRangeScan& obs, bool& there_is_obs,
887  bool& hardware_error)
888 {
889  MRPT_START
890  clearLog();
891  there_is_obs = false;
892  hardware_error = false;
893 
894  if (!hasColor())
895  {
896  THROW_EXCEPTION("This OpenNI2 device does not support color imaging");
897  }
898  if (!hasDepth())
899  {
900  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging");
901  }
902  // Read a frame (depth + rgb)
904  openni::VideoFrameRef frame[STREAM_TYPE_SIZE];
905  for (int i = 0; i < STREAM_TYPE_SIZE; ++i)
906  {
907  if (!m_streams[i] || !m_streams[i]->isValid()) continue;
908  if (m_streams[i]->getFrame(
909  frame[i], tm, there_is_obs, hardware_error) == false)
910  {
911  return false;
912  }
913  if (there_is_obs == false || hardware_error == true)
914  {
915  return false;
916  }
917  }
918 
919  const int width = frame[COLOR_STREAM].getWidth();
920  const int height = frame[COLOR_STREAM].getHeight();
921  if ((frame[DEPTH_STREAM].getWidth() != width) ||
922  (frame[DEPTH_STREAM].getHeight() != height))
923  {
924  m_log << "[" << __FUNCTION__ << "]" << std::endl
925  << " Both frames don't have the same size." << std::endl;
926  return false;
927  }
928  there_is_obs = true;
929  obs.hasConfidenceImage = false;
930  obs.hasIntensityImage = true;
931  obs.hasRangeImage = true;
932  obs.range_is_depth = true;
933  obs.hasPoints3D = false;
935  resize(obs, width, height);
936 
937  const char* data[STREAM_TYPE_SIZE] = {
938  (const char*)frame[COLOR_STREAM].getData(),
939  (const char*)frame[DEPTH_STREAM].getData()};
940  const int step[STREAM_TYPE_SIZE] = {frame[COLOR_STREAM].getStrideInBytes(),
941  frame[DEPTH_STREAM].getStrideInBytes()};
942 
943  for (int yc = 0; yc < height; ++yc)
944  {
945  const auto* pRgb = (const openni::RGB888Pixel*)data[COLOR_STREAM];
946  const auto* pDepth = (const openni::DepthPixel*)data[DEPTH_STREAM];
947  for (int xc = 0; xc < width; ++xc, ++pDepth, ++pRgb)
948  {
949  int x = xc;
950  if (isMirrorMode())
951  {
952  x = width - x - 1;
953  }
954  setPixel(*pRgb, obs.intensityImage, x, yc);
955  setPixel(*pDepth, obs.rangeImage, x, yc);
956  }
957  data[COLOR_STREAM] += step[COLOR_STREAM];
958  data[DEPTH_STREAM] += step[DEPTH_STREAM];
959  }
960 
961  return true;
962  MRPT_END
963 }
964 
965 COpenNI2Generic::CDevice::Ptr COpenNI2Generic::CDevice::create(
966  const openni::DeviceInfo& info, openni::PixelFormat rgb,
967  openni::PixelFormat depth, bool verbose)
968 {
969  return Ptr(new CDevice(info, rgb, depth, verbose));
970 }
971 
972 bool COpenNI2Generic::CDevice::getSerialNumber(std::string& sn)
973 {
974  clearLog();
975  openni::Status rc;
976  bool isOpened = isOpen();
977  if (isOpened == false)
978  {
979  rc = m_device.open(getInfo().getUri());
980  if (rc != openni::STATUS_OK)
981  {
982  m_log << "[" << __FUNCTION__ << "]" << std::endl
983  << " Failed to open device " << getInfo().getUri() << " "
984  << openni::OpenNI::getExtendedError() << std::endl;
985  return false;
986  }
987  }
988  char serialNumber[16];
989  rc = m_device.getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER, &serialNumber);
990  if (rc != openni::STATUS_OK)
991  {
992  m_log << "[" << __FUNCTION__ << "]" << std::endl
993  << " Failed to getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER) "
994  << getInfo().getUri() << " " << openni::OpenNI::getExtendedError()
995  << std::endl;
996  return false;
997  }
998  sn = std::string(serialNumber);
999  if (isOpened == false)
1000  {
1001  m_device.close();
1002  }
1003  return true;
1004 }
1005 
1006 bool COpenNI2Generic::CDevice::getSerialNumber(unsigned int& sn)
1007 {
1008  std::string str;
1009  if (getSerialNumber(str) == false)
1010  {
1011  return false;
1012  }
1013  std::stringstream sst;
1014  sst.str(str);
1015  sst >> sn;
1016  return !sst.fail();
1017 }
1018 //
1020  openni::Device& device, openni::SensorType type, openni::PixelFormat format,
1021  std::ostream& log, bool verbose)
1022  : m_log(log),
1023  m_device(device),
1024  m_strName("Unknown"),
1025  m_type(type),
1026  m_format(format),
1028 {
1029  if (m_type == openni::SENSOR_COLOR)
1030  {
1031  m_strName = "openni::SENSOR_COLOR";
1032  }
1033  else if (m_type == openni::SENSOR_DEPTH)
1034  {
1035  m_strName = "openni::SENSOR_DEPTH";
1036  }
1037  else if (m_type == openni::SENSOR_IR)
1038  {
1039  m_strName = "openni::SENSOR_IR";
1040  }
1041  else
1042  {
1043  m_log << "[" << __FUNCTION__ << "]" << std::endl
1044  << " Unknown SensorType -> " << m_type << std::endl;
1045  }
1046 }
1047 
1048 COpenNI2Generic::CDevice::CStream::~CStream() { destroy(); }
1049 bool COpenNI2Generic::CDevice::CStream::isMirrorSupported() const
1050 {
1051  if (isValid() == false)
1052  {
1053  THROW_EXCEPTION(getName() + " is not opened.");
1054  }
1055  return m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING);
1056 }
1057 
1058 bool COpenNI2Generic::CDevice::CStream::setMirror(bool flag)
1059 {
1060  if (isValid() == false)
1061  {
1062  m_log << "[" << __FUNCTION__ << "]" << std::endl
1063  << " " << getName() << " is not opened." << std::endl;
1064  return false;
1065  }
1066  if (m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING) ==
1067  false)
1068  {
1069  return false;
1070  }
1071  if (m_stream.setMirroringEnabled(flag) != openni::STATUS_OK)
1072  {
1073  m_log << "[" << __FUNCTION__ << "]" << std::endl
1074  << " setMirroringEnabled() failed: "
1075  << openni::OpenNI::getExtendedError() << std::endl;
1076  return false;
1077  }
1078  return true;
1079 }
1080 
1081 bool COpenNI2Generic::CDevice::CStream::isValid() const
1082 {
1083  return m_stream.isValid();
1084 }
1085 
1086 void COpenNI2Generic::CDevice::CStream::destroy() { m_stream.destroy(); }
1087 void COpenNI2Generic::CDevice::CStream::setCloseRange(int& value)
1088 {
1089  if (m_verbose)
1090  printf(
1091  " [CDevice::CStream::setCloseRange] entry with value=%d\n",
1092  value);
1093  m_stream.setProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, value);
1094  if (m_verbose)
1095  printf(
1096  " [CDevice::CStream::setCloseRange] returned from "
1097  "mstream.setProperty()\n");
1098  m_stream.getProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, &value);
1099  if (m_verbose)
1100  printf(
1101  " [CDevice::CStream::setCloseRange] returned from "
1102  "mstream.getProperty() ... value %d\n",
1103  value);
1104 }
1105 
1106 bool COpenNI2Generic::CDevice::CStream::open(int w, int h, int fps)
1107 {
1108  destroy();
1109  if (m_type != openni::SENSOR_COLOR && m_type != openni::SENSOR_DEPTH &&
1110  m_type != openni::SENSOR_IR)
1111  { // SEB added IR
1112  m_log << "[" << __FUNCTION__ << "]" << std::endl
1113  << " Unknown SensorType -> " << m_type << std::endl;
1114  return false;
1115  }
1116  if (m_verbose)
1117  printf(
1118  " [COpenNI2Generic::CDevice::CStream::open] opening sensor "
1119  "stream with m_type == %d\n",
1120  (int)m_type);
1121  openni::Status rc = openni::STATUS_OK;
1122  // if(m_type == openni::SENSOR_COLOR) {
1123  // m_type = openni::SENSOR_IR;
1124  // m_strName="openni::SENSOR_IR"; // SEB added
1125  // if (m_verbose) printf("DBG: changing type to SENSOR_IR
1126  //(%d)\n",(int)m_type);
1127  // } // added whole if stmt
1128  rc = m_stream.create(m_device, m_type);
1129  if (rc != openni::STATUS_OK)
1130  {
1131  m_log << "[" << __FUNCTION__ << "]" << std::endl
1132  << " Couldn't find sensor " << m_strName << ":"
1133  << openni::OpenNI::getExtendedError() << std::endl;
1134  if (m_type == openni::SENSOR_COLOR)
1135  {
1136  m_type = openni::SENSOR_IR;
1137  m_strName = "openni::SENSOR_IR"; // SEB added
1138  if (m_verbose)
1139  printf("DBG: changing type to SENSOR_IR (%d)\n", (int)m_type);
1140  rc = m_stream.create(m_device, m_type);
1141  } // SEB added whole if stmt
1142  else
1143  return false;
1144  }
1145  if (m_verbose) printf("returned OK from stream.create()\n");
1146  openni::VideoMode options = m_stream.getVideoMode();
1147  m_log << "[" << __FUNCTION__ << "]" << std::endl;
1148  m_log << " " << m_strName << std::endl;
1149  m_log << " "
1150  << mrpt::format(
1151  "Initial resolution (%d, %d) FPS %d Format %d",
1152  options.getResolutionX(), options.getResolutionY(),
1153  options.getFps(), options.getPixelFormat())
1154  << std::endl;
1155  if (m_verbose) printf("DBG: calling setONI2StreamMode()\n");
1156  if (setONI2StreamMode(m_stream, w, h, fps, m_format) == false)
1157  {
1158  m_log << " Can't find desired mode in the " << getName() << std::endl;
1159  destroy();
1160  return false;
1161  }
1162  if (m_verbose) printf("DBG: returned OK from setONI2StreamMode()\n");
1163  if (m_verbose) printf("DBG: calling stream.getVideoMode()\n");
1164  options = m_stream.getVideoMode();
1165  m_log << " "
1166  << mrpt::format(
1167  "-> (%d, %d) FPS %d Format %d", options.getResolutionX(),
1168  options.getResolutionY(), options.getFps(),
1169  options.getPixelFormat())
1170  << std::endl;
1171  if (m_verbose)
1172  printf(
1173  " [COpenNI2Generic::CDevice::CStream::open] returning TRUE\n");
1174  return true;
1175 }
1176 
1178 {
1179  if (isValid() == false)
1180  {
1181  m_log << "[" << __FUNCTION__ << "]" << std::endl
1182  << " " << getName() << " is not opened." << std::endl;
1183  return false;
1184  }
1185  if (m_stream.start() != openni::STATUS_OK)
1186  {
1187  m_log << "[" << __FUNCTION__ << "]" << std::endl
1188  << " Couldn't start " << getName()
1189  << " stream:" << openni::OpenNI::getExtendedError() << std::endl;
1190  this->destroy();
1191  return false;
1192  }
1193  return true;
1194 }
1195 
1196 COpenNI2Generic::CDevice::CStream::Ptr
1197  COpenNI2Generic::CDevice::CStream::create(
1198  openni::Device& device, openni::SensorType type,
1199  openni::PixelFormat format, std::ostream& log, bool verbose)
1200 {
1201  return Ptr(new CStream(device, type, format, log, verbose));
1202 }
1203 
1204 bool COpenNI2Generic::CDevice::CStream::getFrame(
1205  openni::VideoFrameRef& frame, mrpt::system::TTimeStamp& timestamp,
1206  bool& there_is_obs, bool& hardware_error)
1207 {
1208  there_is_obs = false;
1209  hardware_error = false;
1210  if (isValid() == false)
1211  {
1212  return false;
1213  }
1214  openni::Status rc = m_stream.readFrame(&frame);
1215  if (rc != openni::STATUS_OK)
1216  {
1217  hardware_error = true;
1218  std::string message =
1219  mrpt::format("Failed to grab frame from %s", getName().c_str());
1220  THROW_EXCEPTION(message);
1221  }
1222  there_is_obs = true;
1223  timestamp = mrpt::system::getCurrentTime();
1224  return true;
1225 }
1226 
1227 #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().
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
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)
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3606
STL namespace.
mrpt::io::CStream CStream
Definition: utils/CStream.h:5
GLenum GLsizei width
Definition: glext.h:3535
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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)...
bool getColorSensorParam(mrpt::img::TCamera &param, unsigned sensor_id=0) const
GLint GLvoid * img
Definition: glext.h:3769
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)
bool hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
Definition: glext.h:4116
unsigned int openDevicesBySerialNum(const std::set< unsigned > &vSerialRequired)
Open a set of RGBD devices specified by their serial number.
void getNextFrameD(mrpt::math::CMatrixF &depth_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().
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
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;.
GLenum GLsizei GLenum format
Definition: glext.h:3535
int getConnectedDevices()
Get a list of the connected OpenNI2 sensors.
GLsizei const GLfloat * value
Definition: glext.h:4134
void showLog(const std::string &message) const
GLenum GLint x
Definition: glext.h:3542
GLuint start
Definition: glext.h:3532
double getHeight(const TPolygon3D &p, const TPoint3D &c)
GLenum GLsizei GLsizei height
Definition: glext.h:3558
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
GLfloat param
Definition: glext.h:3838
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
images resize(NUM_IMGS)
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
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: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019