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