modules/highgui/src/cap_openni2.cpp

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