1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41 #include "precomp.hpp"
42 #include "opencv2/core.hpp"
43 #include "opencv2/imgproc.hpp"
44
45 #ifdef HAVE_OPENNI2
46
47 #if defined TBB_INTERFACE_VERSION && TBB_INTERFACE_VERSION < 5000
48 # undef HAVE_TBB
49 #endif
50
51 #include <queue>
52
53 #ifndef i386
54 # define i386 0
55 #endif
56 #ifndef __arm__
57 # define __arm__ 0
58 #endif
59 #ifndef _ARC
60 # define _ARC 0
61 #endif
62 #ifndef __APPLE__
63 # define __APPLE__ 0
64 #endif
65
66 #define CV_STREAM_TIMEOUT 2000
67
68 #define CV_DEPTH_STREAM 0
69 #define CV_COLOR_STREAM 1
70
71 #include "OpenNI.h"
72 #include "PS1080.h"
73
74 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 class CvCapture_OpenNI2 : public CvCapture
76 {
77 public:
78 enum { DEVICE_DEFAULT=0, DEVICE_MS_KINECT=0, DEVICE_ASUS_XTION=1, DEVICE_MAX=1 };
79
80 static const int INVALID_PIXEL_VAL = 0;
81 static const int INVALID_COORDINATE_VAL = 0;
82
83 #ifdef HAVE_TBB
84 static const int DEFAULT_MAX_BUFFER_SIZE = 8;
85 #else
86 static const int DEFAULT_MAX_BUFFER_SIZE = 2;
87 #endif
88 static const int DEFAULT_IS_CIRCLE_BUFFER = 0;
89 static const int DEFAULT_MAX_TIME_DURATION = 20;
90
91 CvCapture_OpenNI2(int index = 0);
92 CvCapture_OpenNI2(const char * filename);
93 virtual ~CvCapture_OpenNI2();
94
95 virtual double getProperty(int propIdx) const;
96 virtual bool setProperty(int probIdx, double propVal);
97 virtual bool grabFrame();
98 virtual IplImage* retrieveFrame(int outputType);
99
100 bool isOpened() const;
101
102 protected:
103 struct OutputMap
104 {
105 public:
106 cv::Mat mat;
107 IplImage* getIplImagePtr();
108 private:
109 IplImage iplHeader;
110 };
111
112 static const int outputMapsTypesCount = 7;
113
114 static openni::VideoMode defaultColorOutputMode();
115 static openni::VideoMode defaultDepthOutputMode();
116
117 IplImage* retrieveDepthMap();
118 IplImage* retrievePointCloudMap();
119 IplImage* retrieveDisparityMap();
120 IplImage* retrieveDisparityMap_32F();
121 IplImage* retrieveValidDepthMask();
122 IplImage* retrieveBGRImage();
123 IplImage* retrieveGrayImage();
124
125 bool readCamerasParams();
126
127 double getDepthGeneratorProperty(int propIdx) const;
128 bool setDepthGeneratorProperty(int propIdx, double propVal);
129 double getImageGeneratorProperty(int propIdx) const;
130 bool setImageGeneratorProperty(int propIdx, double propVal);
131 double getCommonProperty(int propIdx) const;
132 bool setCommonProperty(int propIdx, double propVal);
133
134 // OpenNI context
135 openni::Device device;
136 bool isContextOpened;
137 openni::Recorder recorder;
138
139 // Data generators with its metadata
140 openni::VideoStream depth, color, **streams;
141 openni::VideoFrameRef depthFrame, colorFrame;
142 cv::Mat depthImage, colorImage;
143
144 int maxBufferSize, maxTimeDuration; // for approx sync
145 bool isCircleBuffer;
146 //cv::Ptr<ApproximateSyncGrabber> approxSyncGrabber;
147
148 // Cameras settings:
149 // TODO find in OpenNI function to convert z->disparity and remove fields "baseline" and depthFocalLength_VGA
150 // Distance between IR projector and IR camera (in meters)
151 double baseline;
152 // Focal length for the IR camera in VGA resolution (in pixels)
153 int depthFocalLength_VGA;
154
155 // The value for shadow (occluded pixels)
156 int shadowValue;
157 // The value for pixels without a valid disparity measurement
158 int noSampleValue;
159
160 int currentStream;
161
162 int numStream;
163 std::vector<OutputMap> outputMaps;
164 };
165
getIplImagePtr()166 IplImage* CvCapture_OpenNI2::OutputMap::getIplImagePtr()
167 {
168 if( mat.empty() )
169 return 0;
170
171 iplHeader = IplImage(mat);
172 return &iplHeader;
173 }
174
isOpened() const175 bool CvCapture_OpenNI2::isOpened() const
176 {
177 return isContextOpened;
178 }
179
defaultColorOutputMode()180 openni::VideoMode CvCapture_OpenNI2::defaultColorOutputMode()
181 {
182 openni::VideoMode mode;
183 mode.setResolution(640, 480);
184 mode.setFps(30);
185 mode.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
186 return mode;
187 }
188
defaultDepthOutputMode()189 openni::VideoMode CvCapture_OpenNI2::defaultDepthOutputMode()
190 {
191 openni::VideoMode mode;
192 mode.setResolution(640, 480);
193 mode.setFps(30);
194 mode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
195 return mode;
196 }
197
CvCapture_OpenNI2(int index)198 CvCapture_OpenNI2::CvCapture_OpenNI2( int index )
199 {
200 numStream = 2;
201 const char* deviceURI = openni::ANY_DEVICE;
202 openni::Status status;
203 int deviceType = DEVICE_DEFAULT;
204
205 noSampleValue = shadowValue = 0;
206
207 isContextOpened = false;
208 maxBufferSize = DEFAULT_MAX_BUFFER_SIZE;
209 isCircleBuffer = DEFAULT_IS_CIRCLE_BUFFER;
210 maxTimeDuration = DEFAULT_MAX_TIME_DURATION;
211
212 if( index >= 10 )
213 {
214 deviceType = index / 10;
215 index %= 10;
216 }
217
218 // Asus XTION and Occipital Structure Sensor do not have an image generator
219 if (deviceType == DEVICE_ASUS_XTION)
220 numStream = 1;
221
222 if( deviceType > DEVICE_MAX )
223 return;
224
225 // Initialize and configure the context.
226 status = openni::OpenNI::initialize();
227
228 if (status != openni::STATUS_OK)
229 {
230 CV_Error(CV_StsError, cv::format("Failed to initialize:", openni::OpenNI::getExtendedError()));
231 return;
232 }
233
234 status = device.open(deviceURI);
235 if( status != openni::STATUS_OK )
236 {
237 CV_Error(CV_StsError, cv::format("OpenCVKinect: Device open failed see: %s\n", openni::OpenNI::getExtendedError()));
238 openni::OpenNI::shutdown();
239 return;
240 }
241
242 //device.setDepthColorSyncEnabled(true);
243
244
245 status = depth.create(device, openni::SENSOR_DEPTH);
246 if (status == openni::STATUS_OK)
247 {
248 if (depth.isValid())
249 {
250 CV_Assert(depth.setVideoMode(defaultDepthOutputMode()) == openni::STATUS_OK); // xn::DepthGenerator supports VGA only! (Jan 2011)
251 }
252
253 status = depth.start();
254 if (status != openni::STATUS_OK)
255 {
256 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't start depth stream: %s\n", openni::OpenNI::getExtendedError()));
257 depth.destroy();
258 return;
259 }
260 }
261 else
262 {
263 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't find depth stream:: %s\n", openni::OpenNI::getExtendedError()));
264 return;
265 }
266
267 streams = new openni::VideoStream*[numStream];
268 streams[CV_DEPTH_STREAM] = &depth;
269
270 // create a color object
271 status = color.create(device, openni::SENSOR_COLOR);
272 if (status == openni::STATUS_OK)
273 {
274 // Set map output mode.
275 if (color.isValid())
276 {
277 CV_Assert(color.setVideoMode(defaultColorOutputMode()) == openni::STATUS_OK);
278 }
279 status = color.start();
280 if (status != openni::STATUS_OK)
281 {
282 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't start color stream: %s\n", openni::OpenNI::getExtendedError()));
283 color.destroy();
284 return;
285 }
286 streams[CV_COLOR_STREAM] = &color;
287 }
288 else if (numStream == 2)
289 {
290 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't find color stream: %s\n", openni::OpenNI::getExtendedError()));
291 return;
292 }
293
294 if( !readCamerasParams() )
295 {
296 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Could not read cameras parameters\n"));
297 return;
298 }
299
300 // if( deviceType == DEVICE_ASUS_XTION )
301 // {
302 // //ps/asus specific
303 // imageGenerator.SetIntProperty("InputFormat", 1 /*XN_IO_IMAGE_FORMAT_YUV422*/);
304 // imageGenerator.SetPixelFormat(XN_PIXEL_FORMAT_RGB24);
305 // depthGenerator.SetIntProperty("RegistrationType", 1 /*XN_PROCESSING_HARDWARE*/);
306 // }
307
308
309 outputMaps.resize( outputMapsTypesCount );
310
311 isContextOpened = true;
312
313 setProperty(CV_CAP_PROP_OPENNI_REGISTRATION, 1.0);
314 }
315
CvCapture_OpenNI2(const char * filename)316 CvCapture_OpenNI2::CvCapture_OpenNI2(const char * filename)
317 {
318 numStream = 2;
319 openni::Status status;
320
321 isContextOpened = false;
322 maxBufferSize = DEFAULT_MAX_BUFFER_SIZE;
323 isCircleBuffer = DEFAULT_IS_CIRCLE_BUFFER;
324 maxTimeDuration = DEFAULT_MAX_TIME_DURATION;
325
326 // Initialize and configure the context.
327 status = openni::OpenNI::initialize();
328
329 if (status != openni::STATUS_OK)
330 {
331 CV_Error(CV_StsError, cv::format("Failed to initialize:", openni::OpenNI::getExtendedError()));
332 return;
333 }
334
335 // Open file
336 status = device.open(filename);
337 if( status != openni::STATUS_OK )
338 {
339 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Failed to open input file (%s): %s\n", filename, openni::OpenNI::getExtendedError()));
340 return;
341 }
342
343 if( !readCamerasParams() )
344 {
345 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::CvCapture_OpenNI2 : Could not read cameras parameters\n"));
346 return;
347 }
348
349 outputMaps.resize( outputMapsTypesCount );
350
351 isContextOpened = true;
352 }
353
~CvCapture_OpenNI2()354 CvCapture_OpenNI2::~CvCapture_OpenNI2()
355 {
356 this->depthFrame.release();
357 this->colorFrame.release();
358 this->depth.stop();
359 this->color.stop();
360 openni::OpenNI::shutdown();
361 }
362
readCamerasParams()363 bool CvCapture_OpenNI2::readCamerasParams()
364 {
365 double pixelSize = 0;
366 if (depth.getProperty<double>(XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE, &pixelSize) != openni::STATUS_OK)
367 {
368 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::readCamerasParams : Could not read pixel size!\n"));
369 return false;
370 }
371
372 // pixel size @ VGA = pixel size @ SXGA x 2
373 pixelSize *= 2.0; // in mm
374
375 // focal length of IR camera in pixels for VGA resolution
376 int zeroPlanDistance; // in mm
377 if (depth.getProperty(XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE, &zeroPlanDistance) != openni::STATUS_OK)
378 {
379 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::readCamerasParams : Could not read virtual plane distance!\n"));
380 return false;
381 }
382
383 if (depth.getProperty<double>(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline) != openni::STATUS_OK)
384 {
385 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::readCamerasParams : Could not read base line!\n"));
386 return false;
387 }
388
389 // baseline from cm -> mm
390 baseline *= 10;
391
392 // focal length from mm -> pixels (valid for 640x480)
393 depthFocalLength_VGA = (int)((double)zeroPlanDistance / (double)pixelSize);
394
395 return true;
396 }
397
getProperty(int propIdx) const398 double CvCapture_OpenNI2::getProperty( int propIdx ) const
399 {
400 double propValue = 0;
401
402 if( isOpened() )
403 {
404 int purePropIdx = propIdx & ~CV_CAP_OPENNI_GENERATORS_MASK;
405
406 if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_IMAGE_GENERATOR )
407 {
408 propValue = getImageGeneratorProperty( purePropIdx );
409 }
410 else if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_DEPTH_GENERATOR )
411 {
412 propValue = getDepthGeneratorProperty( purePropIdx );
413 }
414 else
415 {
416 propValue = getCommonProperty( purePropIdx );
417 }
418 }
419
420 return propValue;
421 }
422
setProperty(int propIdx,double propValue)423 bool CvCapture_OpenNI2::setProperty( int propIdx, double propValue )
424 {
425 bool isSet = false;
426 if( isOpened() )
427 {
428 int purePropIdx = propIdx & ~CV_CAP_OPENNI_GENERATORS_MASK;
429
430 if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_IMAGE_GENERATOR )
431 {
432 isSet = setImageGeneratorProperty( purePropIdx, propValue );
433 }
434 else if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_DEPTH_GENERATOR )
435 {
436 isSet = setDepthGeneratorProperty( purePropIdx, propValue );
437 }
438 else
439 {
440 isSet = setCommonProperty( purePropIdx, propValue );
441 }
442 }
443
444 return isSet;
445 }
446
getCommonProperty(int propIdx) const447 double CvCapture_OpenNI2::getCommonProperty( int propIdx ) const
448 {
449 double propValue = 0;
450
451 switch( propIdx )
452 {
453 // There is a set of properties that correspond to depth generator by default
454 // (is they are pass without particular generator flag). Two reasons of this:
455 // 1) We can assume that depth generator is the main one for depth sensor.
456 // 2) In the initial vertions of OpenNI integration to OpenCV the value of
457 // flag CV_CAP_OPENNI_DEPTH_GENERATOR was 0 (it isn't zero now).
458 case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
459 case CV_CAP_PROP_FRAME_WIDTH :
460 case CV_CAP_PROP_FRAME_HEIGHT :
461 case CV_CAP_PROP_FPS :
462 case CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH :
463 case CV_CAP_PROP_OPENNI_BASELINE :
464 case CV_CAP_PROP_OPENNI_FOCAL_LENGTH :
465 case CV_CAP_PROP_OPENNI_REGISTRATION :
466 propValue = getDepthGeneratorProperty( propIdx );
467 break;
468 case CV_CAP_PROP_OPENNI2_SYNC :
469 propValue = const_cast<CvCapture_OpenNI2 *>(this)->device.getDepthColorSyncEnabled();
470 case CV_CAP_PROP_OPENNI2_MIRROR:
471 {
472 bool isMirroring = color.getMirroringEnabled() && depth.getMirroringEnabled();
473 propValue = isMirroring ? 1.0 : 0.0;
474 break;
475 }
476 default :
477 CV_Error( CV_StsBadArg, cv::format("Such parameter (propIdx=%d) isn't supported for getting.\n", propIdx) );
478 }
479
480 return propValue;
481 }
482
setCommonProperty(int propIdx,double propValue)483 bool CvCapture_OpenNI2::setCommonProperty( int propIdx, double propValue )
484 {
485 bool isSet = false;
486
487 switch( propIdx )
488 {
489 case CV_CAP_PROP_OPENNI2_MIRROR:
490 {
491 bool mirror = propValue > 0.0 ? true : false;
492 isSet = color.setMirroringEnabled(mirror) == openni::STATUS_OK;
493 isSet = depth.setMirroringEnabled(mirror) == openni::STATUS_OK;
494 }
495 break;
496 // There is a set of properties that correspond to depth generator by default
497 // (is they are pass without particular generator flag).
498 case CV_CAP_PROP_OPENNI_REGISTRATION:
499 isSet = setDepthGeneratorProperty( propIdx, propValue );
500 break;
501 case CV_CAP_PROP_OPENNI2_SYNC:
502 isSet = device.setDepthColorSyncEnabled(propValue > 0.0) == openni::STATUS_OK;
503 break;
504 default:
505 CV_Error( CV_StsBadArg, cv::format("Such parameter (propIdx=%d) isn't supported for setting.\n", propIdx) );
506 }
507
508 return isSet;
509 }
510
getDepthGeneratorProperty(int propIdx) const511 double CvCapture_OpenNI2::getDepthGeneratorProperty( int propIdx ) const
512 {
513 double propValue = 0;
514 if( !depth.isValid() )
515 return propValue;
516
517 openni::VideoMode mode;
518
519 switch( propIdx )
520 {
521 case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
522 CV_DbgAssert(depth.isValid());
523 propValue = 1.;
524 break;
525 case CV_CAP_PROP_FRAME_WIDTH :
526 propValue = depth.getVideoMode().getResolutionX();
527 break;
528 case CV_CAP_PROP_FRAME_HEIGHT :
529 propValue = depth.getVideoMode().getResolutionY();
530 break;
531 case CV_CAP_PROP_FPS :
532 mode = depth.getVideoMode();
533 propValue = mode.getFps();
534 break;
535 case CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH :
536 propValue = depth.getMaxPixelValue();
537 break;
538 case CV_CAP_PROP_OPENNI_BASELINE :
539 propValue = baseline;
540 break;
541 case CV_CAP_PROP_OPENNI_FOCAL_LENGTH :
542 propValue = (double)depthFocalLength_VGA;
543 break;
544 case CV_CAP_PROP_OPENNI_REGISTRATION :
545 propValue = device.getImageRegistrationMode();
546 break;
547 case CV_CAP_PROP_POS_MSEC :
548 propValue = (double)depthFrame.getTimestamp();
549 break;
550 case CV_CAP_PROP_POS_FRAMES :
551 propValue = depthFrame.getFrameIndex();
552 break;
553 default :
554 CV_Error( CV_StsBadArg, cv::format("Depth generator does not support such parameter (propIdx=%d) for getting.\n", propIdx) );
555 }
556
557 return propValue;
558 }
559
setDepthGeneratorProperty(int propIdx,double propValue)560 bool CvCapture_OpenNI2::setDepthGeneratorProperty( int propIdx, double propValue )
561 {
562 bool isSet = false;
563
564 CV_Assert( depth.isValid() );
565
566 switch( propIdx )
567 {
568 case CV_CAP_PROP_OPENNI_REGISTRATION:
569 {
570 if( propValue < 1.0 ) // "on"
571 {
572 // if there isn't image generator (i.e. ASUS XtionPro doesn't have it)
573 // then the property isn't avaliable
574 if ( color.isValid() )
575 {
576 openni::ImageRegistrationMode mode = propValue < 1.0 ? openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR : openni::IMAGE_REGISTRATION_OFF;
577 if( !device.getImageRegistrationMode() == mode )
578 {
579 if (device.isImageRegistrationModeSupported(mode))
580 {
581 openni::Status status = device.setImageRegistrationMode(mode);
582 if( status != openni::STATUS_OK )
583 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::setDepthGeneratorProperty : %s\n", openni::OpenNI::getExtendedError()));
584 else
585 isSet = true;
586 }
587 else
588 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::setDepthGeneratorProperty : Unsupported viewpoint.\n"));
589 }
590 else
591 isSet = true;
592 }
593 }
594 else // "off"
595 {
596 openni::Status status = device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF);
597 if( status != openni::STATUS_OK )
598 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::setDepthGeneratorProperty : %s\n", openni::OpenNI::getExtendedError()));
599 else
600 isSet = true;
601 }
602 }
603 break;
604 default:
605 CV_Error( CV_StsBadArg, cv::format("Depth generator does not support such parameter (propIdx=%d) for setting.\n", propIdx) );
606 }
607
608 return isSet;
609 }
610
getImageGeneratorProperty(int propIdx) const611 double CvCapture_OpenNI2::getImageGeneratorProperty( int propIdx ) const
612 {
613 double propValue = 0.;
614 if( !color.isValid() )
615 return propValue;
616
617 openni::VideoMode mode;
618 switch( propIdx )
619 {
620 case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
621 CV_DbgAssert( color.isValid() );
622 propValue = 1.;
623 break;
624 case CV_CAP_PROP_FRAME_WIDTH :
625 propValue = color.getVideoMode().getResolutionX();
626 break;
627 case CV_CAP_PROP_FRAME_HEIGHT :
628 propValue = color.getVideoMode().getResolutionY();
629 break;
630 case CV_CAP_PROP_FPS :
631 propValue = color.getVideoMode().getFps();
632 break;
633 case CV_CAP_PROP_POS_MSEC :
634 propValue = (double)colorFrame.getTimestamp();
635 break;
636 case CV_CAP_PROP_POS_FRAMES :
637 propValue = (double)colorFrame.getFrameIndex();
638 break;
639 default :
640 CV_Error( CV_StsBadArg, cv::format("Image generator does not support such parameter (propIdx=%d) for getting.\n", propIdx) );
641 }
642
643 return propValue;
644 }
645
setImageGeneratorProperty(int propIdx,double propValue)646 bool CvCapture_OpenNI2::setImageGeneratorProperty(int propIdx, double propValue)
647 {
648 bool isSet = false;
649 if( !color.isValid() )
650 return isSet;
651
652 switch( propIdx )
653 {
654 case CV_CAP_PROP_OPENNI_OUTPUT_MODE :
655 {
656 openni::VideoMode mode = color.getVideoMode();
657
658 switch( cvRound(propValue) )
659 {
660 case CV_CAP_OPENNI_VGA_30HZ :
661 mode.setResolution(640,480);
662 mode.setFps(30);
663 break;
664 case CV_CAP_OPENNI_SXGA_15HZ :
665 mode.setResolution(1280, 960);
666 mode.setFps(15);
667 break;
668 case CV_CAP_OPENNI_SXGA_30HZ :
669 mode.setResolution(1280, 960);
670 mode.setFps(30);
671 break;
672 case CV_CAP_OPENNI_QVGA_30HZ :
673 mode.setResolution(320, 240);
674 mode.setFps(30);
675 break;
676 case CV_CAP_OPENNI_QVGA_60HZ :
677 mode.setResolution(320, 240);
678 mode.setFps(60);
679 break;
680 default :
681 CV_Error( CV_StsBadArg, "Unsupported image generator output mode.\n");
682 }
683
684 openni::Status status = color.setVideoMode( mode );
685 if( status != openni::STATUS_OK )
686 CV_Error(CV_StsError, cv::format("CvCapture_OpenNI2::setImageGeneratorProperty : %s\n", openni::OpenNI::getExtendedError()));
687 else
688 isSet = true;
689 break;
690 }
691 default:
692 CV_Error( CV_StsBadArg, cv::format("Image generator does not support such parameter (propIdx=%d) for setting.\n", propIdx) );
693 }
694
695 return isSet;
696 }
697
grabFrame()698 bool CvCapture_OpenNI2::grabFrame()
699 {
700 if( !isOpened() )
701 return false;
702
703 bool isGrabbed = false;
704
705 openni::Status status = openni::OpenNI::waitForAnyStream(streams, numStream, ¤tStream, CV_STREAM_TIMEOUT);
706 if( status != openni::STATUS_OK )
707 return false;
708
709 if( depth.isValid() )
710 depth.readFrame(&depthFrame);
711 if (color.isValid())
712 color.readFrame(&colorFrame);
713 isGrabbed = true;
714
715 return isGrabbed;
716 }
717
getDepthMapFromMetaData(const openni::VideoFrameRef & depthMetaData,cv::Mat & depthMap,int noSampleValue,int shadowValue)718 inline void getDepthMapFromMetaData(const openni::VideoFrameRef& depthMetaData, cv::Mat& depthMap, int noSampleValue, int shadowValue)
719 {
720 depthMap.create(depthMetaData.getHeight(), depthMetaData.getWidth(), CV_16UC1);
721 depthMap.data = (uchar*)depthMetaData.getData();
722
723 cv::Mat badMask = (depthMap == (double)noSampleValue) | (depthMap == (double)shadowValue) | (depthMap == 0);
724
725 // mask the pixels with invalid depth
726 depthMap.setTo( cv::Scalar::all( CvCapture_OpenNI2::INVALID_PIXEL_VAL ), badMask );
727 }
728
retrieveDepthMap()729 IplImage* CvCapture_OpenNI2::retrieveDepthMap()
730 {
731 if( !depth.isValid() )
732 return 0;
733
734 getDepthMapFromMetaData( depthFrame, outputMaps[CV_CAP_OPENNI_DEPTH_MAP].mat, noSampleValue, shadowValue );
735
736 return outputMaps[CV_CAP_OPENNI_DEPTH_MAP].getIplImagePtr();
737 }
738
retrievePointCloudMap()739 IplImage* CvCapture_OpenNI2::retrievePointCloudMap()
740 {
741 if( !depthFrame.isValid() )
742 return 0;
743
744 cv::Mat depthImg;
745 getDepthMapFromMetaData(depthFrame, depthImg, noSampleValue, shadowValue);
746
747 const int badPoint = INVALID_PIXEL_VAL;
748 const float badCoord = INVALID_COORDINATE_VAL;
749 int cols = depthFrame.getWidth(), rows = depthFrame.getHeight();
750 cv::Mat pointCloud_XYZ( rows, cols, CV_32FC3, cv::Scalar::all(badPoint) );
751
752 float worldX, worldY, worldZ;
753 for( int y = 0; y < rows; y++ )
754 {
755 for (int x = 0; x < cols; x++)
756 {
757 openni::CoordinateConverter::convertDepthToWorld(depth, x, y, depthImg.at<unsigned short>(y, x), &worldX, &worldY, &worldZ);
758
759 if (depthImg.at<unsigned short>(y, x) == badPoint) // not valid
760 pointCloud_XYZ.at<cv::Point3f>(y, x) = cv::Point3f(badCoord, badCoord, badCoord);
761 else
762 {
763 pointCloud_XYZ.at<cv::Point3f>(y, x) = cv::Point3f(worldX*0.001f, worldY*0.001f, worldZ*0.001f); // from mm to meters
764 }
765 }
766 }
767
768 outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].mat = pointCloud_XYZ;
769
770 return outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].getIplImagePtr();
771 }
772
computeDisparity_32F(const openni::VideoFrameRef & depthMetaData,cv::Mat & disp,double baseline,int F,int noSampleValue,int shadowValue)773 static void computeDisparity_32F( const openni::VideoFrameRef& depthMetaData, cv::Mat& disp, double baseline, int F, int noSampleValue, int shadowValue)
774 {
775 cv::Mat depth;
776 getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
777 CV_Assert( depth.type() == CV_16UC1 );
778
779 // disparity = baseline * F / z;
780
781 float mult = (float)(baseline /*mm*/ * F /*pixels*/);
782
783 disp.create( depth.size(), CV_32FC1);
784 disp = cv::Scalar::all( CvCapture_OpenNI2::INVALID_PIXEL_VAL );
785 for( int y = 0; y < disp.rows; y++ )
786 {
787 for( int x = 0; x < disp.cols; x++ )
788 {
789 unsigned short curDepth = depth.at<unsigned short>(y,x);
790 if( curDepth != CvCapture_OpenNI2::INVALID_PIXEL_VAL )
791 disp.at<float>(y,x) = mult / curDepth;
792 }
793 }
794 }
795
retrieveDisparityMap()796 IplImage* CvCapture_OpenNI2::retrieveDisparityMap()
797 {
798 if (!depthFrame.isValid())
799 return 0;
800
801 cv::Mat disp32;
802 computeDisparity_32F(depthFrame, disp32, baseline, depthFocalLength_VGA, noSampleValue, shadowValue);
803
804 disp32.convertTo( outputMaps[CV_CAP_OPENNI_DISPARITY_MAP].mat, CV_8UC1 );
805
806 return outputMaps[CV_CAP_OPENNI_DISPARITY_MAP].getIplImagePtr();
807 }
808
retrieveDisparityMap_32F()809 IplImage* CvCapture_OpenNI2::retrieveDisparityMap_32F()
810 {
811 if (!depthFrame.isValid())
812 return 0;
813
814 computeDisparity_32F(depthFrame, outputMaps[CV_CAP_OPENNI_DISPARITY_MAP_32F].mat, baseline, depthFocalLength_VGA, noSampleValue, shadowValue);
815
816 return outputMaps[CV_CAP_OPENNI_DISPARITY_MAP_32F].getIplImagePtr();
817 }
818
retrieveValidDepthMask()819 IplImage* CvCapture_OpenNI2::retrieveValidDepthMask()
820 {
821 if (!depthFrame.isValid())
822 return 0;
823
824 cv::Mat d;
825 getDepthMapFromMetaData(depthFrame, d, noSampleValue, shadowValue);
826
827 outputMaps[CV_CAP_OPENNI_VALID_DEPTH_MASK].mat = d != CvCapture_OpenNI2::INVALID_PIXEL_VAL;
828
829 return outputMaps[CV_CAP_OPENNI_VALID_DEPTH_MASK].getIplImagePtr();
830 }
831
getBGRImageFromMetaData(const openni::VideoFrameRef & imageMetaData,cv::Mat & bgrImage)832 inline void getBGRImageFromMetaData( const openni::VideoFrameRef& imageMetaData, cv::Mat& bgrImage )
833 {
834 cv::Mat bufferImage;
835 if( imageMetaData.getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888 )
836 CV_Error( CV_StsUnsupportedFormat, "Unsupported format of grabbed image\n" );
837
838 bgrImage.create(imageMetaData.getHeight(), imageMetaData.getWidth(), CV_8UC3);
839 bufferImage.create(imageMetaData.getHeight(), imageMetaData.getWidth(), CV_8UC3);
840 bufferImage.data = (uchar*)imageMetaData.getData();
841
842 cv::cvtColor(bufferImage, bgrImage, cv::COLOR_RGB2BGR);
843 }
844
retrieveBGRImage()845 IplImage* CvCapture_OpenNI2::retrieveBGRImage()
846 {
847 if( !color.isValid() )
848 return 0;
849
850 getBGRImageFromMetaData( colorFrame, outputMaps[CV_CAP_OPENNI_BGR_IMAGE].mat );
851
852 return outputMaps[CV_CAP_OPENNI_BGR_IMAGE].getIplImagePtr();
853 }
854
retrieveGrayImage()855 IplImage* CvCapture_OpenNI2::retrieveGrayImage()
856 {
857 if (!colorFrame.isValid())
858 return 0;
859
860 CV_Assert(colorFrame.getVideoMode().getPixelFormat() == openni::PIXEL_FORMAT_RGB888); // RGB
861
862 cv::Mat rgbImage;
863 getBGRImageFromMetaData(colorFrame, rgbImage);
864 cv::cvtColor( rgbImage, outputMaps[CV_CAP_OPENNI_GRAY_IMAGE].mat, CV_BGR2GRAY );
865
866 return outputMaps[CV_CAP_OPENNI_GRAY_IMAGE].getIplImagePtr();
867 }
868
retrieveFrame(int outputType)869 IplImage* CvCapture_OpenNI2::retrieveFrame( int outputType )
870 {
871 IplImage* image = 0;
872 CV_Assert( outputType < outputMapsTypesCount && outputType >= 0);
873
874 if( outputType == CV_CAP_OPENNI_DEPTH_MAP )
875 {
876 image = retrieveDepthMap();
877 }
878 else if( outputType == CV_CAP_OPENNI_POINT_CLOUD_MAP )
879 {
880 image = retrievePointCloudMap();
881 }
882 else if( outputType == CV_CAP_OPENNI_DISPARITY_MAP )
883 {
884 image = retrieveDisparityMap();
885 }
886 else if( outputType == CV_CAP_OPENNI_DISPARITY_MAP_32F )
887 {
888 image = retrieveDisparityMap_32F();
889 }
890 else if( outputType == CV_CAP_OPENNI_VALID_DEPTH_MASK )
891 {
892 image = retrieveValidDepthMask();
893 }
894 else if( outputType == CV_CAP_OPENNI_BGR_IMAGE )
895 {
896 image = retrieveBGRImage();
897 }
898 else if( outputType == CV_CAP_OPENNI_GRAY_IMAGE )
899 {
900 image = retrieveGrayImage();
901 }
902
903 return image;
904 }
905
cvCreateCameraCapture_OpenNI(int index)906 CvCapture* cvCreateCameraCapture_OpenNI( int index )
907 {
908 CvCapture_OpenNI2* capture = new CvCapture_OpenNI2( index );
909
910 if( capture->isOpened() )
911 return capture;
912
913 delete capture;
914 return 0;
915 }
916
cvCreateFileCapture_OpenNI(const char * filename)917 CvCapture* cvCreateFileCapture_OpenNI( const char* filename )
918 {
919 CvCapture_OpenNI2* capture = new CvCapture_OpenNI2( filename );
920
921 if( capture->isOpened() )
922 return capture;
923
924 delete capture;
925 return 0;
926 }
927
928 #endif
929