@ -65,8 +65,10 @@
# define CV_STREAM_TIMEOUT 2000
# define CV_DEPTH_STREAM 0
# define CV_COLOR_STREAM 1
# define CV_DEPTH_STREAM 0
# define CV_COLOR_STREAM 1
# define CV_IR_STREAM 2
# define CV_MAX_NUM_STREAMS 3
# include "OpenNI.h"
# include "PS1080.h"
@ -109,10 +111,9 @@ protected:
IplImage iplHeader ;
} ;
static const int outputMapsTypesCount = 7 ;
static const int outputMapsTypesCount = 8 ;
static openni : : VideoMode defaultColorOutputMode ( ) ;
static openni : : VideoMode defaultDepthOutputMode ( ) ;
static openni : : VideoMode defaultStreamOutputMode ( int stream ) ;
IplImage * retrieveDepthMap ( ) ;
IplImage * retrievePointCloudMap ( ) ;
@ -121,13 +122,17 @@ protected:
IplImage * retrieveValidDepthMask ( ) ;
IplImage * retrieveBGRImage ( ) ;
IplImage * retrieveGrayImage ( ) ;
IplImage * retrieveIrImage ( ) ;
openni : : Status toggleStream ( int stream , bool toggle ) ;
bool readCamerasParams ( ) ;
double getDepthGeneratorProperty ( int propIdx ) const ;
bool setDepthGeneratorProperty ( int propIdx , double propVal ) ;
double getImageGeneratorProperty ( int propIdx ) const ;
bool setImageGeneratorProperty ( int propIdx , double propVal ) ;
double getIrGeneratorProperty ( int propIdx ) const ;
bool setIrGeneratorProperty ( int propIdx , double propVal ) ;
double getCommonProperty ( int propIdx ) const ;
bool setCommonProperty ( int propIdx , double propVal ) ;
@ -137,9 +142,9 @@ protected:
openni : : Recorder recorder ;
// Data generators with its metadata
openni : : VideoStream depth , color , * * streams ;
openni : : VideoFrameRef depthFrame , colorFrame ;
cv : : Mat depthImage , colorImage ;
openni : : VideoStream streams [ CV_MAX_NUM_STREAMS ] ;
openni : : VideoFrameRef streamFrames [ CV_MAX_NUM_STREAMS ] ;
cv : : Mat streamImages [ CV_MAX_NUM_STREAMS ] ;
int maxBufferSize , maxTimeDuration ; // for approx sync
bool isCircleBuffer ;
@ -157,9 +162,6 @@ protected:
// The value for pixels without a valid disparity measurement
int noSampleValue ;
int currentStream ;
int numStream ;
std : : vector < OutputMap > outputMaps ;
} ;
@ -177,27 +179,28 @@ bool CvCapture_OpenNI2::isOpened() const
return isContextOpened ;
}
openni : : VideoMode CvCapture_OpenNI2 : : defaultColorOutputMode ( )
{
openni : : VideoMode mode ;
mode . setResolution ( 640 , 480 ) ;
mode . setFps ( 30 ) ;
mode . setPixelFormat ( openni : : PIXEL_FORMAT_RGB888 ) ;
return mode ;
}
openni : : VideoMode CvCapture_OpenNI2 : : defaultDepthOutputMode ( )
openni : : VideoMode CvCapture_OpenNI2 : : defaultStreamOutputMode ( int stream )
{
openni : : VideoMode mode ;
mode . setResolution ( 640 , 480 ) ;
mode . setFps ( 30 ) ;
mode . setPixelFormat ( openni : : PIXEL_FORMAT_DEPTH_1_MM ) ;
switch ( stream )
{
case CV_DEPTH_STREAM :
mode . setPixelFormat ( openni : : PIXEL_FORMAT_DEPTH_1_MM ) ;
break ;
case CV_COLOR_STREAM :
mode . setPixelFormat ( openni : : PIXEL_FORMAT_RGB888 ) ;
break ;
case CV_IR_STREAM :
mode . setPixelFormat ( openni : : PIXEL_FORMAT_GRAY16 ) ;
break ;
}
return mode ;
}
CvCapture_OpenNI2 : : CvCapture_OpenNI2 ( int index )
{
numStream = 2 ;
const char * deviceURI = openni : : ANY_DEVICE ;
openni : : Status status ;
int deviceType = DEVICE_DEFAULT ;
@ -215,13 +218,6 @@ CvCapture_OpenNI2::CvCapture_OpenNI2( int index )
index % = 10 ;
}
// Asus XTION and Occipital Structure Sensor do not have an image generator
if ( deviceType = = DEVICE_ASUS_XTION )
numStream = 1 ;
if ( deviceType > DEVICE_MAX )
return ;
// Initialize and configure the context.
status = openni : : OpenNI : : initialize ( ) ;
@ -247,83 +243,98 @@ CvCapture_OpenNI2::CvCapture_OpenNI2( int index )
return ;
}
//device.setDepthColorSyncEnabled(true);
status = depth . create ( device , openni : : SENSOR_DEPTH ) ;
if ( status = = openni : : STATUS_OK )
status = toggleStream ( CV_DEPTH_STREAM , true ) ;
// Asus XTION and Occipital Structure Sensor do not have an image generator
if ( deviceType ! = DEVICE_ASUS_XTION )
status = openni : : Status ( status | toggleStream ( CV_COLOR_STREAM , true ) ) ;
if ( status ! = openni : : STATUS_OK )
{
if ( depth . isValid ( ) )
{
CV_Assert ( depth . setVideoMode ( defaultDepthOutputMode ( ) ) = = openni : : STATUS_OK ) ; // xn::DepthGenerator supports VGA only! (Jan 2011)
}
status = depth . start ( ) ;
if ( status ! = openni : : STATUS_OK )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't start depth stream: %s \n " , openni : : OpenNI : : getExtendedError ( ) ) ) ;
depth . destroy ( ) ;
return ;
}
openni : : OpenNI : : shutdown ( ) ;
return ;
}
else
if ( ! readCamerasParams ( ) )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't find depth stream:: %s \n " , openni : : OpenNI : : getExtendedError ( ) ) ) ;
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Could not read cameras parameters \n " ) ) ;
return ;
}
streams = new openni : : VideoStream * [ numStream ] ;
streams [ CV_DEPTH_STREAM ] = & depth ;
// create a color object
status = color . create ( device , openni : : SENSOR_COLOR ) ;
if ( status = = openni : : STATUS_OK )
outputMaps . resize ( outputMapsTypesCount ) ;
isContextOpened = true ;
setProperty ( CV_CAP_PROP_OPENNI_REGISTRATION , 1.0 ) ;
}
openni : : Status CvCapture_OpenNI2 : : toggleStream ( int stream , bool toggle )
{
openni : : Status status ;
// for logging
static const char * stream_names [ CV_MAX_NUM_STREAMS ] = {
" depth " ,
" color " ,
" IR "
} ;
static const openni : : SensorType stream_sensor_types [ CV_MAX_NUM_STREAMS ] = {
openni : : SENSOR_DEPTH ,
openni : : SENSOR_COLOR ,
openni : : SENSOR_IR
} ;
if ( toggle ) // want to open stream
{
// Set map output mode.
if ( color . isValid ( ) )
// already opened
if ( streams [ stream ] . isValid ( ) )
return openni : : STATUS_OK ;
// open stream
status = streams [ stream ] . create ( device , stream_sensor_types [ stream ] ) ;
if ( status = = openni : : STATUS_OK )
{
CV_Assert ( color . setVideoMode ( defaultColorOutputMode ( ) ) = = openni : : STATUS_OK ) ;
// set video mode
status = streams [ stream ] . setVideoMode ( defaultStreamOutputMode ( stream ) ) ; // xn::DepthGenerator supports VGA only! (Jan 2011)
if ( status ! = openni : : STATUS_OK )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't set %s stream output mode: %s \n " ,
stream_names [ stream ] ,
openni : : OpenNI : : getExtendedError ( ) ) ) ;
streams [ stream ] . destroy ( ) ;
return status ;
}
// start stream
status = streams [ stream ] . start ( ) ;
if ( status ! = openni : : STATUS_OK )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't start %s stream: %s \n " ,
stream_names [ stream ] ,
openni : : OpenNI : : getExtendedError ( ) ) ) ;
streams [ stream ] . destroy ( ) ;
return status ;
}
}
status = color . start ( ) ;
if ( status ! = openni : : STATUS_OK )
else
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't start color stream: %s \n " , openni : : OpenNI : : getExtendedError ( ) ) ) ;
color . destroy ( ) ;
return ;
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't find %s stream:: %s \n " ,
stream_names [ stream ] ,
openni : : OpenNI : : getExtendedError ( ) ) ) ;
return status ;
}
streams [ CV_COLOR_STREAM ] = & color ;
}
else if ( numStream = = 2 )
else if ( streams [ stream ] . isValid ( ) ) // want to close stream
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Couldn't find color stream: %s \n " , openni : : OpenNI : : getExtendedError ( ) ) ) ;
return ;
streams [ stream ] . stop ( ) ;
streams [ stream ] . destroy ( ) ;
}
if ( ! readCamerasParams ( ) )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Could not read cameras parameters \n " ) ) ;
return ;
}
// if( deviceType == DEVICE_ASUS_XTION )
// {
// //ps/asus specific
// imageGenerator.SetIntProperty("InputFormat", 1 /*XN_IO_IMAGE_FORMAT_YUV422*/);
// imageGenerator.SetPixelFormat(XN_PIXEL_FORMAT_RGB24);
// depthGenerator.SetIntProperty("RegistrationType", 1 /*XN_PROCESSING_HARDWARE*/);
// }
outputMaps . resize ( outputMapsTypesCount ) ;
isContextOpened = true ;
setProperty ( CV_CAP_PROP_OPENNI_REGISTRATION , 1.0 ) ;
return openni : : STATUS_OK ;
}
CvCapture_OpenNI2 : : CvCapture_OpenNI2 ( const char * filename )
{
numStream = 2 ;
openni : : Status status ;
isContextOpened = false ;
@ -348,6 +359,13 @@ CvCapture_OpenNI2::CvCapture_OpenNI2(const char * filename)
return ;
}
status = openni : : Status ( toggleStream ( CV_DEPTH_STREAM , true ) | toggleStream ( CV_COLOR_STREAM , true ) ) ;
if ( status ! = openni : : STATUS_OK )
{
openni : : OpenNI : : shutdown ( ) ;
return ;
}
if ( ! readCamerasParams ( ) )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::CvCapture_OpenNI2 : Could not read cameras parameters \n " ) ) ;
@ -361,17 +379,20 @@ CvCapture_OpenNI2::CvCapture_OpenNI2(const char * filename)
CvCapture_OpenNI2 : : ~ CvCapture_OpenNI2 ( )
{
this - > depthFrame . release ( ) ;
this - > colorFrame . release ( ) ;
this - > depth . stop ( ) ;
this - > color . stop ( ) ;
for ( int i = 0 ; i < CV_MAX_NUM_STREAMS ; + + i )
{
streamFrames [ i ] . release ( ) ;
streams [ i ] . stop ( ) ;
streams [ i ] . destroy ( ) ;
}
device . close ( ) ;
openni : : OpenNI : : shutdown ( ) ;
}
bool CvCapture_OpenNI2 : : readCamerasParams ( )
{
double pixelSize = 0 ;
if ( depth . getProperty < double > ( XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE , & pixelSize ) ! = openni : : STATUS_OK )
if ( streams [ CV_DEPTH_STREAM ] . getProperty < double > ( XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE , & pixelSize ) ! = openni : : STATUS_OK )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::readCamerasParams : Could not read pixel size! \n " ) ) ;
return false ;
@ -382,13 +403,13 @@ bool CvCapture_OpenNI2::readCamerasParams()
// focal length of IR camera in pixels for VGA resolution
int zeroPlanDistance ; // in mm
if ( depth . getProperty ( XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE , & zeroPlanDistance ) ! = openni : : STATUS_OK )
if ( streams [ CV_DEPTH_STREAM ] . getProperty ( XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE , & zeroPlanDistance ) ! = openni : : STATUS_OK )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::readCamerasParams : Could not read virtual plane distance! \n " ) ) ;
return false ;
}
if ( depth . getProperty < double > ( XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE , & baseline ) ! = openni : : STATUS_OK )
if ( streams [ CV_DEPTH_STREAM ] . getProperty < double > ( XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE , & baseline ) ! = openni : : STATUS_OK )
{
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::readCamerasParams : Could not read base line! \n " ) ) ;
return false ;
@ -419,6 +440,10 @@ double CvCapture_OpenNI2::getProperty( int propIdx ) const
{
propValue = getDepthGeneratorProperty ( purePropIdx ) ;
}
else if ( ( propIdx & CV_CAP_OPENNI_GENERATORS_MASK ) = = CV_CAP_OPENNI_IR_GENERATOR )
{
propValue = getIrGeneratorProperty ( purePropIdx ) ;
}
else
{
propValue = getCommonProperty ( purePropIdx ) ;
@ -443,6 +468,10 @@ bool CvCapture_OpenNI2::setProperty( int propIdx, double propValue )
{
isSet = setDepthGeneratorProperty ( purePropIdx , propValue ) ;
}
else if ( ( propIdx & CV_CAP_OPENNI_GENERATORS_MASK ) = = CV_CAP_OPENNI_IR_GENERATOR )
{
isSet = setIrGeneratorProperty ( purePropIdx , propValue ) ;
}
else
{
isSet = setCommonProperty ( purePropIdx , propValue ) ;
@ -458,12 +487,6 @@ double CvCapture_OpenNI2::getCommonProperty( int propIdx ) const
switch ( propIdx )
{
// There is a set of properties that correspond to depth generator by default
// (is they are pass without particular generator flag). Two reasons of this:
// 1) We can assume that depth generator is the main one for depth sensor.
// 2) In the initial vertions of OpenNI integration to OpenCV the value of
// flag CV_CAP_OPENNI_DEPTH_GENERATOR was 0 (it isn't zero now).
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
case CV_CAP_PROP_FRAME_WIDTH :
case CV_CAP_PROP_FRAME_HEIGHT :
case CV_CAP_PROP_FPS :
@ -477,7 +500,9 @@ double CvCapture_OpenNI2::getCommonProperty( int propIdx ) const
propValue = const_cast < CvCapture_OpenNI2 * > ( this ) - > device . getDepthColorSyncEnabled ( ) ;
case CV_CAP_PROP_OPENNI2_MIRROR :
{
bool isMirroring = color . getMirroringEnabled ( ) & & depth . getMirroringEnabled ( ) ;
bool isMirroring = false ;
for ( int i = 0 ; i < CV_MAX_NUM_STREAMS ; + + i )
isMirroring | = streams [ i ] . getMirroringEnabled ( ) ;
propValue = isMirroring ? 1.0 : 0.0 ;
break ;
}
@ -497,8 +522,11 @@ bool CvCapture_OpenNI2::setCommonProperty( int propIdx, double propValue )
case CV_CAP_PROP_OPENNI2_MIRROR :
{
bool mirror = propValue > 0.0 ? true : false ;
isSet = color . setMirroringEnabled ( mirror ) = = openni : : STATUS_OK ;
isSet = depth . setMirroringEnabled ( mirror ) = = openni : : STATUS_OK ;
for ( int i = 0 ; i < CV_MAX_NUM_STREAMS ; + + i )
{
if ( streams [ i ] . isValid ( ) )
isSet | = streams [ i ] . setMirroringEnabled ( mirror ) = = openni : : STATUS_OK ;
}
}
break ;
// There is a set of properties that correspond to depth generator by default
@ -509,6 +537,7 @@ bool CvCapture_OpenNI2::setCommonProperty( int propIdx, double propValue )
case CV_CAP_PROP_OPENNI2_SYNC :
isSet = device . setDepthColorSyncEnabled ( propValue > 0.0 ) = = openni : : STATUS_OK ;
break ;
default :
CV_Error ( CV_StsBadArg , cv : : format ( " Such parameter (propIdx=%d) isn't supported for setting. \n " , propIdx ) ) ;
}
@ -519,29 +548,28 @@ bool CvCapture_OpenNI2::setCommonProperty( int propIdx, double propValue )
double CvCapture_OpenNI2 : : getDepthGeneratorProperty ( int propIdx ) const
{
double propValue = 0 ;
if ( ! depth . isValid ( ) )
if ( ! streams [ CV_DEPTH_STREAM ] . isValid ( ) )
return propValue ;
openni : : VideoMode mode ;
switch ( propIdx )
{
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
CV_DbgAssert ( depth . isValid ( ) ) ;
propValue = 1. ;
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
propValue = streams [ CV_DEPTH_STREAM ] . isValid ( ) ;
break ;
case CV_CAP_PROP_FRAME_WIDTH :
propValue = depth . getVideoMode ( ) . getResolutionX ( ) ;
propValue = streams [ CV_DEPTH_STREAM ] . getVideoMode ( ) . getResolutionX ( ) ;
break ;
case CV_CAP_PROP_FRAME_HEIGHT :
propValue = depth . getVideoMode ( ) . getResolutionY ( ) ;
propValue = streams [ CV_DEPTH_STREAM ] . getVideoMode ( ) . getResolutionY ( ) ;
break ;
case CV_CAP_PROP_FPS :
mode = depth . getVideoMode ( ) ;
mode = streams [ CV_DEPTH_STREAM ] . getVideoMode ( ) ;
propValue = mode . getFps ( ) ;
break ;
case CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH :
propValue = depth . getMaxPixelValue ( ) ;
propValue = streams [ CV_DEPTH_STREAM ] . getMaxPixelValue ( ) ;
break ;
case CV_CAP_PROP_OPENNI_BASELINE :
propValue = baseline ;
@ -553,10 +581,10 @@ double CvCapture_OpenNI2::getDepthGeneratorProperty( int propIdx ) const
propValue = device . getImageRegistrationMode ( ) ;
break ;
case CV_CAP_PROP_POS_MSEC :
propValue = ( double ) depthFrame . getTimestamp ( ) ;
propValue = ( double ) streamFrames [ CV_DEPTH_STREAM ] . getTimestamp ( ) ;
break ;
case CV_CAP_PROP_POS_FRAMES :
propValue = depthFrame . getFrameIndex ( ) ;
propValue = streamFrames [ CV_DEPTH_STREAM ] . getFrameIndex ( ) ;
break ;
default :
CV_Error ( CV_StsBadArg , cv : : format ( " Depth generator does not support such parameter (propIdx=%d) for getting. \n " , propIdx ) ) ;
@ -569,17 +597,20 @@ bool CvCapture_OpenNI2::setDepthGeneratorProperty( int propIdx, double propValue
{
bool isSet = false ;
CV_Assert ( depth . isValid ( ) ) ;
switch ( propIdx )
{
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
if ( isContextOpened )
isSet = toggleStream ( CV_DEPTH_STREAM , propValue > 0.0 ) = = openni : : STATUS_OK ;
break ;
case CV_CAP_PROP_OPENNI_REGISTRATION :
{
CV_Assert ( streams [ CV_DEPTH_STREAM ] . isValid ( ) ) ;
if ( propValue ! = 0.0 ) // "on"
{
// if there isn't image generator (i.e. ASUS XtionPro doesn't have it)
// then the property isn't avaliable
if ( color . isValid ( ) )
if ( streams [ CV_COLOR_STREAM ] . isValid ( ) )
{
openni : : ImageRegistrationMode mode = propValue ! = 0.0 ? openni : : IMAGE_REGISTRATION_DEPTH_TO_COLOR : openni : : IMAGE_REGISTRATION_OFF ;
if ( ! device . getImageRegistrationMode ( ) = = mode )
@ -619,30 +650,29 @@ bool CvCapture_OpenNI2::setDepthGeneratorProperty( int propIdx, double propValue
double CvCapture_OpenNI2 : : getImageGeneratorProperty ( int propIdx ) const
{
double propValue = 0. ;
if ( ! color . isValid ( ) )
if ( ! streams [ CV_COLOR_STREAM ] . isValid ( ) )
return propValue ;
openni : : VideoMode mode ;
switch ( propIdx )
{
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
CV_DbgAssert ( color . isValid ( ) ) ;
propValue = 1. ;
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
propValue = streams [ CV_COLOR_STREAM ] . isValid ( ) ;
break ;
case CV_CAP_PROP_FRAME_WIDTH :
propValue = color . getVideoMode ( ) . getResolutionX ( ) ;
propValue = streams [ CV_COLOR_STREAM ] . getVideoMode ( ) . getResolutionX ( ) ;
break ;
case CV_CAP_PROP_FRAME_HEIGHT :
propValue = color . getVideoMode ( ) . getResolutionY ( ) ;
propValue = streams [ CV_COLOR_STREAM ] . getVideoMode ( ) . getResolutionY ( ) ;
break ;
case CV_CAP_PROP_FPS :
propValue = color . getVideoMode ( ) . getFps ( ) ;
propValue = streams [ CV_COLOR_STREAM ] . getVideoMode ( ) . getFps ( ) ;
break ;
case CV_CAP_PROP_POS_MSEC :
propValue = ( double ) colorFrame . getTimestamp ( ) ;
propValue = ( double ) streamFrames [ CV_COLOR_STREAM ] . getTimestamp ( ) ;
break ;
case CV_CAP_PROP_POS_FRAMES :
propValue = ( double ) colorFrame . getFrameIndex ( ) ;
propValue = ( double ) streamFrames [ CV_COLOR_STREAM ] . getFrameIndex ( ) ;
break ;
default :
CV_Error ( CV_StsBadArg , cv : : format ( " Image generator does not support such parameter (propIdx=%d) for getting. \n " , propIdx ) ) ;
@ -654,14 +684,18 @@ double CvCapture_OpenNI2::getImageGeneratorProperty( int propIdx ) const
bool CvCapture_OpenNI2 : : setImageGeneratorProperty ( int propIdx , double propValue )
{
bool isSet = false ;
if ( ! color . isValid ( ) )
return isSet ;
switch ( propIdx )
{
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
if ( isContextOpened )
isSet = toggleStream ( CV_COLOR_STREAM , propValue > 0.0 ) = = openni : : STATUS_OK ;
break ;
case CV_CAP_PROP_OPENNI_OUTPUT_MODE :
{
openni : : VideoMode mode = color . getVideoMode ( ) ;
if ( ! streams [ CV_COLOR_STREAM ] . isValid ( ) )
return isSet ;
openni : : VideoMode mode = streams [ CV_COLOR_STREAM ] . getVideoMode ( ) ;
switch ( cvRound ( propValue ) )
{
@ -689,7 +723,7 @@ bool CvCapture_OpenNI2::setImageGeneratorProperty(int propIdx, double propValue)
CV_Error ( CV_StsBadArg , " Unsupported image generator output mode. \n " ) ;
}
openni : : Status status = color . setVideoMode ( mode ) ;
openni : : Status status = streams [ CV_COLOR_STREAM ] . setVideoMode ( mode ) ;
if ( status ! = openni : : STATUS_OK )
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::setImageGeneratorProperty : %s \n " , openni : : OpenNI : : getExtendedError ( ) ) ) ;
else
@ -703,6 +737,96 @@ bool CvCapture_OpenNI2::setImageGeneratorProperty(int propIdx, double propValue)
return isSet ;
}
double CvCapture_OpenNI2 : : getIrGeneratorProperty ( int propIdx ) const
{
double propValue = 0. ;
if ( ! streams [ CV_IR_STREAM ] . isValid ( ) )
return propValue ;
openni : : VideoMode mode ;
switch ( propIdx )
{
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
propValue = streams [ CV_IR_STREAM ] . isValid ( ) ;
break ;
case CV_CAP_PROP_FRAME_WIDTH :
propValue = streams [ CV_IR_STREAM ] . getVideoMode ( ) . getResolutionX ( ) ;
break ;
case CV_CAP_PROP_FRAME_HEIGHT :
propValue = streams [ CV_IR_STREAM ] . getVideoMode ( ) . getResolutionY ( ) ;
break ;
case CV_CAP_PROP_FPS :
propValue = streams [ CV_IR_STREAM ] . getVideoMode ( ) . getFps ( ) ;
break ;
case CV_CAP_PROP_POS_MSEC :
propValue = ( double ) streamFrames [ CV_IR_STREAM ] . getTimestamp ( ) ;
break ;
case CV_CAP_PROP_POS_FRAMES :
propValue = ( double ) streamFrames [ CV_IR_STREAM ] . getFrameIndex ( ) ;
break ;
default :
CV_Error ( CV_StsBadArg , cv : : format ( " Image generator does not support such parameter (propIdx=%d) for getting. \n " , propIdx ) ) ;
}
return propValue ;
}
bool CvCapture_OpenNI2 : : setIrGeneratorProperty ( int propIdx , double propValue )
{
bool isSet = false ;
switch ( propIdx )
{
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
if ( isContextOpened )
isSet = toggleStream ( CV_IR_STREAM , propValue > 0.0 ) = = openni : : STATUS_OK ;
break ;
case CV_CAP_PROP_OPENNI_OUTPUT_MODE :
{
if ( ! streams [ CV_IR_STREAM ] . isValid ( ) )
return isSet ;
openni : : VideoMode mode = streams [ CV_IR_STREAM ] . getVideoMode ( ) ;
switch ( cvRound ( propValue ) )
{
case CV_CAP_OPENNI_VGA_30HZ :
mode . setResolution ( 640 , 480 ) ;
mode . setFps ( 30 ) ;
break ;
case CV_CAP_OPENNI_SXGA_15HZ :
mode . setResolution ( 1280 , 960 ) ;
mode . setFps ( 15 ) ;
break ;
case CV_CAP_OPENNI_SXGA_30HZ :
mode . setResolution ( 1280 , 960 ) ;
mode . setFps ( 30 ) ;
break ;
case CV_CAP_OPENNI_QVGA_30HZ :
mode . setResolution ( 320 , 240 ) ;
mode . setFps ( 30 ) ;
break ;
case CV_CAP_OPENNI_QVGA_60HZ :
mode . setResolution ( 320 , 240 ) ;
mode . setFps ( 60 ) ;
break ;
default :
CV_Error ( CV_StsBadArg , " Unsupported image generator output mode. \n " ) ;
}
openni : : Status status = streams [ CV_IR_STREAM ] . setVideoMode ( mode ) ;
if ( status ! = openni : : STATUS_OK )
CV_Error ( CV_StsError , cv : : format ( " CvCapture_OpenNI2::setImageGeneratorProperty : %s \n " , openni : : OpenNI : : getExtendedError ( ) ) ) ;
else
isSet = true ;
break ;
}
default :
CV_Error ( CV_StsBadArg , cv : : format ( " Image generator does not support such parameter (propIdx=%d) for setting. \n " , propIdx ) ) ;
}
return isSet ;
}
bool CvCapture_OpenNI2 : : grabFrame ( )
{
if ( ! isOpened ( ) )
@ -710,14 +834,22 @@ bool CvCapture_OpenNI2::grabFrame()
bool isGrabbed = false ;
openni : : Status status = openni : : OpenNI : : waitForAnyStream ( streams , numStream , & currentStream , CV_STREAM_TIMEOUT ) ;
int numActiveStreams = 0 ;
openni : : VideoStream * streamPtrs [ CV_MAX_NUM_STREAMS ] ;
for ( int i = 0 ; i < CV_MAX_NUM_STREAMS ; + + i ) {
streamPtrs [ numActiveStreams + + ] = & streams [ i ] ;
}
int currentStream ;
openni : : Status status = openni : : OpenNI : : waitForAnyStream ( streamPtrs , numActiveStreams , & currentStream , CV_STREAM_TIMEOUT ) ;
if ( status ! = openni : : STATUS_OK )
return false ;
if ( depth . isValid ( ) )
depth . readFrame ( & depthFrame ) ;
if ( color . isValid ( ) )
color . readFrame ( & colorFrame ) ;
for ( int i = 0 ; i < CV_MAX_NUM_STREAMS ; + + i )
{
if ( streams [ i ] . isValid ( ) )
streams [ i ] . readFrame ( & streamFrames [ i ] ) ;
}
isGrabbed = true ;
return isGrabbed ;
@ -736,25 +868,25 @@ inline void getDepthMapFromMetaData(const openni::VideoFrameRef& depthMetaData,
IplImage * CvCapture_OpenNI2 : : retrieveDepthMap ( )
{
if ( ! depth . isValid ( ) )
if ( ! streamFrames [ CV_DEPTH_STREAM ] . isValid ( ) )
return 0 ;
getDepthMapFromMetaData ( depthFrame , outputMaps [ CV_CAP_OPENNI_DEPTH_MAP ] . mat , noSampleValue , shadowValue ) ;
getDepthMapFromMetaData ( streamFrames [ CV_DEPTH_STREAM ] , outputMaps [ CV_CAP_OPENNI_DEPTH_MAP ] . mat , noSampleValue , shadowValue ) ;
return outputMaps [ CV_CAP_OPENNI_DEPTH_MAP ] . getIplImagePtr ( ) ;
}
IplImage * CvCapture_OpenNI2 : : retrievePointCloudMap ( )
{
if ( ! depthFrame . isValid ( ) )
if ( ! streamFrames [ CV_DEPTH_STREAM ] . isValid ( ) )
return 0 ;
cv : : Mat depthImg ;
getDepthMapFromMetaData ( depthFrame , depthImg , noSampleValue , shadowValue ) ;
getDepthMapFromMetaData ( streamFrames [ CV_DEPTH_STREAM ] , depthImg , noSampleValue , shadowValue ) ;
const int badPoint = INVALID_PIXEL_VAL ;
const float badCoord = INVALID_COORDINATE_VAL ;
int cols = depthFrame . getWidth ( ) , rows = depthFrame . getHeight ( ) ;
int cols = streamFrames [ CV_DEPTH_STREAM ] . getWidth ( ) , rows = streamFrames [ CV_DEPTH_STREAM ] . getHeight ( ) ;
cv : : Mat pointCloud_XYZ ( rows , cols , CV_32FC3 , cv : : Scalar : : all ( badPoint ) ) ;
float worldX , worldY , worldZ ;
@ -762,7 +894,7 @@ IplImage* CvCapture_OpenNI2::retrievePointCloudMap()
{
for ( int x = 0 ; x < cols ; x + + )
{
openni : : CoordinateConverter : : convertDepthToWorld ( depth , x , y , depthImg . at < unsigned short > ( y , x ) , & worldX , & worldY , & worldZ ) ;
openni : : CoordinateConverter : : convertDepthToWorld ( streams [ CV_DEPTH_STREAM ] , x , y , depthImg . at < unsigned short > ( y , x ) , & worldX , & worldY , & worldZ ) ;
if ( depthImg . at < unsigned short > ( y , x ) = = badPoint ) // not valid
pointCloud_XYZ . at < cv : : Point3f > ( y , x ) = cv : : Point3f ( badCoord , badCoord , badCoord ) ;
@ -803,11 +935,11 @@ static void computeDisparity_32F( const openni::VideoFrameRef& depthMetaData, cv
IplImage * CvCapture_OpenNI2 : : retrieveDisparityMap ( )
{
if ( ! depthFrame . isValid ( ) )
if ( ! streamFrames [ CV_DEPTH_STREAM ] . isValid ( ) )
return 0 ;
cv : : Mat disp32 ;
computeDisparity_32F ( depthFrame , disp32 , baseline , depthFocalLength_VGA , noSampleValue , shadowValue ) ;
computeDisparity_32F ( streamFrames [ CV_DEPTH_STREAM ] , disp32 , baseline , depthFocalLength_VGA , noSampleValue , shadowValue ) ;
disp32 . convertTo ( outputMaps [ CV_CAP_OPENNI_DISPARITY_MAP ] . mat , CV_8UC1 ) ;
@ -816,21 +948,21 @@ IplImage* CvCapture_OpenNI2::retrieveDisparityMap()
IplImage * CvCapture_OpenNI2 : : retrieveDisparityMap_32F ( )
{
if ( ! depthFrame . isValid ( ) )
if ( ! streamFrames [ CV_DEPTH_STREAM ] . isValid ( ) )
return 0 ;
computeDisparity_32F ( depthFrame , outputMaps [ CV_CAP_OPENNI_DISPARITY_MAP_32F ] . mat , baseline , depthFocalLength_VGA , noSampleValue , shadowValue ) ;
computeDisparity_32F ( streamFrames [ CV_DEPTH_STREAM ] , outputMaps [ CV_CAP_OPENNI_DISPARITY_MAP_32F ] . mat , baseline , depthFocalLength_VGA , noSampleValue , shadowValue ) ;
return outputMaps [ CV_CAP_OPENNI_DISPARITY_MAP_32F ] . getIplImagePtr ( ) ;
}
IplImage * CvCapture_OpenNI2 : : retrieveValidDepthMask ( )
{
if ( ! depthFrame . isValid ( ) )
if ( ! streamFrames [ CV_DEPTH_STREAM ] . isValid ( ) )
return 0 ;
cv : : Mat d ;
getDepthMapFromMetaData ( depthFrame , d , noSampleValue , shadowValue ) ;
getDepthMapFromMetaData ( streamFrames [ CV_DEPTH_STREAM ] , d , noSampleValue , shadowValue ) ;
outputMaps [ CV_CAP_OPENNI_VALID_DEPTH_MASK ] . mat = d ! = CvCapture_OpenNI2 : : INVALID_PIXEL_VAL ;
@ -850,30 +982,58 @@ inline void getBGRImageFromMetaData( const openni::VideoFrameRef& imageMetaData,
cv : : cvtColor ( bufferImage , bgrImage , cv : : COLOR_RGB2BGR ) ;
}
inline void getGrayImageFromMetaData ( const openni : : VideoFrameRef & imageMetaData , cv : : Mat & grayImage )
{
if ( imageMetaData . getVideoMode ( ) . getPixelFormat ( ) = = openni : : PIXEL_FORMAT_GRAY8 )
{
grayImage . create ( imageMetaData . getHeight ( ) , imageMetaData . getWidth ( ) , CV_8UC1 ) ;
grayImage . data = ( uchar * ) imageMetaData . getData ( ) ;
}
else if ( imageMetaData . getVideoMode ( ) . getPixelFormat ( ) = = openni : : PIXEL_FORMAT_GRAY16 )
{
grayImage . create ( imageMetaData . getHeight ( ) , imageMetaData . getWidth ( ) , CV_16UC1 ) ;
grayImage . data = ( uchar * ) imageMetaData . getData ( ) ;
}
else
{
CV_Error ( CV_StsUnsupportedFormat , " Unsupported format of grabbed image \n " ) ;
}
}
IplImage * CvCapture_OpenNI2 : : retrieveBGRImage ( )
{
if ( ! color . isValid ( ) )
if ( ! streamFrames [ CV_COLOR_STREAM ] . isValid ( ) )
return 0 ;
getBGRImageFromMetaData ( colorFrame , outputMaps [ CV_CAP_OPENNI_BGR_IMAGE ] . mat ) ;
getBGRImageFromMetaData ( streamFrames [ CV_COLOR_STREAM ] , outputMaps [ CV_CAP_OPENNI_BGR_IMAGE ] . mat ) ;
return outputMaps [ CV_CAP_OPENNI_BGR_IMAGE ] . getIplImagePtr ( ) ;
}
IplImage * CvCapture_OpenNI2 : : retrieveGrayImage ( )
{
if ( ! colorFrame . isValid ( ) )
if ( ! streamFrames [ CV_COLOR_STREAM ] . isValid ( ) )
return 0 ;
CV_Assert ( colorFrame . getVideoMode ( ) . getPixelFormat ( ) = = openni : : PIXEL_FORMAT_RGB888 ) ; // RGB
CV_Assert ( streamFrames [ CV_COLOR_STREAM ] . getVideoMode ( ) . getPixelFormat ( ) = = openni : : PIXEL_FORMAT_RGB888 ) ; // RGB
cv : : Mat rgbImage ;
getBGRImageFromMetaData ( colorFrame , rgbImage ) ;
getBGRImageFromMetaData ( streamFrames [ CV_COLOR_STREAM ] , rgbImage ) ;
cv : : cvtColor ( rgbImage , outputMaps [ CV_CAP_OPENNI_GRAY_IMAGE ] . mat , CV_BGR2GRAY ) ;
return outputMaps [ CV_CAP_OPENNI_GRAY_IMAGE ] . getIplImagePtr ( ) ;
}
IplImage * CvCapture_OpenNI2 : : retrieveIrImage ( )
{
if ( ! streamFrames [ CV_IR_STREAM ] . isValid ( ) )
return 0 ;
getGrayImageFromMetaData ( streamFrames [ CV_IR_STREAM ] , outputMaps [ CV_CAP_OPENNI_IR_IMAGE ] . mat ) ;
return outputMaps [ CV_CAP_OPENNI_IR_IMAGE ] . getIplImagePtr ( ) ;
}
IplImage * CvCapture_OpenNI2 : : retrieveFrame ( int outputType )
{
IplImage * image = 0 ;
@ -907,6 +1067,10 @@ IplImage* CvCapture_OpenNI2::retrieveFrame( int outputType )
{
image = retrieveGrayImage ( ) ;
}
else if ( outputType = = CV_CAP_OPENNI_IR_IMAGE )
{
image = retrieveIrImage ( ) ;
}
return image ;
}