Main MRPT website > C++ reference for MRPT 1.5.7
COpenNI2Generic.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled header
11 
13 #include <mrpt/utils/CTimeLogger.h>
15 #include <mrpt/system/threads.h>
16 
17 // Universal include for all versions of OpenCV
18 #include <mrpt/otherlibs/do_opencv_includes.h>
19 #include <atomic>
20 #include <mutex>
21 #include <memory>
22 
23 #if MRPT_HAS_OPENNI2
24 
25 // This seems to be assumed by OpenNI.h and undefined for some reason in GCC/Ubuntu
26 #if !defined(MRPT_OS_WINDOWS)
27 # define linux 1
28 #endif
29 
30 # include <OpenNI.h>
31 # include <PS1080.h>
32 #endif
34 
35 using namespace mrpt::hwdrivers;
36 using namespace mrpt::system;
37 using namespace mrpt::obs;
38 using namespace mrpt::synch;
39 using namespace std;
40 
41 
42 std::vector<stlplus::smart_ptr<COpenNI2Generic::CDevice> > vDevices;
43 std::recursive_mutex vDevices_mx;
44 std::atomic<int> numInstances(0);
45 
46 #if MRPT_HAS_OPENNI2
47 bool setONI2StreamMode(openni::VideoStream& stream, int w, int h, int fps, openni::PixelFormat format);
48 std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab = 0);
49 bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2);
50 #endif // MRPT_HAS_OPENNI2
51 
53 {
54  return numInstances; // get atomic
55 }
56 
57 /*-------------------------------------------------------------
58 ctor
59 -------------------------------------------------------------*/
61  m_width(640),
62  m_height(480),
63  m_fps(30.0f),
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 // MRPT_HAS_OPENNI2
68  m_verbose(false),
69  m_grab_image(true),
70  m_grab_depth(true),
71  m_grab_3D_points(true)
72 {
73  const char * sVerbose = getenv("MRPT_HWDRIVERS_VERBOSE");
74  m_verbose = (sVerbose!=NULL) && atoi(sVerbose)!=0;
75  // Start automatically:
76  if (!this->start()) {
77 #if MRPT_HAS_OPENNI2
78  THROW_EXCEPTION(mrpt::format("After initialization:\n %s\n", openni::OpenNI::getExtendedError()))
79 #endif
80  }
81 }
82 
83 COpenNI2Generic::COpenNI2Generic(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!=NULL) && atoi(sVerbose)!=0;
98  // Open?
99  if (open_streams_now) {
100  if (!this->start()) {
101 #if MRPT_HAS_OPENNI2
102  THROW_EXCEPTION(mrpt::format("After initialization:\n %s\n", openni::OpenNI::getExtendedError()))
103 #endif
104  }
105  }
106 }
107 
109 {
110 #if MRPT_HAS_OPENNI2
111  if(numInstances == 0){
112  if(openni::OpenNI::initialize() != openni::STATUS_OK){
113  return false;
114  }else{
115  std::cerr << "[" << __FUNCTION__ << "]" << std::endl << " Initialized OpenNI2." << std::endl;
116  }
117  }
118  numInstances++;
119  return true;
120 #else
121  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
122 #endif // MRPT_HAS_OPENNI2
123 }
124 
125 /*-------------------------------------------------------------
126 dtor
127 -------------------------------------------------------------*/
129 {
130  numInstances--;
131  if(numInstances == 0){
132  kill();
133  }
134 }
135 
137  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
138  return vDevices.size();
139 }
140 
141 void COpenNI2Generic::setVerbose(bool verbose){
142  m_verbose = verbose;
143 }
144 
146  return m_verbose;
147 }
148 
149 void COpenNI2Generic::showLog(const std::string& message)const{
150  if(isVerbose() == false){
151  return;
152  }
153  std::cerr << message;
154 }
155 /** This method can or cannot be implemented in the derived class, depending on the need for it.
156 * \exception This method must throw an exception with a descriptive message if some critical error is found.
157 */
159 {
160 #if MRPT_HAS_OPENNI2
161  // Get devices list
162  openni::Array<openni::DeviceInfo> oni2InfoArray;
163  openni::OpenNI::enumerateDevices(&oni2InfoArray);
164 
165  const size_t numDevices = oni2InfoArray.getSize();
166  showLog(mrpt::format("[%s]\n", __FUNCTION__));
167  showLog(mrpt::format(" Get device list. %d devices connected.\n", (int)numDevices));
168 
169  // Search new devices.
170  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
171  std::set<int> newDevices;
172  for(unsigned i=0; i < numDevices; i++){
173  const openni::DeviceInfo& info = oni2InfoArray[i];
174  showLog(mrpt::format(" Device[%d]\n", i));
175  showLog(oni2DevInfoStr(info, 3) + "\n");
176 
177  bool isExist = false;
178  for(unsigned int j = 0, j_end = vDevices.size();j < j_end && isExist == false;++j){
179  if(cmpONI2Device(info, vDevices[j]->getInfo())){
180  isExist = true;
181  }
182  }
183  if(isExist == false){
184  newDevices.insert(i);
185  }
186  }
187  // Add new devices to device list(static member).
188  for(std::set<int>::const_iterator it = newDevices.begin(), it_end = newDevices.end();it != it_end;++it){
189  const openni::DeviceInfo& info = oni2InfoArray[*it];
190  CDevice::Ptr device = CDevice::create(info, (openni::PixelFormat)m_rgb_format, (openni::PixelFormat)m_depth_format, m_verbose);
191  vDevices.push_back(device);
192  {
193  unsigned int sn;
194  if (device->getSerialNumber(sn)) {
195  showLog(mrpt::format("Device[%d]: serial number: `%u`\n",*it,sn) );
196  }
197  }
198  }
199 
200  if(getNumDevices() == 0){
201  showLog(" No devices connected -> EXIT\n");
202  }else{
203  showLog(mrpt::format(" %d devices were found.\n", getNumDevices()));
204  }
205  return getNumDevices();
206 #else
207  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
208 #endif // MRPT_HAS_OPENNI2
209 }
210 
212 {
213 #if MRPT_HAS_OPENNI2
214  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
215  vDevices.clear();
216  openni::OpenNI::shutdown();
217 #else
218  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
219 #endif // MRPT_HAS_OPENNI2
220 }
221 
222 bool COpenNI2Generic::isOpen(const unsigned sensor_id) const
223 {
224 #if MRPT_HAS_OPENNI2
225  if((int)sensor_id >= getNumDevices()){
226  return false;
227  }
228  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
229  return vDevices[sensor_id]->isOpen();
230 #else
231  MRPT_UNUSED_PARAM(sensor_id);
232  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
233 #endif // MRPT_HAS_OPENNI2
234 }
235 
236 void COpenNI2Generic::open(unsigned sensor_id)
237 {
238 #if MRPT_HAS_OPENNI2
239  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
240  // Sensor index validation.
241  if (!getNumDevices()){
242  THROW_EXCEPTION("No OpenNI2 devices found.")
243  }
244  if ((int)sensor_id >= getNumDevices()){
245  THROW_EXCEPTION("Sensor index is higher than the number of connected devices.")
246  }
247  showLog(mrpt::format("[%s]\n", __FUNCTION__));
248  showLog(mrpt::format(" open[%d] ...\n", sensor_id));
249 
250  if(isOpen(sensor_id)){
251  showLog(mrpt::format(" The sensor [%d] is already opened\n", sensor_id));
252  return;
253  }
254  if (m_verbose) printf("[COpenNI2Generic] DBG: [%s] about to call vDevices[%d]->open()\n",__FUNCTION__,sensor_id);
255  vDevices[sensor_id]->open(m_width, m_height, m_fps);
256  showLog(vDevices[sensor_id]->getLog() + "\n");
257  showLog(mrpt::format(" Device [%d] ", sensor_id));
258  if(vDevices[sensor_id]->isOpen()){
259  showLog(" open successfully.\n");
260  }else{
261  showLog(" open failed.\n");
262  }
263  mrpt::system::sleep(1000); // Sleep
264 #else
265  MRPT_UNUSED_PARAM(sensor_id);
266  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
267 #endif // MRPT_HAS_OPENNI2
268 }
269 
270 unsigned int COpenNI2Generic::openDevicesBySerialNum(const std::set<unsigned>& serial_required)
271 {
272 #if MRPT_HAS_OPENNI2
273  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
274  showLog(mrpt::format("[%s]\n", __FUNCTION__));
275  unsigned num_open_dev = 0;
276  for(unsigned sensor_id=0; sensor_id < vDevices.size(); sensor_id++)
277  {
278  unsigned int serialNum;
279  if(vDevices[sensor_id]->getSerialNumber(serialNum) == false){
280  showLog(vDevices[sensor_id]->getLog());
281  continue;
282  }
283  if (m_verbose) printf("[COpenNI2Generic::openDevicesBySerialNum] checking device with serial '%d'\n",serialNum);
284 
285  if(serial_required.find(serialNum) == serial_required.end()){
286  vDevices[sensor_id]->close();
287  continue;
288  }
289  if(vDevices[sensor_id]->isOpen()){
290  num_open_dev++;
291  continue;
292  }
293  int width = m_width;
294  int height = m_height;
295  if (m_verbose) printf("[COpenNI2Generic] DBG: [%s] about to call vDevices[%d]->open(%d,%d,%d)\n",
296  __FUNCTION__,sensor_id,width,height,(int)m_fps);
297  if(vDevices[sensor_id]->open(width, height, m_fps) == false){
298  showLog(vDevices[sensor_id]->getLog());
299  continue;
300  }
301  num_open_dev++;
302  if (m_verbose) printf("[COpenNI2Generic] DBG: [%s] now has %d devices open\n", __FUNCTION__,num_open_dev);
303  }
304  return num_open_dev;
305 #else
306  MRPT_UNUSED_PARAM(serial_required);
307  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
308 #endif // MRPT_HAS_OPENNI2
309 }
310 
311 unsigned int COpenNI2Generic::openDeviceBySerial(const unsigned int SerialRequired){
312  std::set<unsigned> serial_required;
313  serial_required.insert(SerialRequired);
314  return openDevicesBySerialNum(serial_required);
315 }
316 
317 bool COpenNI2Generic::getDeviceIDFromSerialNum(const unsigned int SerialRequired, int& sensor_id) const{
318 #if MRPT_HAS_OPENNI2
319  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
320  for(size_t i = 0, i_end = vDevices.size();i < i_end;++i){
321  unsigned int sn;
322  if(vDevices[i]->getSerialNumber(sn) == false){
323  continue;
324  }
325  if(sn == SerialRequired){
326  sensor_id = (int)i;
327  return true;
328  }
329  }
330  return false;
331 #else
332  MRPT_UNUSED_PARAM(SerialRequired); MRPT_UNUSED_PARAM(sensor_id);
333  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
334 #endif // MRPT_HAS_OPENNI2
335 }
336 
337 void COpenNI2Generic::close(unsigned sensor_id)
338 {
339 #if MRPT_HAS_OPENNI2
340  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
341  // Sensor index validation.
342  if (!getNumDevices()){
343  THROW_EXCEPTION("No OpenNI2 devices found.")
344  }
345  if ((int)sensor_id >= getNumDevices()){
346  THROW_EXCEPTION("Sensor index is higher than the number of connected devices.")
347  }
348  vDevices[sensor_id]->close();
349 #else
350  MRPT_UNUSED_PARAM(sensor_id);
351  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
352 #endif // MRPT_HAS_OPENNI2
353 }
354 
355 /** The main data retrieving function, to be called after calling loadConfig() and initialize().
356 * \param out_obs The output retrieved observation (only if there_is_obs=true).
357 * \param timestamp The timestamp of the capture (only if there_is_obs=true).
358 * \param there_is_obs If set to false, there was no new observation.
359 * \param hardware_error True on hardware/comms error.
360 * \param sensor_id The index of the sensor accessed.
361 *
362 */
364  mrpt::utils::CImage &rgb_img,
365  uint64_t &timestamp,
366  bool &there_is_obs,
367  bool &hardware_error,
368  unsigned sensor_id )
369 {
370 #if MRPT_HAS_OPENNI2
371  // Sensor index validation.
372  if (!getNumDevices()){
373  THROW_EXCEPTION("No OpenNI2 devices found.")
374  }
375  if ((int)sensor_id >= getNumDevices()){
376  THROW_EXCEPTION("Sensor index is higher than the number of connected devices.")
377  }
378  if(vDevices[sensor_id]->getNextFrameRGB(rgb_img, timestamp, there_is_obs, hardware_error) == false){
379  showLog(mrpt::format("[%s]\n", __FUNCTION__ ));
380  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
381  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
382  }
383 #else
384  MRPT_UNUSED_PARAM(rgb_img); MRPT_UNUSED_PARAM(timestamp);
385  MRPT_UNUSED_PARAM(there_is_obs); MRPT_UNUSED_PARAM(hardware_error); MRPT_UNUSED_PARAM(sensor_id);
386  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
387 #endif // MRPT_HAS_OPENNI2
388 }
389 
390 /** The main data retrieving function, to be called after calling loadConfig() and initialize().
391 * \param depth_img The output retrieved depth image (only if there_is_obs=true).
392 * \param timestamp The timestamp of the capture (only if there_is_obs=true).
393 * \param there_is_obs If set to false, there was no new observation.
394 * \param hardware_error True on hardware/comms error.
395 * \param sensor_id The index of the sensor accessed.
396 *
397 */
399  mrpt::math::CMatrix &depth_img,
400  uint64_t &timestamp,
401  bool &there_is_obs,
402  bool &hardware_error,
403  unsigned sensor_id )
404 {
405 #if MRPT_HAS_OPENNI2
406  // Sensor index validation.
407  if (getNumDevices() == 0){
408  THROW_EXCEPTION("No OpenNI2 devices found.")
409  }
410  if ((int)sensor_id >= getNumDevices()){
411  THROW_EXCEPTION("Sensor index is higher than the number of connected devices.")
412  }
413  if(vDevices[sensor_id]->getNextFrameD(depth_img, timestamp, there_is_obs, hardware_error) == false){
414  showLog(mrpt::format("[%s]\n", __FUNCTION__));
415  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
416  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
417  }
418 #else
419  MRPT_UNUSED_PARAM(depth_img); MRPT_UNUSED_PARAM(timestamp);
420  MRPT_UNUSED_PARAM(there_is_obs); MRPT_UNUSED_PARAM(hardware_error); MRPT_UNUSED_PARAM(sensor_id);
421  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
422 #endif // MRPT_HAS_OPENNI2
423 }
424 
425 /** The main data retrieving function, to be called after calling loadConfig() and initialize().
426 * \param out_obs The output retrieved observation (only if there_is_obs=true).
427 * \param there_is_obs If set to false, there was no new observation.
428 * \param hardware_error True on hardware/comms error.
429 * \param sensor_id The index of the sensor accessed.
430 *
431 */
434  bool &there_is_obs,
435  bool &hardware_error,
436  unsigned sensor_id )
437 {
438 #if MRPT_HAS_OPENNI2
439  // Sensor index validation.
440  if (!getNumDevices()){
441  THROW_EXCEPTION("No OpenNI2 devices found.")
442  }
443  if ((int)sensor_id >= getNumDevices()){
444  THROW_EXCEPTION("Sensor index is higher than the number of connected devices.")
445  }
446  if(vDevices[sensor_id]->getNextFrameRGBD(out_obs, there_is_obs, hardware_error) == false){
447  showLog(mrpt::format("[%s]\n", __FUNCTION__));
448  showLog(mrpt::format(" Error [%d]th Sensor.\n", sensor_id));
449  showLog(std::string(" ") + vDevices[sensor_id]->getLog() + "\n");
450  }
451 #else
452  MRPT_UNUSED_PARAM(out_obs); MRPT_UNUSED_PARAM(there_is_obs); MRPT_UNUSED_PARAM(hardware_error);
453  MRPT_UNUSED_PARAM(sensor_id);
454  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
455 #endif // MRPT_HAS_OPENNI2
456 }
457 
459 #if MRPT_HAS_OPENNI2
460  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
461  if(isOpen(sensor_id) == false){
462  return false;
463  }
464  return vDevices[sensor_id]->getCameraParam(CDevice::COLOR_STREAM, param);
465 #else
466  MRPT_UNUSED_PARAM(param); MRPT_UNUSED_PARAM(sensor_id);
467  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
468 #endif // MRPT_HAS_OPENNI2
469 }
470 
472 #if MRPT_HAS_OPENNI2
473  std::lock_guard<std::recursive_mutex> lock(vDevices_mx);
474  if(isOpen(sensor_id) == false){
475  return false;
476  }
477  return vDevices[sensor_id]->getCameraParam(CDevice::DEPTH_STREAM, param);
478 #else
479  MRPT_UNUSED_PARAM(param); MRPT_UNUSED_PARAM(sensor_id);
480  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
481 #endif // MRPT_HAS_OPENNI2
482 }
483 
484 
485 #if MRPT_HAS_OPENNI2
486 /*
487 void openni::VideoMode::setResolution()
488 Setter function for the resolution of this VideoMode. Application use of this function is not recommended.
489 Instead, use SensorInfo::getSupportedVideoModes() to obtain a list of valid video modes
490 
491 -- cited from OpenNI2 help. setResolution() is not recommended.
492 */
493 bool setONI2StreamMode(openni::VideoStream& stream, int w, int h, int fps, openni::PixelFormat format){
494  // std::cout << "[COpenNI2Generic] Ask mode: " << w << "x" << h << " " << fps << " fps. format " << format << std::endl;
495  bool found = false;
496  const openni::Array<openni::VideoMode>& modes = stream.getSensorInfo().getSupportedVideoModes();
497  for(int i = 0, i_end = modes.getSize();i < i_end;++i){
498  // if (m_verbose) std::cout << "[COpenNI2Generic] Mode: " << modes[i].getResolutionX() << "x" << modes[i].getResolutionY() << " " << modes[i].getFps() << " fps. format " << modes[i].getPixelFormat() << std::endl;
499  if(modes[i].getResolutionX() != w){
500  continue;
501  }
502  if(modes[i].getResolutionY() != h){
503  continue;
504  }
505  if(modes[i].getFps() != fps){
506  continue;
507  }
508  if(modes[i].getPixelFormat() != format){
509  continue;
510  }
511  openni::Status rc = stream.setVideoMode(modes[i]);
512  if(rc != openni::STATUS_OK){
513  return false;
514  }
515  return true;
516  }
517  return false;
518 }
519 
520 std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab){
521  std::stringstream sst;
522  std::string space;
523  for(int i = 0;i < tab;++i){
524  space += " ";
525  }
526  sst << space << "name=" << info.getName() << std::endl;
527  sst << space << "uri=" << info.getUri() << std::endl;
528  sst << space << "vendor=" << info.getVendor() << std::endl;
529  sst << space << "product=" << info.getUsbProductId();
530  return sst.str();
531 }
532 
533 bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2){
534  return (strcmp(i1.getUri(), i2.getUri()) == 0);
535 }
536 //
537 COpenNI2Generic::CDevice::CDevice(const openni::DeviceInfo& info, openni::PixelFormat rgb, openni::PixelFormat depth,bool verbose)
538  :m_info(info), m_mirror(true), m_verbose(verbose)
539 {
540  m_streams[COLOR_STREAM] = CStream::create(m_device, openni::SENSOR_COLOR, rgb , m_log, m_verbose);
541  m_streams[IR_STREAM] = CStream::create(m_device, openni::SENSOR_IR, rgb , m_log, m_verbose);
542  m_streams[DEPTH_STREAM] = CStream::create(m_device, openni::SENSOR_DEPTH, depth, m_log, m_verbose);
543 }
544 
545 COpenNI2Generic::CDevice::~CDevice(){
546  close();
547 }
548 
549 bool COpenNI2Generic::CDevice::synchMirrorMode(){
550  m_mirror = false;
551  // Check whether both stream support mirroring.
552  for(int i = 0;i < STREAM_TYPE_SIZE;++i){
553  if (!m_streams[i]) continue;
554  bool mirror_support;
555  try{
556  mirror_support = m_streams[i]->isMirrorSupported();
557  }catch(std::logic_error& e){
558  throw(e);
559  }
560  if(mirror_support == false){
561  m_log << "[" << __FUNCTION__ << "]" << std::endl;
562  m_log << " openni::STREAM_PROPERTY_MIRRORING is not supported on " << m_streams[i]->getName() << "." << std::endl;
563  m_log << " We assume this is MS Kinect and taken images are inverted to right and left." << std::endl;
564  // In this case, getMirroringEnabled() method always returns false. So we cannot confirm whether the images are inverted or not.
565  m_mirror = true;
566  break;
567  }
568  }
569  // Set both stream to same mirror mode.
570  for(int i = 0;i < STREAM_TYPE_SIZE;++i){
571  if (!m_streams[i]) continue;
572  if(m_streams[i]->isMirrorSupported() == false){
573  break;
574  }
575  if(m_streams[i]->setMirror(m_mirror) == false){
576  return false;
577  }
578  }
579  return true;
580 }
581 
582 bool COpenNI2Generic::CDevice::startStreams(){
583  MRPT_START
584  int num_ok = 0;
585  for(int i = 0;i < STREAM_TYPE_SIZE;++i){
586  if (!m_streams[i]) continue;
587  if (m_verbose) printf(" [%s] calling m_streams[%d]->start()\n",__FUNCTION__,i);
588  if(m_streams[i]->start() == false){
589  if (m_verbose) printf(" [%s] m_streams[%d]->start() returned FALSE!\n",__FUNCTION__,i);
590  }
591  else {
592  num_ok++;
593  }
594  if (m_verbose) printf(" [%s] m_streams[%d]->start() returned TRUE\n",__FUNCTION__,i);
595  }
596  if (m_verbose) printf(" [COpenNI2Generic::CDevice::startStreams()] %d streams were started.\n", num_ok);
597  return num_ok>0;
598  MRPT_END
599 }
600 
601 bool COpenNI2Generic::CDevice::isOpen()const{
602  return (m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid()) ||
603  (m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid());
604 }
605 
606 void COpenNI2Generic::CDevice::close(){
607  for(int i = 0;i < STREAM_TYPE_SIZE;++i){
608  if (!m_streams[i]) continue;
609  m_streams[i]->destroy();
610  }
611  m_device.close();
612 }
613 
614 bool COpenNI2Generic::CDevice::open(int w, int h, int fps){
615  MRPT_START
616  if (m_verbose) printf(" [COpenNI2Generic::CDevice::open()] Called with w=%i h=%i fps=%i\n",w,h,fps);
617  clearLog();
618  close();
619  openni::Status rc = m_device.open(getInfo().getUri());
620  if(rc != openni::STATUS_OK){
621  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Failed to open device " << getInfo().getUri() << " " << openni::OpenNI::getExtendedError() << std::endl;
622  return false;
623  }
624  for(int i = 0;i < STREAM_TYPE_SIZE;++i){
625  if(!m_streams[i]) continue;
626  if (m_verbose) printf(" [%s] calling m_streams[%d]->open()\n",__FUNCTION__,i);
627 
628  if(m_streams[i]->open(w, h, fps) == false)
629  {
630  if (m_verbose) printf(" [%s] m_streams[%d]->open() returned FALSE\n",__FUNCTION__,i);
631  return false;
632  }
633  if (m_verbose) printf(" [%s] m_streams[%d]->open() returned OK\n",__FUNCTION__,i);
634  }
635 
636  if(synchMirrorMode() == false){
637  close();
638  return false;
639  }
640 
641  if (m_streams[DEPTH_STREAM]) {
642  int CloseRange=0;
643  m_streams[DEPTH_STREAM]->setCloseRange(CloseRange);
644  m_log << " Close range: " << (CloseRange? "On" : "Off") << std::endl;
645  }
646 
647  if (m_verbose) printf(" DBG: checking if imageRegistrationMode is supported\n");
648  if(m_device.isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) &&
649  m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid() &&
650  m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid() )
651  {
652 //SEB if(m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF) != openni::STATUS_OK){
653  if(m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) != openni::STATUS_OK){
654  m_log << " setImageRegistrationMode() Failed:" << openni::OpenNI::getExtendedError() << endl;
655  }else{
656  m_log << " setImageRegistrationMode() Success" << endl;
657  }
658  }else{
659  m_log << " Device doesn't do image registration!" << endl;
660  }
661 
662  if (0) // hasColor())
663  { // printf("DBG: hasColor() returned TRUE\n");
664  m_streams[COLOR_STREAM]->disableAutoExposure();
665  printf("DBG: returned from disableAutoExposure()\n");
666  }
667 
668  if(startStreams() == false){
669  close();
670  return false;
671  }
672  return true;
673  MRPT_END
674 }
675 
676 bool COpenNI2Generic::CDevice::getNextFrameRGB(mrpt::utils::CImage &img, uint64_t &timestamp, bool &there_is_obs, bool &hardware_error){
677  MRPT_START
678  if(!hasColor()){
679  THROW_EXCEPTION("This OpenNI2 device does not support color imaging")
680  }
681  openni::VideoFrameRef frame;
682  if(m_streams[COLOR_STREAM]->getFrame(frame, timestamp, there_is_obs, hardware_error) == false){
683  return false;
684  }
685  copyFrame<openni::RGB888Pixel, mrpt::utils::CImage>(frame, img);
686 
687  return true;
688  MRPT_END
689 }
690 
691 bool COpenNI2Generic::CDevice::getNextFrameD(mrpt::math::CMatrix &img, uint64_t &timestamp, bool &there_is_obs, bool &hardware_error){
692  MRPT_START
693  if(!hasDepth()){
694  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging")
695  }
696  openni::VideoFrameRef frame;
697  if(m_streams[DEPTH_STREAM]->getFrame(frame, timestamp, there_is_obs, hardware_error) == false){
698  return false;
699  }
700  copyFrame<openni::DepthPixel, mrpt::math::CMatrix>(frame, img);
701 
702  return true;
703  MRPT_END
704 }
705 
706 bool COpenNI2Generic::CDevice::getNextFrameRGBD(mrpt::obs::CObservation3DRangeScan &obs, bool &there_is_obs, bool &hardware_error){
707  MRPT_START
708  clearLog();
709  there_is_obs = false;
710  hardware_error = false;
711 
712  if(!hasColor()){
713  THROW_EXCEPTION("This OpenNI2 device does not support color imaging")
714  }
715  if(!hasDepth()){
716  THROW_EXCEPTION("This OpenNI2 device does not support depth imaging")
717  }
718  // Read a frame (depth + rgb)
720  openni::VideoFrameRef frame[STREAM_TYPE_SIZE];
721  for(int i = 0;i < STREAM_TYPE_SIZE;++i){
722  if (!m_streams[i] || !m_streams[i]->isValid()) continue;
723  if(m_streams[i]->getFrame(frame[i], tm, there_is_obs, hardware_error) == false){
724  return false;
725  }
726  if(there_is_obs == false || hardware_error == true){
727  return false;
728  }
729  }
730 
731  const int width = frame[COLOR_STREAM].getWidth();
732  const int height = frame[COLOR_STREAM].getHeight();
733  if((frame[DEPTH_STREAM].getWidth() != width) || (frame[DEPTH_STREAM].getHeight() != height)){
734  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Both frames don't have the same size." << std::endl;
735  return false;
736  }
737  there_is_obs = true;
738  obs.hasConfidenceImage = false;
739  obs.hasIntensityImage = true;
740  obs.hasRangeImage = true;
741  obs.range_is_depth = true;
742  obs.hasPoints3D = false;
744  resize(obs, width, height);
745 
746  const char* data[STREAM_TYPE_SIZE] =
747  {
748  (const char*)frame[COLOR_STREAM].getData(),
749  (const char*)frame[DEPTH_STREAM].getData()
750  };
751  const int step[STREAM_TYPE_SIZE] =
752  {
753  frame[COLOR_STREAM].getStrideInBytes(),
754  frame[DEPTH_STREAM].getStrideInBytes()
755  };
756 
757  for (int yc = 0; yc < height; ++yc){
758  const openni::RGB888Pixel* pRgb = (const openni::RGB888Pixel*)data[COLOR_STREAM];
759  const openni::DepthPixel * pDepth = (const openni::DepthPixel *)data[DEPTH_STREAM];
760  for (int xc = 0; xc < width; ++xc, ++pDepth, ++pRgb){
761  int x = xc;
762  if(isMirrorMode()){
763  x = width - x - 1;
764  }
765  setPixel(*pRgb , obs.intensityImage, x, yc);
766  setPixel(*pDepth, obs.rangeImage , x, yc);
767  }
768  data[COLOR_STREAM] += step[COLOR_STREAM];
769  data[DEPTH_STREAM] += step[DEPTH_STREAM];
770  }
771 
772  return true;
773  MRPT_END
774 }
775 
776 COpenNI2Generic::CDevice::Ptr COpenNI2Generic::CDevice::create(const openni::DeviceInfo& info, openni::PixelFormat rgb, openni::PixelFormat depth, bool verbose){
777  return Ptr(new CDevice(info, rgb, depth,verbose));
778 }
779 
780 bool COpenNI2Generic::CDevice::getSerialNumber(std::string& sn){
781  clearLog();
782  openni::Status rc;
783  bool isOpened = isOpen();
784  if(isOpened == false){
785  rc = m_device.open(getInfo().getUri());
786  if(rc != openni::STATUS_OK){
787  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Failed to open device " << getInfo().getUri() << " " << openni::OpenNI::getExtendedError() << std::endl;
788  return false;
789  }
790  }
791  char serialNumber[16];
792  rc = m_device.getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER, &serialNumber);
793  if(rc != openni::STATUS_OK){
794  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Failed to getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER) " << getInfo().getUri() << " " << openni::OpenNI::getExtendedError() << std::endl;
795  return false;
796  }
797  sn = std::string(serialNumber);
798  if(isOpened == false){
799  m_device.close();
800  }
801  return true;
802 }
803 
804 bool COpenNI2Generic::CDevice::getSerialNumber(unsigned int& sn){
805  std::string str;
806  if(getSerialNumber(str) == false){
807  return false;
808  }
809  std::stringstream sst;
810  sst.str(str);
811  sst >> sn;
812  return !sst.fail();
813 }
814 //
815 COpenNI2Generic::CDevice::CStream::CStream(openni::Device& device, openni::SensorType type, openni::PixelFormat format, std::ostream& log,bool verbose)
816  :m_log(log), m_device(device), m_strName("Unknown"), m_type(type), m_format(format), m_verbose(verbose)
817 {
818  if(m_type == openni::SENSOR_COLOR){
819  m_strName = "openni::SENSOR_COLOR";
820  }else if(m_type == openni::SENSOR_DEPTH){
821  m_strName = "openni::SENSOR_DEPTH";
822  }else if(m_type == openni::SENSOR_IR){
823  m_strName = "openni::SENSOR_IR";
824  }else{
825  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Unknown SensorType -> " << m_type << std::endl;
826  }
827 }
828 
829 COpenNI2Generic::CDevice::CStream::~CStream(){
830  destroy();
831 }
832 
833 bool COpenNI2Generic::CDevice::CStream::isMirrorSupported()const{
834  if(isValid() == false){
835  THROW_EXCEPTION(getName() + " is not opened.");
836  }
837  return m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING);
838 }
839 
840 bool COpenNI2Generic::CDevice::CStream::setMirror(bool flag){
841  if(isValid() == false){
842  m_log << "[" << __FUNCTION__ << "]" << std::endl << " " << getName() << " is not opened." << std::endl;
843  return false;
844  }
845  if(m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING) == false){
846  return false;
847  }
848  if(m_stream.setMirroringEnabled(flag) != openni::STATUS_OK){
849  m_log << "[" << __FUNCTION__ << "]" << std::endl << " setMirroringEnabled() failed: " << openni::OpenNI::getExtendedError() << std::endl;
850  return false;
851  }
852  return true;
853 }
854 
855 bool COpenNI2Generic::CDevice::CStream::isValid()const{
856  return m_stream.isValid();
857 }
858 
859 void COpenNI2Generic::CDevice::CStream::destroy(){
860  m_stream.destroy();
861 }
862 
863 void COpenNI2Generic::CDevice::CStream::setCloseRange(int& value){
864  if (m_verbose) printf(" [CDevice::CStream::setCloseRange] entry with value=%d\n",value);
865  m_stream.setProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, value);
866  if (m_verbose) printf(" [CDevice::CStream::setCloseRange] returned from mstream.setProperty()\n");
867  m_stream.getProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, &value);
868  if (m_verbose) printf(" [CDevice::CStream::setCloseRange] returned from mstream.getProperty() ... value %d\n",value);
869 }
870 
871 bool COpenNI2Generic::CDevice::CStream::open(int w, int h, int fps){
872  destroy();
873  if(m_type != openni::SENSOR_COLOR && m_type != openni::SENSOR_DEPTH && m_type != openni::SENSOR_IR){ // SEB added IR
874  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Unknown SensorType -> " << m_type << std::endl;
875  return false;
876  }
877  if (m_verbose) printf(" [COpenNI2Generic::CDevice::CStream::open] opening sensor stream with m_type == %d\n",(int)m_type);
878  openni::Status rc = openni::STATUS_OK;
879 // if(m_type == openni::SENSOR_COLOR) {
880 // m_type = openni::SENSOR_IR;
881 // m_strName="openni::SENSOR_IR"; // SEB added
882 // if (m_verbose) printf("DBG: changing type to SENSOR_IR (%d)\n",(int)m_type);
883 // } // added whole if stmt
884  rc = m_stream.create(m_device, m_type);
885  if(rc != openni::STATUS_OK){
886  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Couldn't find sensor "
887  << m_strName << ":" << openni::OpenNI::getExtendedError() << std::endl;
888  if(m_type == openni::SENSOR_COLOR) {
889  m_type = openni::SENSOR_IR;
890  m_strName="openni::SENSOR_IR"; // SEB added
891  if (m_verbose) printf("DBG: changing type to SENSOR_IR (%d)\n",(int)m_type);
892  rc = m_stream.create(m_device, m_type);
893  } // SEB added whole if stmt
894  else
895  return false;
896  }
897  if (m_verbose) printf("returned OK from stream.create()\n");
898  openni::VideoMode options = m_stream.getVideoMode();
899  m_log << "[" << __FUNCTION__ << "]" << std::endl;
900  m_log << " " << m_strName << std::endl;
901  m_log << " " << mrpt::format("Initial resolution (%d, %d) FPS %d Format %d", options.getResolutionX(), options.getResolutionY(), options.getFps(), options.getPixelFormat()) << std::endl;
902  if (m_verbose) printf("DBG: calling setONI2StreamMode()\n");
903  if(setONI2StreamMode(m_stream, w, h, fps, m_format) == false){
904  m_log << " Can't find desired mode in the " << getName() << std::endl;
905  destroy();
906  return false;
907  }
908  if (m_verbose) printf("DBG: returned OK from setONI2StreamMode()\n");
909  if (m_verbose) printf("DBG: calling stream.getVideoMode()\n");
910  options = m_stream.getVideoMode();
911  m_log << " " << mrpt::format("-> (%d, %d) FPS %d Format %d", options.getResolutionX(), options.getResolutionY(), options.getFps(), options.getPixelFormat()) << std::endl;
912  if (m_verbose) printf(" [COpenNI2Generic::CDevice::CStream::open] returning TRUE\n");
913  return true;
914 }
915 
917  if(isValid() == false){
918  m_log << "[" << __FUNCTION__ << "]" << std::endl << " " << getName() << " is not opened." << std::endl;
919  return false;
920  }
921  if(m_stream.start() != openni::STATUS_OK){
922  m_log << "[" << __FUNCTION__ << "]" << std::endl << " Couldn't start " << getName() << " stream:" << openni::OpenNI::getExtendedError() << std::endl;
923  this->destroy();
924  return false;
925  }
926  return true;
927 }
928 
929 COpenNI2Generic::CDevice::CStream::Ptr COpenNI2Generic::CDevice::CStream::create(openni::Device& device, openni::SensorType type, openni::PixelFormat format, std::ostream& log, bool verbose){
930  return Ptr(new CStream(device, type, format, log,verbose));
931 }
932 
933 bool COpenNI2Generic::CDevice::CStream::getFrame(openni::VideoFrameRef& frame, uint64_t &timestamp, bool &there_is_obs, bool &hardware_error)
934 {
935  there_is_obs = false;
936  hardware_error = false;
937  if(isValid() == false){
938  return false;
939  }
940  openni::Status rc = m_stream.readFrame(&frame);
941  if(rc != openni::STATUS_OK){
942  hardware_error = true;
943  std::string message = mrpt::format("Failed to grab frame from %s", getName().c_str());
944  THROW_EXCEPTION(message);
945  }
946  there_is_obs = true;
947  timestamp = mrpt::system::getCurrentTime();
948  return true;
949 }
950 
951 #endif // MRPT_HAS_OPENNI2
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
bool m_grab_image
The data that the RGBD sensors can return.
Status
Definition: xmlParser.cpp:523
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glew.h:1168
void getNextFrameRGB(mrpt::utils::CImage &rgb_img, uint64_t &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 namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
bool getDeviceIDFromSerialNum(const unsigned int SerialRequired, int &sensor_id) const
Get the ID of the device corresponding to &#39;SerialRequired&#39;.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
#define THROW_EXCEPTION(msg)
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
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)
std::vector< stlplus::smart_ptr< COpenNI2Generic::CDevice > > vDevices
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLuint start
Definition: glew.h:1167
bool getColorSensorParam(mrpt::utils::TCamera &param, unsigned sensor_id=0) const
int m_width
The same options (width, height and fps) are set for all the sensors.
void kill()
Kill the OpenNI2 driver.
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
GLsizei const GLfloat * value
Definition: glew.h:1756
GLint GLvoid * img
Definition: glew.h:1290
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)...
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
static int getNumInstances()
Get the number of OpenNI2 cameras currently open via COpenNI2Generic.
void showLog(const std::string &message) const
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
This namespace contains representation of robot actions and observations.
int getNumDevices() const
The number of available devices at initialization.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
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...
std::atomic< int > numInstances(0)
This namespace provides multitask, synchronization utilities.
Definition: atomic_incr.h:29
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
bool hasPoints3D
true means the field points3D contains valid data.
GLint GLint GLint GLint GLint GLint GLsizei GLsizei height
Definition: glew.h:1166
void getNextFrameD(mrpt::math::CMatrix &depth_img, uint64_t &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 isOpen(const unsigned sensor_id) const
Whether there is a working connection to the sensor.
unsigned int openDevicesBySerialNum(const std::set< unsigned > &vSerialRequired)
Open a set of RGBD devices specified by their serial number.
#define MRPT_START
unsigned __int64 uint64_t
Definition: rptypes.h:52
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
class HWDRIVERS_IMPEXP CDevice
backing_store_ptr info
Definition: jmemsys.h:170
bool hasIntensityImage
true means the field intensityImage contains valid data
std::recursive_mutex vDevices_mx
int numDevices
Definition: wglew.h:895
void open(unsigned sensor_id=0)
Try to open the camera (all the parameters [resolution,fps,...] must be set before calling this) - us...
int getConnectedDevices()
Get a list of the connected OpenNI2 sensors.
bool m_grab_3D_points
Default: all true.
GLfloat param
Definition: glew.h:1435
GLuint GLuint GLsizei GLenum type
Definition: glew.h:1167
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
GLint GLint GLint GLint GLint GLint GLsizei width
Definition: glew.h:1166
double getHeight(const TPolygon3D &p, const TPoint3D &c)
bool getDepthSensorParam(mrpt::utils::TCamera &param, unsigned sensor_id=0) const
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31
bool hasConfidenceImage
true means the field confidenceImage contains valid data
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018