Debian Codegraph @

libs/hwdrivers/src/COpenNI2Generic.cpp

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