1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  *     * Redistributions of source code must retain the above copyright
9  *       notice, this list of conditions and the following disclaimer.
10  *     * Redistributions in binary form must reproduce the above copyright
11  *       notice, this list of conditions and the following disclaimer in the
12  *       documentation and/or other materials provided with the distribution.
13  *     * Neither the name of the Willow Garage, Inc. nor the names of its
14  *       contributors may be used to endorse or promote products derived from
15  *       this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  *      Author: Julius Kammerl (jkammerl@willowgarage.com)
30  */
31 
32 #include <OpenNI.h>
33 #include <PS1080.h> // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property
34 
35 #include <boost/algorithm/string/replace.hpp>
36 
37 #include "pcl/io/openni2/openni2_device.h"
38 #include "pcl/io/openni2/openni2_convert.h"
39 #include "pcl/io/openni2/openni2_frame_listener.h"
40 
41 #include "pcl/io/io_exception.h"
42 
43 #include <string>
44 
45 using namespace openni;
46 using namespace pcl::io::openni2;
47 
48 using openni::VideoMode;
49 using std::vector;
50 
OpenNI2Device(const std::string & device_URI)51 pcl::io::openni2::OpenNI2Device::OpenNI2Device (const std::string& device_URI) :
52   ir_video_started_(false),
53   color_video_started_(false),
54   depth_video_started_(false)
55 {
56   openni::Status status = openni::OpenNI::initialize ();
57   if (status != openni::STATUS_OK)
58     THROW_IO_EXCEPTION ("Initialize failed\n%s\n", OpenNI::getExtendedError ());
59 
60   openni_device_.reset (new openni::Device);
61 
62   if (device_URI.length () > 0)
63     status = openni_device_->open (device_URI.c_str ());
64   else
65     status = openni_device_->open (openni::ANY_DEVICE);
66 
67   if (status != openni::STATUS_OK)
68     THROW_IO_EXCEPTION ("Initialize failed\n%s\n", openni::OpenNI::getExtendedError ());
69 
70   // Get depth calculation parameters
71   // Some of these are device-spefic and may not exist
72   baseline_ = 0.0;
73   if ( getDepthVideoStream ()->isPropertySupported (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE) )
74   {
75     double baseline;
76     getDepthVideoStream ()->getProperty (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline); // Device specific -- from PS1080.h
77     baseline_ = static_cast<float> (baseline * 0.01f);  // baseline from cm -> meters
78   }
79   shadow_value_ = 0;     // This does not exist in OpenNI 2, and is maintained for compatibility with the OpenNI 1.x grabber
80   no_sample_value_ = 0;  // This does not exist in OpenNI 2, and is maintained for compatibility with the OpenNI 1.x grabber
81 
82   // Set default resolution if not reading a file
83   if (!openni_device_->isFile ())
84   {
85     if (openni_device_->hasSensor (openni::SENSOR_COLOR))
86     {
87       setColorVideoMode (getDefaultColorMode ());
88     }
89     if (openni_device_->hasSensor (openni::SENSOR_DEPTH))
90     {
91       setDepthVideoMode (getDefaultDepthMode ());
92     }
93     if (openni_device_->hasSensor (openni::SENSOR_IR))
94     {
95       setIRVideoMode (getDefaultIRMode ());
96     }
97   }
98 
99   if (openni_device_->isFile ())
100   {
101     openni_device_->getPlaybackControl ()->setSpeed (1.0f);
102   }
103 
104   device_info_.reset (new openni::DeviceInfo);
105   *device_info_ = openni_device_->getDeviceInfo ();
106 
107   color_frame_listener.reset (new OpenNI2FrameListener);
108   depth_frame_listener.reset (new OpenNI2FrameListener);
109   ir_frame_listener.reset (new OpenNI2FrameListener);
110 }
111 
~OpenNI2Device()112 pcl::io::openni2::OpenNI2Device::~OpenNI2Device ()
113 {
114   stopAllStreams ();
115 
116   shutdown ();
117 
118   openni_device_->close ();
119 }
120 
121 const std::string
getUri() const122 pcl::io::openni2::OpenNI2Device::getUri () const
123 {
124   return (std::string (device_info_->getUri ()));
125 }
126 
127 const std::string
getVendor() const128 pcl::io::openni2::OpenNI2Device::getVendor () const
129 {
130   return (std::string (device_info_->getVendor ()));
131 }
132 
133 const std::string
getName() const134 pcl::io::openni2::OpenNI2Device::getName () const
135 {
136   return (std::string (device_info_->getName ()));
137 }
138 
139 std::uint16_t
getUsbVendorId() const140 pcl::io::openni2::OpenNI2Device::getUsbVendorId () const
141 {
142   return (device_info_->getUsbVendorId ());
143 }
144 
145 std::uint16_t
getUsbProductId() const146 pcl::io::openni2::OpenNI2Device::getUsbProductId () const
147 {
148   return (device_info_->getUsbProductId ());
149 }
150 
151 const std::string
getStringID() const152 pcl::io::openni2::OpenNI2Device::getStringID () const
153 {
154   std::string ID_str = getName () + "_" + getVendor ();
155 
156   boost::replace_all (ID_str, "/", "");
157   boost::replace_all (ID_str, ".", "");
158   boost::replace_all (ID_str, "@", "");
159 
160   return (ID_str);
161 }
162 
163 bool
isValid() const164 pcl::io::openni2::OpenNI2Device::isValid () const
165 {
166   return (openni_device_.get () != nullptr) && openni_device_->isValid ();
167 }
168 
169 float
getIRFocalLength() const170 pcl::io::openni2::OpenNI2Device::getIRFocalLength () const
171 {
172   auto stream = getIRVideoStream ();
173 
174   int frameWidth = stream->getVideoMode ().getResolutionX ();
175   float hFov = stream->getHorizontalFieldOfView ();
176   float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f));
177   return (calculatedFocalLengthX);
178 }
179 
180 float
getColorFocalLength() const181 pcl::io::openni2::OpenNI2Device::getColorFocalLength () const
182 {
183   auto stream = getColorVideoStream ();
184 
185   int frameWidth = stream->getVideoMode ().getResolutionX ();
186   float hFov = stream->getHorizontalFieldOfView ();
187   float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f));
188   return (calculatedFocalLengthX);
189 }
190 
191 float
getDepthFocalLength() const192 pcl::io::openni2::OpenNI2Device::getDepthFocalLength () const
193 {
194   auto stream = getDepthVideoStream ();
195 
196   int frameWidth = stream->getVideoMode ().getResolutionX ();
197   float hFov = stream->getHorizontalFieldOfView ();
198   float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f));
199   return (calculatedFocalLengthX);
200 }
201 
202 float
getBaseline()203 pcl::io::openni2::OpenNI2Device::getBaseline()
204 {
205   return (baseline_);
206 }
207 
208 std::uint64_t
getShadowValue()209 pcl::io::openni2::OpenNI2Device::getShadowValue()
210 {
211   return (shadow_value_);
212 }
213 
214 bool
isIRVideoModeSupported(const OpenNI2VideoMode & video_mode) const215 pcl::io::openni2::OpenNI2Device::isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const
216 {
217   getSupportedIRVideoModes ();
218 
219   bool supported = false;
220 
221   std::vector<OpenNI2VideoMode>::const_iterator it = ir_video_modes_.begin ();
222   std::vector<OpenNI2VideoMode>::const_iterator it_end = ir_video_modes_.end ();
223 
224   while (it != it_end && !supported)
225   {
226     supported = (*it == video_mode);
227     ++it;
228   }
229 
230   return (supported);
231 }
232 
233 bool
isColorVideoModeSupported(const OpenNI2VideoMode & video_mode) const234 pcl::io::openni2::OpenNI2Device::isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const
235 {
236   getSupportedColorVideoModes ();
237 
238   bool supported = false;
239 
240   std::vector<OpenNI2VideoMode>::const_iterator it = color_video_modes_.begin ();
241   std::vector<OpenNI2VideoMode>::const_iterator it_end = color_video_modes_.end ();
242 
243   while (it != it_end && !supported)
244   {
245     supported = (*it == video_mode);
246     ++it;
247   }
248 
249   return (supported);
250 }
251 
252 bool
isDepthVideoModeSupported(const OpenNI2VideoMode & video_mode) const253 pcl::io::openni2::OpenNI2Device::isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const
254 {
255   getSupportedDepthVideoModes ();
256 
257   bool supported = false;
258 
259   std::vector<OpenNI2VideoMode>::const_iterator it = depth_video_modes_.begin ();
260   std::vector<OpenNI2VideoMode>::const_iterator it_end = depth_video_modes_.end ();
261 
262   while (it != it_end && !supported)
263   {
264     supported = (*it == video_mode);
265     ++it;
266   }
267 
268   return (supported);
269 }
270 
271 bool
hasIRSensor() const272 pcl::io::openni2::OpenNI2Device::hasIRSensor () const
273 {
274   return (openni_device_->hasSensor (openni::SENSOR_IR));
275 }
276 
277 bool
hasColorSensor() const278 pcl::io::openni2::OpenNI2Device::hasColorSensor () const
279 {
280   return (openni_device_->hasSensor (openni::SENSOR_COLOR));
281 }
282 
283 bool
hasDepthSensor() const284 pcl::io::openni2::OpenNI2Device::hasDepthSensor () const
285 {
286   return (openni_device_->hasSensor (openni::SENSOR_DEPTH));
287 }
288 
289 void
startIRStream()290 pcl::io::openni2::OpenNI2Device::startIRStream ()
291 {
292   if (auto stream = getIRVideoStream ())
293   {
294     stream->setMirroringEnabled (false);
295     stream->addNewFrameListener (ir_frame_listener.get ());
296     stream->start ();
297     ir_video_started_ = true;
298   }
299 }
300 
301 void
startColorStream()302 pcl::io::openni2::OpenNI2Device::startColorStream ()
303 {
304   if (auto stream = getColorVideoStream ())
305   {
306     stream->setMirroringEnabled (false);
307     stream->addNewFrameListener (color_frame_listener.get ());
308     stream->start ();
309     color_video_started_ = true;
310   }
311 }
312 void
startDepthStream()313 pcl::io::openni2::OpenNI2Device::startDepthStream ()
314 {
315   if (auto stream = getDepthVideoStream ())
316   {
317     stream->setMirroringEnabled (false);
318     stream->addNewFrameListener (depth_frame_listener.get ());
319     stream->start ();
320     depth_video_started_ = true;
321   }
322 }
323 
324 void
stopAllStreams()325 pcl::io::openni2::OpenNI2Device::stopAllStreams ()
326 {
327   stopIRStream ();
328   stopColorStream ();
329   stopDepthStream ();
330 }
331 
332 void
stopIRStream()333 pcl::io::openni2::OpenNI2Device::stopIRStream ()
334 {
335   if (ir_video_stream_.get () != nullptr)
336   {
337     ir_video_stream_->stop ();
338     ir_video_started_ = false;
339   }
340 }
341 void
stopColorStream()342 pcl::io::openni2::OpenNI2Device::stopColorStream ()
343 {
344   if (color_video_stream_.get () != nullptr)
345   {
346     color_video_stream_->stop ();
347     color_video_started_ = false;
348   }
349 }
350 void
stopDepthStream()351 pcl::io::openni2::OpenNI2Device::stopDepthStream ()
352 {
353   if (depth_video_stream_.get () != nullptr)
354   {
355     depth_video_stream_->stop ();
356     depth_video_started_ = false;
357   }
358 }
359 
360 void
shutdown()361 pcl::io::openni2::OpenNI2Device::shutdown ()
362 {
363   if (ir_video_stream_.get () != nullptr)
364     ir_video_stream_->destroy ();
365 
366   if (color_video_stream_.get () != nullptr)
367     color_video_stream_->destroy ();
368 
369   if (depth_video_stream_.get () != nullptr)
370     depth_video_stream_->destroy ();
371 
372 }
373 
374 bool
isIRStreamStarted()375 pcl::io::openni2::OpenNI2Device::isIRStreamStarted ()
376 {
377   return (ir_video_started_);
378 }
379 bool
isColorStreamStarted()380 pcl::io::openni2::OpenNI2Device::isColorStreamStarted ()
381 {
382   return (color_video_started_);
383 }
384 bool
isDepthStreamStarted()385 pcl::io::openni2::OpenNI2Device::isDepthStreamStarted ()
386 {
387   return (depth_video_started_);
388 }
389 
390 bool
isImageRegistrationModeSupported() const391 pcl::io::openni2::OpenNI2Device::isImageRegistrationModeSupported () const
392 {
393   return (openni_device_->isImageRegistrationModeSupported (openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR));
394 }
395 
396 void
setImageRegistrationMode(bool setEnable)397 pcl::io::openni2::OpenNI2Device::setImageRegistrationMode (bool setEnable)
398 {
399   bool registrationSupported = isImageRegistrationModeSupported ();
400   if (registrationSupported)
401   {
402     if (setEnable)
403     {
404       openni::Status rc = openni_device_->setImageRegistrationMode (openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
405       if (rc != openni::STATUS_OK)
406         THROW_IO_EXCEPTION ("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError ());
407     }
408     else
409     {
410       openni::Status rc = openni_device_->setImageRegistrationMode (openni::IMAGE_REGISTRATION_OFF);
411       if (rc != openni::STATUS_OK)
412         THROW_IO_EXCEPTION ("Disabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError ());
413     }
414   }
415 }
416 
417 bool
isDepthRegistered() const418 pcl::io::openni2::OpenNI2Device::isDepthRegistered () const
419 {
420   return openni_device_->getImageRegistrationMode () == openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR;
421 }
422 
423 void
setSynchronization(bool enabled)424 pcl::io::openni2::OpenNI2Device::setSynchronization (bool enabled)
425 {
426   openni::Status rc = openni_device_->setDepthColorSyncEnabled (enabled);
427   if (rc != openni::STATUS_OK)
428     THROW_IO_EXCEPTION ("Enabling depth color synchronization failed: \n%s\n", openni::OpenNI::getExtendedError ());
429 }
430 
431 const OpenNI2VideoMode
getIRVideoMode()432 pcl::io::openni2::OpenNI2Device::getIRVideoMode ()
433 {
434   if (auto stream = getIRVideoStream ())
435     return openniModeToGrabberMode (stream->getVideoMode ());
436   THROW_IO_EXCEPTION ("Could not create video stream.");
437   return {};
438 }
439 
440 const OpenNI2VideoMode
getColorVideoMode()441 pcl::io::openni2::OpenNI2Device::getColorVideoMode ()
442 {
443   if (auto stream = getColorVideoStream ())
444     return openniModeToGrabberMode (stream->getVideoMode ());
445   THROW_IO_EXCEPTION ("Could not create video stream.");
446   return {};
447 }
448 
449 const OpenNI2VideoMode
getDepthVideoMode()450 pcl::io::openni2::OpenNI2Device::getDepthVideoMode ()
451 {
452   if (auto stream = getDepthVideoStream ())
453     return openniModeToGrabberMode (stream->getVideoMode ());
454   THROW_IO_EXCEPTION ("Could not create video stream.");
455   return {};
456 }
457 
458 void
setIRVideoMode(const OpenNI2VideoMode & video_mode)459 pcl::io::openni2::OpenNI2Device::setIRVideoMode (const OpenNI2VideoMode& video_mode)
460 {
461   if (auto stream = getIRVideoStream ())
462   {
463     const openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode);
464     const openni::Status rc = stream->setVideoMode (videoMode);
465     if (rc != openni::STATUS_OK)
466       THROW_IO_EXCEPTION ("Couldn't set IR video mode: \n%s\n", openni::OpenNI::getExtendedError ());
467   }
468 }
469 
470 void
setColorVideoMode(const OpenNI2VideoMode & video_mode)471 pcl::io::openni2::OpenNI2Device::setColorVideoMode (const OpenNI2VideoMode& video_mode)
472 {
473   if (auto stream = getColorVideoStream ())
474   {
475     openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode);
476     const openni::Status rc = stream->setVideoMode (videoMode);
477     if (rc != openni::STATUS_OK)
478       THROW_IO_EXCEPTION ("Couldn't set color video mode: \n%s\n", openni::OpenNI::getExtendedError ());
479   }
480 }
481 
482 void
setDepthVideoMode(const OpenNI2VideoMode & video_mode)483 pcl::io::openni2::OpenNI2Device::setDepthVideoMode (const OpenNI2VideoMode& video_mode)
484 {
485   if (auto stream = getDepthVideoStream ())
486   {
487     const openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode);
488     const openni::Status rc = stream->setVideoMode (videoMode);
489     if (rc != openni::STATUS_OK)
490       THROW_IO_EXCEPTION ("Couldn't set depth video mode: \n%s\n", openni::OpenNI::getExtendedError ());
491   }
492 }
493 
494 OpenNI2VideoMode
getDefaultIRMode() const495 pcl::io::openni2::OpenNI2Device::getDefaultIRMode () const
496 {
497   // Search for and return VGA@30 Hz mode
498   std::vector<OpenNI2VideoMode> modeList = getSupportedIRVideoModes ();
499   for (const auto &mode : modeList)
500   {
501     if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
502       return mode;
503   }
504   if (modeList.empty())
505       THROW_IO_EXCEPTION("Device claims to have a IR sensor, but doesn't have any IR streaming mode");
506   return (modeList.at (0)); // Return first mode if we can't find VGA
507 }
508 
509 OpenNI2VideoMode
getDefaultColorMode() const510 pcl::io::openni2::OpenNI2Device::getDefaultColorMode () const
511 {
512   // Search for and return VGA@30 Hz mode
513   std::vector<OpenNI2VideoMode> modeList = getSupportedColorVideoModes ();
514   for (const auto &mode : modeList)
515   {
516     if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
517       return mode;
518   }
519   if (modeList.empty())
520       THROW_IO_EXCEPTION("Device claims to have a color sensor, but doesn't have any color streaming mode");
521   return (modeList.at (0)); // Return first mode if we can't find VGA
522 }
523 
524 OpenNI2VideoMode
getDefaultDepthMode() const525 pcl::io::openni2::OpenNI2Device::getDefaultDepthMode () const
526 {
527   // Search for and return VGA@30 Hz mode
528   std::vector<OpenNI2VideoMode> modeList = getSupportedDepthVideoModes ();
529   for (const auto &mode : modeList)
530   {
531     if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
532       return mode;
533   }
534   if (modeList.empty())
535       THROW_IO_EXCEPTION("Device claims to have a depth sensor, but doesn't have any depth streaming mode");
536   return (modeList.at (0)); // Return first mode if we can't find VGA
537 }
538 
539 const std::vector<OpenNI2VideoMode>&
getSupportedIRVideoModes() const540 pcl::io::openni2::OpenNI2Device::getSupportedIRVideoModes () const
541 {
542   ir_video_modes_.clear ();
543 
544   if (auto stream = getIRVideoStream ())
545   {
546     const openni::SensorInfo& sensor_info = stream->getSensorInfo ();
547     ir_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ());
548   }
549 
550   return (ir_video_modes_);
551 }
552 
553 const std::vector<OpenNI2VideoMode>&
getSupportedColorVideoModes() const554 pcl::io::openni2::OpenNI2Device::getSupportedColorVideoModes () const
555 {
556   color_video_modes_.clear ();
557 
558   if (auto stream = getColorVideoStream ())
559   {
560     const openni::SensorInfo& sensor_info = stream->getSensorInfo ();
561     color_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ());
562   }
563 
564   return (color_video_modes_);
565 }
566 
567 const std::vector<OpenNI2VideoMode>&
getSupportedDepthVideoModes() const568 pcl::io::openni2::OpenNI2Device::getSupportedDepthVideoModes () const
569 {
570   depth_video_modes_.clear ();
571 
572   if (auto stream = getDepthVideoStream ())
573   {
574     const openni::SensorInfo& sensor_info = stream->getSensorInfo ();
575     depth_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ());
576   }
577 
578   return (depth_video_modes_);
579 }
580 
581 bool
findCompatibleIRMode(const OpenNI2VideoMode & requested_mode,OpenNI2VideoMode & actual_mode) const582 pcl::io::openni2::OpenNI2Device::findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const
583 {
584   if ( isIRVideoModeSupported (requested_mode) )
585   {
586     actual_mode = requested_mode;
587     return (true);
588   }
589   // Find a resize-compatable mode
590   std::vector<OpenNI2VideoMode> supportedModes = getSupportedIRVideoModes ();
591   bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode);
592   return (found);
593 }
594 
595 bool
findCompatibleColorMode(const OpenNI2VideoMode & requested_mode,OpenNI2VideoMode & actual_mode) const596 pcl::io::openni2::OpenNI2Device::findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const
597 {
598   if ( isColorVideoModeSupported (requested_mode) )
599   {
600     actual_mode = requested_mode;
601     return (true);
602   }
603   // Find a resize-compatable mode
604   std::vector<OpenNI2VideoMode> supportedModes = getSupportedColorVideoModes ();
605   bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode);
606   return (found);
607 }
608 
609 bool
findCompatibleDepthMode(const OpenNI2VideoMode & requested_mode,OpenNI2VideoMode & actual_mode) const610 pcl::io::openni2::OpenNI2Device::findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const
611 {
612   if ( isDepthVideoModeSupported (requested_mode) )
613   {
614     actual_mode = requested_mode;
615     return (true);
616   }
617   // Find a resize-compatable mode
618   std::vector<OpenNI2VideoMode> supportedModes = getSupportedDepthVideoModes ();
619   bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode);
620   return (found);
621 }
622 
623 // Generic support method for the above findCompatable...Mode calls above
624 bool
findCompatibleVideoMode(const std::vector<OpenNI2VideoMode> & supportedModes,const OpenNI2VideoMode & requested_mode,OpenNI2VideoMode & actual_mode) const625 pcl::io::openni2::OpenNI2Device::findCompatibleVideoMode (const std::vector<OpenNI2VideoMode>& supportedModes, const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const
626 {
627   bool found = false;
628   for (const auto &supportedMode : supportedModes)
629   {
630     if (supportedMode.frame_rate_ == requested_mode.frame_rate_
631       && resizingSupported (supportedMode.x_resolution_, supportedMode.y_resolution_, requested_mode.x_resolution_, requested_mode.y_resolution_))
632     {
633       if (found)
634       { // check whether the new mode is better -> smaller than the current one.
635         if (actual_mode.x_resolution_ * actual_mode.x_resolution_ > supportedMode.x_resolution_ * supportedMode.y_resolution_ )
636           actual_mode = supportedMode;
637       }
638       else
639       {
640         actual_mode = supportedMode;
641         found = true;
642       }
643     }
644   }
645   return (found);
646 }
647 
648 bool
resizingSupported(std::size_t input_width,std::size_t input_height,std::size_t output_width,std::size_t output_height) const649 pcl::io::openni2::OpenNI2Device::resizingSupported (std::size_t input_width, std::size_t input_height, std::size_t output_width, std::size_t output_height) const
650 {
651   return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 );
652 }
653 
654 void
setAutoExposure(bool enable)655 pcl::io::openni2::OpenNI2Device::setAutoExposure (bool enable)
656 {
657   if (auto stream = getColorVideoStream ())
658   {
659     openni::CameraSettings* camera_seeting = stream->getCameraSettings ();
660     if (camera_seeting)
661     {
662       const openni::Status rc = camera_seeting->setAutoExposureEnabled (enable);
663       if (rc != openni::STATUS_OK)
664         THROW_IO_EXCEPTION ("Couldn't set auto exposure: \n%s\n", openni::OpenNI::getExtendedError ());
665     }
666 
667   }
668 }
669 
670 void
setAutoWhiteBalance(bool enable)671 pcl::io::openni2::OpenNI2Device::setAutoWhiteBalance (bool enable)
672 {
673   if (auto stream = getColorVideoStream ())
674   {
675     openni::CameraSettings* camera_seeting = stream->getCameraSettings ();
676     if (camera_seeting)
677     {
678       const openni::Status rc = camera_seeting->setAutoWhiteBalanceEnabled (enable);
679       if (rc != openni::STATUS_OK)
680         THROW_IO_EXCEPTION ("Couldn't set auto white balance: \n%s\n", openni::OpenNI::getExtendedError ());
681     }
682   }
683 }
684 
685 bool
getAutoExposure() const686 pcl::io::openni2::OpenNI2Device::getAutoExposure () const
687 {
688   bool ret = false;
689 
690   if (auto stream = getColorVideoStream ())
691   {
692     openni::CameraSettings* camera_seeting = stream->getCameraSettings ();
693     if (camera_seeting)
694       ret = camera_seeting->getAutoExposureEnabled ();
695   }
696 
697   return (ret);
698 }
699 
700 bool
getAutoWhiteBalance() const701 pcl::io::openni2::OpenNI2Device::getAutoWhiteBalance () const
702 {
703   bool ret = false;
704 
705   if (auto stream = getColorVideoStream ())
706   {
707     openni::CameraSettings* camera_setting = stream->getCameraSettings ();
708     if (camera_setting)
709       ret = camera_setting->getAutoWhiteBalanceEnabled ();
710   }
711 
712   return (ret);
713 }
714 
getDepthFrameCount()715 int pcl::io::openni2::OpenNI2Device::getDepthFrameCount ()
716 {
717   if (!openni_device_->isFile () || !getDepthVideoStream ())
718   {
719     return 0;
720   }
721   return openni_device_->getPlaybackControl ()->getNumberOfFrames(*getDepthVideoStream ());
722 }
723 
getColorFrameCount()724 int OpenNI2Device::getColorFrameCount ()
725 {
726   if (!openni_device_->isFile () || !getColorVideoStream ())
727   {
728     return 0;
729   }
730   return openni_device_->getPlaybackControl ()->getNumberOfFrames (*getColorVideoStream ());
731 }
732 
getIRFrameCount()733 int OpenNI2Device::getIRFrameCount ()
734 {
735   if (!openni_device_->isFile () || !getIRVideoStream ())
736   {
737     return 0;
738   }
739   return openni_device_->getPlaybackControl ()->getNumberOfFrames (*getIRVideoStream ());
740 }
741 
setPlaybackSpeed(double speed)742 bool OpenNI2Device::setPlaybackSpeed (double speed)
743 {
744     return openni_device_->getPlaybackControl ()->setSpeed (speed) == openni::STATUS_OK;
745 }
746 
747 std::shared_ptr<openni::VideoStream>
getIRVideoStream() const748 pcl::io::openni2::OpenNI2Device::getIRVideoStream () const
749 {
750   if (ir_video_stream_.get () == nullptr)
751   {
752     if (hasIRSensor ())
753     {
754       ir_video_stream_.reset (new openni::VideoStream);
755 
756       const openni::Status rc = ir_video_stream_->create (*openni_device_, openni::SENSOR_IR);
757       if (rc != openni::STATUS_OK)
758         THROW_IO_EXCEPTION ("Couldn't create IR video stream: \n%s\n", openni::OpenNI::getExtendedError ());
759     }
760   }
761   return (ir_video_stream_);
762 }
763 
764 std::shared_ptr<openni::VideoStream>
getColorVideoStream() const765 pcl::io::openni2::OpenNI2Device::getColorVideoStream () const
766 {
767   if (color_video_stream_.get () == nullptr)
768   {
769     if (hasColorSensor ())
770     {
771       color_video_stream_.reset (new openni::VideoStream);
772 
773       const openni::Status rc = color_video_stream_->create (*openni_device_, openni::SENSOR_COLOR);
774       if (rc != openni::STATUS_OK)
775         THROW_IO_EXCEPTION ("Couldn't create color video stream: \n%s\n", openni::OpenNI::getExtendedError ());
776     }
777   }
778   return (color_video_stream_);
779 }
780 
781 std::shared_ptr<openni::VideoStream>
getDepthVideoStream() const782 pcl::io::openni2::OpenNI2Device::getDepthVideoStream () const
783 {
784   if (depth_video_stream_.get () == nullptr)
785   {
786     if (hasDepthSensor ())
787     {
788       depth_video_stream_.reset (new openni::VideoStream);
789 
790       const openni::Status rc = depth_video_stream_->create (*openni_device_, openni::SENSOR_DEPTH);
791       if (rc != openni::STATUS_OK)
792         THROW_IO_EXCEPTION ("Couldn't create depth video stream: \n%s\n", openni::OpenNI::getExtendedError ());
793     }
794   }
795   return (depth_video_stream_);
796 }
797 
operator <<(std::ostream & stream,const OpenNI2Device & device)798 std::ostream& pcl::io::openni2::operator<< (std::ostream& stream, const OpenNI2Device& device)
799 {
800 
801   stream << "Device info (" << device.getUri () << ")" << std::endl;
802   stream << "   Vendor: " << device.getVendor () << std::endl;
803   stream << "   Name: " << device.getName () << std::endl;
804   stream << "   USB Vendor ID: " << device.getUsbVendorId () << std::endl;
805   stream << "   USB Product ID: " << device.getUsbVendorId () << std::endl << std::endl;
806 
807   if (device.hasIRSensor ())
808   {
809     stream << "IR sensor video modes:" << std::endl;
810     const std::vector<OpenNI2VideoMode>& video_modes = device.getSupportedIRVideoModes ();
811 
812     for (const auto &video_mode : video_modes)
813       stream << "   - " << video_mode << std::endl;
814   }
815   else
816   {
817     stream << "No IR sensor available" << std::endl;
818   }
819 
820   if (device.hasColorSensor ())
821   {
822     stream << "Color sensor video modes:" << std::endl;
823     const std::vector<OpenNI2VideoMode>& video_modes = device.getSupportedColorVideoModes ();
824 
825     for (const auto &video_mode : video_modes)
826       stream << "   - " << video_mode << std::endl;
827   }
828   else
829   {
830     stream << "No Color sensor available" << std::endl;
831   }
832 
833   if (device.hasDepthSensor ())
834   {
835     stream << "Depth sensor video modes:" << std::endl;
836     const std::vector<OpenNI2VideoMode>& video_modes = device.getSupportedDepthVideoModes ();
837 
838     for (const auto &video_mode : video_modes)
839       stream << "   - " << video_mode << std::endl;
840   }
841   else
842   {
843     stream << "No Depth sensor available" << std::endl;
844   }
845 
846   return (stream);
847 
848 }
849 
850 void
setColorCallback(StreamCallbackFunction color_callback)851 pcl::io::openni2::OpenNI2Device::setColorCallback (StreamCallbackFunction color_callback)
852 {
853   color_frame_listener->setCallback (std::move(color_callback));
854 }
855 
856 void
setDepthCallback(StreamCallbackFunction depth_callback)857 pcl::io::openni2::OpenNI2Device::setDepthCallback (StreamCallbackFunction depth_callback)
858 {
859   depth_frame_listener->setCallback (std::move(depth_callback));
860 }
861 
862 void
setIRCallback(StreamCallbackFunction ir_callback)863 pcl::io::openni2::OpenNI2Device::setIRCallback (StreamCallbackFunction ir_callback)
864 {
865   ir_frame_listener->setCallback (std::move(ir_callback));
866 }
867