1385 lines
44 KiB
C++
1385 lines
44 KiB
C++
/*M///////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
|
//
|
|
// By downloading, copying, installing or using the software you agree to this license.
|
|
// If you do not agree to this license, do not download, install,
|
|
// copy or use the software.
|
|
//
|
|
//
|
|
// Intel License Agreement
|
|
// For Open Source Computer Vision Library
|
|
//
|
|
// Copyright (C) 2000, Intel Corporation, all rights reserved.
|
|
// Third party copyrights are property of their respective owners.
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without modification,
|
|
// are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistribution's of source code must retain the above copyright notice,
|
|
// this list of conditions and the following disclaimer.
|
|
//
|
|
// * Redistribution's in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
//
|
|
// * The name of Intel Corporation may not be used to endorse or promote products
|
|
// derived from this software without specific prior written permission.
|
|
//
|
|
// This software is provided by the copyright holders and contributors "as is" and
|
|
// any express or implied warranties, including, but not limited to, the implied
|
|
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
|
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
|
// indirect, incidental, special, exemplary, or consequential damages
|
|
// (including, but not limited to, procurement of substitute goods or services;
|
|
// loss of use, data, or profits; or business interruption) however caused
|
|
// and on any theory of liability, whether in contract, strict liability,
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
//
|
|
//M*/
|
|
#include "precomp.hpp"
|
|
#include "opencv2/core/core.hpp"
|
|
#include "opencv2/imgproc/imgproc.hpp"
|
|
|
|
#ifdef HAVE_OPENNI
|
|
|
|
#if defined TBB_INTERFACE_VERSION && TBB_INTERFACE_VERSION < 5000
|
|
# undef HAVE_TBB
|
|
#endif
|
|
|
|
#include <iostream>
|
|
#include <queue>
|
|
|
|
#ifndef i386
|
|
# define i386 0
|
|
#endif
|
|
#ifndef __arm__
|
|
# define __arm__ 0
|
|
#endif
|
|
#ifndef _ARC
|
|
# define _ARC 0
|
|
#endif
|
|
#ifndef __APPLE__
|
|
# define __APPLE__ 0
|
|
#endif
|
|
|
|
#include "XnCppWrapper.h"
|
|
|
|
const std::string XMLConfig =
|
|
"<OpenNI>"
|
|
"<Licenses>"
|
|
"<License vendor=\"PrimeSense\" key=\"0KOIk2JeIBYClPWVnMoRKn5cdY4=\"/>"
|
|
"</Licenses>"
|
|
"<Log writeToConsole=\"false\" writeToFile=\"false\">"
|
|
"<LogLevel value=\"3\"/>"
|
|
"<Masks>"
|
|
"<Mask name=\"ALL\" on=\"true\"/>"
|
|
"</Masks>"
|
|
"<Dumps>"
|
|
"</Dumps>"
|
|
"</Log>"
|
|
"<ProductionNodes>"
|
|
"<Node type=\"Image\" name=\"Image1\" stopOnError=\"false\">"
|
|
"<Configuration>"
|
|
"<MapOutputMode xRes=\"640\" yRes=\"480\" FPS=\"30\"/>"
|
|
"<Mirror on=\"false\"/>"
|
|
"</Configuration>"
|
|
"</Node> "
|
|
"<Node type=\"Depth\" name=\"Depth1\">"
|
|
"<Configuration>"
|
|
"<MapOutputMode xRes=\"640\" yRes=\"480\" FPS=\"30\"/>"
|
|
"<Mirror on=\"false\"/>"
|
|
"</Configuration>"
|
|
"</Node>"
|
|
"</ProductionNodes>"
|
|
"</OpenNI>\n";
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
class ApproximateSyncGrabber
|
|
{
|
|
public:
|
|
ApproximateSyncGrabber( xn::Context &_context,
|
|
xn::DepthGenerator &_depthGenerator,
|
|
xn::ImageGenerator &_imageGenerator,
|
|
int _maxBufferSize, bool _isCircleBuffer, int _maxTimeDuration ) :
|
|
context(_context), depthGenerator(_depthGenerator), imageGenerator(_imageGenerator),
|
|
maxBufferSize(_maxBufferSize), isCircleBuffer(_isCircleBuffer), maxTimeDuration(_maxTimeDuration)
|
|
{
|
|
task = 0;
|
|
|
|
CV_Assert( depthGenerator.IsValid() );
|
|
CV_Assert( imageGenerator.IsValid() );
|
|
}
|
|
|
|
void setMaxBufferSize( int _maxBufferSize )
|
|
{
|
|
maxBufferSize = _maxBufferSize;
|
|
#ifdef HAVE_TBB
|
|
task->setMaxBufferSize();
|
|
#endif
|
|
}
|
|
inline int getMaxBufferSize() const { return maxBufferSize; }
|
|
|
|
void setIsCircleBuffer( bool _isCircleBuffer ) { isCircleBuffer = _isCircleBuffer; }
|
|
bool getIsCircleBuffer() const { return isCircleBuffer; }
|
|
|
|
void setMaxTimeDuration( int _maxTimeDuration ) { maxTimeDuration = _maxTimeDuration; }
|
|
int getMaxTimeDuration() const { return maxTimeDuration; }
|
|
|
|
bool grab( xn::DepthMetaData& depthMetaData,
|
|
xn::ImageMetaData& imageMetaData )
|
|
{
|
|
CV_Assert( task );
|
|
|
|
|
|
while( task->grab(depthMetaData, imageMetaData) == false )
|
|
{
|
|
#ifndef HAVE_TBB
|
|
task->spin();
|
|
#endif
|
|
}
|
|
return true;
|
|
|
|
}
|
|
|
|
void start()
|
|
{
|
|
CV_Assert( depthGenerator.IsValid() );
|
|
CV_Assert( imageGenerator.IsValid() );
|
|
#ifdef HAVE_TBB
|
|
task = new( tbb::task::allocate_root() ) TBBApproximateSynchronizerTask( *this );
|
|
tbb::task::enqueue(*task);
|
|
#else
|
|
task = new ApproximateSynchronizer( *this );
|
|
#endif
|
|
}
|
|
|
|
void finish()
|
|
{
|
|
#ifdef HAVE_TBB
|
|
if( task )
|
|
tbb::task::destroy( *task );
|
|
#else
|
|
task.release();
|
|
#endif
|
|
}
|
|
|
|
bool isRun() const { return task != 0; }
|
|
|
|
xn::Context &context;
|
|
xn::DepthGenerator &depthGenerator;
|
|
xn::ImageGenerator &imageGenerator;
|
|
|
|
private:
|
|
int maxBufferSize;
|
|
bool isCircleBuffer;
|
|
int maxTimeDuration;
|
|
|
|
class ApproximateSynchronizerBase
|
|
{
|
|
public:
|
|
ApproximateSynchronizerBase( ApproximateSyncGrabber& _approxSyncGrabber ) :
|
|
approxSyncGrabber(_approxSyncGrabber), isDepthFilled(false), isImageFilled(false)
|
|
{}
|
|
|
|
virtual ~ApproximateSynchronizerBase() {}
|
|
|
|
virtual bool isSpinContinue() const = 0;
|
|
virtual void pushDepthMetaData( xn::DepthMetaData& depthMetaData ) = 0;
|
|
virtual void pushImageMetaData( xn::ImageMetaData& imageMetaData ) = 0;
|
|
virtual bool popDepthMetaData( xn::DepthMetaData& depthMetaData ) = 0;
|
|
virtual bool popImageMetaData( xn::ImageMetaData& imageMetaData ) = 0;
|
|
|
|
void spin()
|
|
{
|
|
while(isSpinContinue() == true)
|
|
{
|
|
XnStatus status = approxSyncGrabber.context.WaitAnyUpdateAll();
|
|
if( status != XN_STATUS_OK )
|
|
continue;
|
|
|
|
//xn::DepthMetaData depth;
|
|
//xn::ImageMetaData image;
|
|
approxSyncGrabber.depthGenerator.GetMetaData(depth);
|
|
approxSyncGrabber.imageGenerator.GetMetaData(image);
|
|
|
|
if( depth.Data() && depth.IsDataNew() )
|
|
pushDepthMetaData( depth );
|
|
|
|
if( image.Data() && image.IsDataNew() )
|
|
pushImageMetaData( image );
|
|
}
|
|
}
|
|
|
|
virtual bool grab( xn::DepthMetaData& depthMetaData,
|
|
xn::ImageMetaData& imageMetaData )
|
|
{
|
|
while(1)
|
|
{
|
|
if( !isDepthFilled )
|
|
isDepthFilled = popDepthMetaData(depth);
|
|
if( !isImageFilled )
|
|
isImageFilled = popImageMetaData(image);
|
|
|
|
if( !isDepthFilled || !isImageFilled )
|
|
break;
|
|
|
|
double timeDiff = 1e-3 * std::abs(static_cast<double>(depth.Timestamp()) - static_cast<double>(image.Timestamp()));
|
|
|
|
if( timeDiff <= approxSyncGrabber.maxTimeDuration )
|
|
{
|
|
depthMetaData.InitFrom(depth);
|
|
imageMetaData.InitFrom(image);
|
|
isDepthFilled = isImageFilled = false;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
if( depth.Timestamp() < image.Timestamp() )
|
|
isDepthFilled = false;
|
|
else
|
|
isImageFilled = false;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
protected:
|
|
ApproximateSyncGrabber& approxSyncGrabber;
|
|
xn::DepthMetaData depth;
|
|
xn::ImageMetaData image;
|
|
bool isDepthFilled;
|
|
bool isImageFilled;
|
|
};
|
|
|
|
// If there isn't TBB the synchronization will be executed in the main thread.
|
|
class ApproximateSynchronizer: public ApproximateSynchronizerBase
|
|
{
|
|
public:
|
|
ApproximateSynchronizer( ApproximateSyncGrabber& _approxSyncGrabber ) :
|
|
ApproximateSynchronizerBase(_approxSyncGrabber)
|
|
{}
|
|
|
|
virtual bool isSpinContinue() const
|
|
{
|
|
int maxBufferSize = approxSyncGrabber.getMaxBufferSize();
|
|
return (maxBufferSize <= 0) || (static_cast<int>(depthQueue.size()) < maxBufferSize &&
|
|
static_cast<int>(imageQueue.size()) < maxBufferSize); // "<" to may push
|
|
}
|
|
|
|
virtual inline void pushDepthMetaData( xn::DepthMetaData& depthMetaData )
|
|
{
|
|
cv::Ptr<xn::DepthMetaData> depthPtr = new xn::DepthMetaData;
|
|
depthPtr->CopyFrom(depthMetaData);
|
|
depthQueue.push(depthPtr);
|
|
}
|
|
virtual inline void pushImageMetaData( xn::ImageMetaData& imageMetaData )
|
|
{
|
|
cv::Ptr<xn::ImageMetaData> imagePtr = new xn::ImageMetaData;
|
|
imagePtr->CopyFrom(imageMetaData);
|
|
imageQueue.push(imagePtr);
|
|
}
|
|
virtual inline bool popDepthMetaData( xn::DepthMetaData& depthMetaData )
|
|
{
|
|
if( depthQueue.empty() )
|
|
return false;
|
|
|
|
depthMetaData.CopyFrom(*depthQueue.front());
|
|
depthQueue.pop();
|
|
return true;
|
|
}
|
|
virtual inline bool popImageMetaData( xn::ImageMetaData& imageMetaData )
|
|
{
|
|
if( imageQueue.empty() )
|
|
return false;
|
|
|
|
imageMetaData.CopyFrom(*imageQueue.front());
|
|
imageQueue.pop();
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
std::queue<cv::Ptr<xn::DepthMetaData> > depthQueue;
|
|
std::queue<cv::Ptr<xn::ImageMetaData> > imageQueue;
|
|
};
|
|
|
|
#ifdef HAVE_TBB
|
|
// If there is TBB the synchronization will be executed in own thread.
|
|
class TBBApproximateSynchronizer: public ApproximateSynchronizerBase
|
|
{
|
|
public:
|
|
TBBApproximateSynchronizer( ApproximateSyncGrabber& approxSyncGrabber ) :
|
|
ApproximateSynchronizerBase(approxSyncGrabber)
|
|
{
|
|
setMaxBufferSize();
|
|
}
|
|
|
|
void setMaxBufferSize()
|
|
{
|
|
int maxBufferSize = ApproximateSynchronizerBase::approxSyncGrabber.getMaxBufferSize();
|
|
if( maxBufferSize >= 0 )
|
|
{
|
|
depthQueue.set_capacity( maxBufferSize );
|
|
imageQueue.set_capacity( maxBufferSize );
|
|
}
|
|
}
|
|
|
|
virtual inline bool isSpinContinue() const { return true; }
|
|
|
|
virtual inline void pushDepthMetaData( xn::DepthMetaData& depthMetaData )
|
|
{
|
|
cv::Ptr<xn::DepthMetaData> depthPtr = new xn::DepthMetaData, tmp;
|
|
depthPtr->CopyFrom(depthMetaData);
|
|
|
|
tbb::mutex mtx;
|
|
mtx.lock();
|
|
if( depthQueue.try_push(depthPtr) == false )
|
|
{
|
|
if( approxSyncGrabber.getIsCircleBuffer() )
|
|
{
|
|
CV_Assert( depthQueue.try_pop(tmp) );
|
|
CV_Assert( depthQueue.try_push(depthPtr) );
|
|
}
|
|
}
|
|
mtx.unlock();
|
|
}
|
|
|
|
virtual inline void pushImageMetaData( xn::ImageMetaData& imageMetaData )
|
|
{
|
|
cv::Ptr<xn::ImageMetaData> imagePtr = new xn::ImageMetaData, tmp;
|
|
imagePtr->CopyFrom(imageMetaData);
|
|
|
|
tbb::mutex mtx;
|
|
mtx.lock();
|
|
if( imageQueue.try_push(imagePtr) == false )
|
|
{
|
|
if( approxSyncGrabber.getIsCircleBuffer() )
|
|
{
|
|
CV_Assert( imageQueue.try_pop(tmp) );
|
|
CV_Assert( imageQueue.try_push(imagePtr) );
|
|
}
|
|
}
|
|
mtx.unlock();
|
|
}
|
|
|
|
virtual inline bool popDepthMetaData( xn::DepthMetaData& depthMetaData )
|
|
{
|
|
cv::Ptr<xn::DepthMetaData> depthPtr;
|
|
bool isPop = depthQueue.try_pop(depthPtr);
|
|
if( isPop )
|
|
depthMetaData.CopyFrom(*depthPtr);
|
|
return isPop;
|
|
}
|
|
virtual inline bool popImageMetaData( xn::ImageMetaData& imageMetaData )
|
|
{
|
|
cv::Ptr<xn::ImageMetaData> imagePtr;
|
|
bool isPop = imageQueue.try_pop(imagePtr);
|
|
if( isPop )
|
|
imageMetaData.CopyFrom(*imagePtr);
|
|
return isPop;
|
|
}
|
|
|
|
private:
|
|
tbb::concurrent_bounded_queue<cv::Ptr<xn::DepthMetaData> > depthQueue;
|
|
tbb::concurrent_bounded_queue<cv::Ptr<xn::ImageMetaData> > imageQueue;
|
|
};
|
|
|
|
class TBBApproximateSynchronizerTask: public tbb::task
|
|
{
|
|
public:
|
|
TBBApproximateSynchronizerTask( ApproximateSyncGrabber& approxSyncGrabber ) :
|
|
synchronizer(approxSyncGrabber)
|
|
{}
|
|
|
|
void setMaxBufferSize()
|
|
{
|
|
synchronizer.setMaxBufferSize();
|
|
}
|
|
|
|
bool grab( xn::DepthMetaData& depthMetaData,
|
|
xn::ImageMetaData& imageMetaData )
|
|
{
|
|
return synchronizer.grab( depthMetaData, imageMetaData );
|
|
}
|
|
|
|
private:
|
|
tbb::task* execute()
|
|
{
|
|
synchronizer.spin();
|
|
return 0;
|
|
}
|
|
TBBApproximateSynchronizer synchronizer;
|
|
};
|
|
#endif // HAVE_TBB
|
|
|
|
#ifdef HAVE_TBB
|
|
TBBApproximateSynchronizerTask* task;
|
|
#else
|
|
cv::Ptr<ApproximateSynchronizer> task;
|
|
#endif
|
|
};
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
class CvCapture_OpenNI : public CvCapture
|
|
{
|
|
public:
|
|
enum { DEVICE_DEFAULT=0, DEVICE_MS_KINECT=0, DEVICE_ASUS_XTION=1, DEVICE_MAX=1 };
|
|
|
|
static const int INVALID_PIXEL_VAL = 0;
|
|
static const int INVALID_COORDINATE_VAL = 0;
|
|
|
|
#ifdef HAVE_TBB
|
|
static const int DEFAULT_MAX_BUFFER_SIZE = 8;
|
|
#else
|
|
static const int DEFAULT_MAX_BUFFER_SIZE = 2;
|
|
#endif
|
|
static const int DEFAULT_IS_CIRCLE_BUFFER = 0;
|
|
static const int DEFAULT_MAX_TIME_DURATION = 20;
|
|
|
|
CvCapture_OpenNI(int index=0);
|
|
CvCapture_OpenNI(const char * filename);
|
|
virtual ~CvCapture_OpenNI();
|
|
|
|
virtual double getProperty(int propIdx);
|
|
virtual bool setProperty(int probIdx, double propVal);
|
|
virtual bool grabFrame();
|
|
virtual IplImage* retrieveFrame(int outputType);
|
|
|
|
bool isOpened() const;
|
|
|
|
protected:
|
|
struct OutputMap
|
|
{
|
|
public:
|
|
cv::Mat mat;
|
|
IplImage* getIplImagePtr();
|
|
private:
|
|
IplImage iplHeader;
|
|
};
|
|
|
|
static const int outputMapsTypesCount = 7;
|
|
|
|
static XnMapOutputMode defaultMapOutputMode();
|
|
|
|
IplImage* retrieveDepthMap();
|
|
IplImage* retrievePointCloudMap();
|
|
IplImage* retrieveDisparityMap();
|
|
IplImage* retrieveDisparityMap_32F();
|
|
IplImage* retrieveValidDepthMask();
|
|
IplImage* retrieveBGRImage();
|
|
IplImage* retrieveGrayImage();
|
|
|
|
bool readCamerasParams();
|
|
|
|
double getDepthGeneratorProperty(int propIdx);
|
|
bool setDepthGeneratorProperty(int propIdx, double propVal);
|
|
double getImageGeneratorProperty(int propIdx);
|
|
bool setImageGeneratorProperty(int propIdx, double propVal);
|
|
double getCommonProperty(int propIdx);
|
|
bool setCommonProperty(int propIdx, double propVal);
|
|
|
|
// OpenNI context
|
|
xn::Context context;
|
|
bool isContextOpened;
|
|
|
|
xn::ProductionNode productionNode;
|
|
|
|
// Data generators with its metadata
|
|
xn::DepthGenerator depthGenerator;
|
|
xn::DepthMetaData depthMetaData;
|
|
|
|
xn::ImageGenerator imageGenerator;
|
|
xn::ImageMetaData imageMetaData;
|
|
|
|
int maxBufferSize, maxTimeDuration; // for approx sync
|
|
bool isCircleBuffer;
|
|
cv::Ptr<ApproximateSyncGrabber> approxSyncGrabber;
|
|
|
|
// Cameras settings:
|
|
// TODO find in OpenNI function to convert z->disparity and remove fields "baseline" and depthFocalLength_VGA
|
|
// Distance between IR projector and IR camera (in meters)
|
|
XnDouble baseline;
|
|
// Focal length for the IR camera in VGA resolution (in pixels)
|
|
XnUInt64 depthFocalLength_VGA;
|
|
|
|
// The value for shadow (occluded pixels)
|
|
XnUInt64 shadowValue;
|
|
// The value for pixels without a valid disparity measurement
|
|
XnUInt64 noSampleValue;
|
|
|
|
std::vector<OutputMap> outputMaps;
|
|
};
|
|
|
|
IplImage* CvCapture_OpenNI::OutputMap::getIplImagePtr()
|
|
{
|
|
if( mat.empty() )
|
|
return 0;
|
|
|
|
iplHeader = IplImage(mat);
|
|
return &iplHeader;
|
|
}
|
|
|
|
bool CvCapture_OpenNI::isOpened() const
|
|
{
|
|
return isContextOpened;
|
|
}
|
|
|
|
XnMapOutputMode CvCapture_OpenNI::defaultMapOutputMode()
|
|
{
|
|
XnMapOutputMode mode;
|
|
mode.nXRes = XN_VGA_X_RES;
|
|
mode.nYRes = XN_VGA_Y_RES;
|
|
mode.nFPS = 30;
|
|
return mode;
|
|
}
|
|
|
|
CvCapture_OpenNI::CvCapture_OpenNI( int index )
|
|
{
|
|
int deviceType = DEVICE_DEFAULT;
|
|
XnStatus status;
|
|
|
|
isContextOpened = false;
|
|
maxBufferSize = DEFAULT_MAX_BUFFER_SIZE;
|
|
isCircleBuffer = DEFAULT_IS_CIRCLE_BUFFER;
|
|
maxTimeDuration = DEFAULT_MAX_TIME_DURATION;
|
|
|
|
if( index >= 10 )
|
|
{
|
|
deviceType = index / 10;
|
|
index %= 10;
|
|
}
|
|
|
|
if( deviceType > DEVICE_MAX )
|
|
return;
|
|
|
|
// Initialize and configure the context.
|
|
status = context.Init();
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to initialize the context: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
// Find devices
|
|
xn::NodeInfoList devicesList;
|
|
status = context.EnumerateProductionTrees( XN_NODE_TYPE_DEVICE, NULL, devicesList, 0 );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to enumerate production trees: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
// Chose device according to index
|
|
xn::NodeInfoList::Iterator it = devicesList.Begin();
|
|
for( int i = 0; i < index && it!=devicesList.End(); ++i ) it++;
|
|
if ( it == devicesList.End() )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed device with index " << index << std::endl;
|
|
return;
|
|
}
|
|
|
|
xn::NodeInfo deviceNode = *it;
|
|
status = context.CreateProductionTree( deviceNode, productionNode );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to create production tree: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
xn::ScriptNode scriptNode;
|
|
status = context.RunXmlScript( XMLConfig.c_str(), scriptNode );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to run xml script: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
// Associate generators with context.
|
|
// enumerate the nodes to find if depth generator is present
|
|
xn::NodeInfoList depthList;
|
|
status = context.EnumerateExistingNodes( depthList, XN_NODE_TYPE_DEPTH );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to enumerate depth generators: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
if( depthList.IsEmpty() )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : The device doesn't have depth generator. Such devices aren't supported now." << std::endl;
|
|
return;
|
|
}
|
|
status = depthGenerator.Create( context );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to create depth generator: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
// enumerate the nodes to find if image generator is present
|
|
xn::NodeInfoList imageList;
|
|
status = context.EnumerateExistingNodes( imageList, XN_NODE_TYPE_IMAGE );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to enumerate image generators: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
if( !imageList.IsEmpty() )
|
|
{
|
|
status = imageGenerator.Create( context );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to create image generator: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Set map output mode.
|
|
if( depthGenerator.IsValid() )
|
|
{
|
|
CV_DbgAssert( depthGenerator.SetMapOutputMode(defaultMapOutputMode()) == XN_STATUS_OK ); // xn::DepthGenerator supports VGA only! (Jan 2011)
|
|
}
|
|
if( imageGenerator.IsValid() )
|
|
{
|
|
CV_DbgAssert( imageGenerator.SetMapOutputMode(defaultMapOutputMode()) == XN_STATUS_OK );
|
|
}
|
|
|
|
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*/);
|
|
}
|
|
|
|
// Start generating data.
|
|
status = context.StartGeneratingAll();
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to start generating OpenNI data: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
if( !readCamerasParams() )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Could not read cameras parameters" << std::endl;
|
|
return;
|
|
}
|
|
|
|
outputMaps.resize( outputMapsTypesCount );
|
|
|
|
isContextOpened = true;
|
|
|
|
setProperty(CV_CAP_PROP_OPENNI_REGISTRATION, 1.0);
|
|
}
|
|
|
|
CvCapture_OpenNI::CvCapture_OpenNI(const char * filename)
|
|
{
|
|
XnStatus status;
|
|
|
|
isContextOpened = false;
|
|
maxBufferSize = DEFAULT_MAX_BUFFER_SIZE;
|
|
isCircleBuffer = DEFAULT_IS_CIRCLE_BUFFER;
|
|
maxTimeDuration = DEFAULT_MAX_TIME_DURATION;
|
|
|
|
// Initialize and configure the context.
|
|
status = context.Init();
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to initialize the context: "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
// Open file
|
|
status = context.OpenFileRecording( filename, productionNode );
|
|
if( status != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Failed to open input file (" << filename << "): "
|
|
<< std::string(xnGetStatusString(status)) << std::endl;
|
|
return;
|
|
}
|
|
|
|
context.FindExistingNode( XN_NODE_TYPE_DEPTH, depthGenerator );
|
|
context.FindExistingNode( XN_NODE_TYPE_IMAGE, imageGenerator );
|
|
|
|
if( !readCamerasParams() )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::CvCapture_OpenNI : Could not read cameras parameters" << std::endl;
|
|
return;
|
|
}
|
|
|
|
outputMaps.resize( outputMapsTypesCount );
|
|
|
|
isContextOpened = true;
|
|
}
|
|
|
|
CvCapture_OpenNI::~CvCapture_OpenNI()
|
|
{
|
|
context.StopGeneratingAll();
|
|
context.Release();
|
|
}
|
|
|
|
bool CvCapture_OpenNI::readCamerasParams()
|
|
{
|
|
XnDouble pixelSize = 0;
|
|
if( depthGenerator.GetRealProperty( "ZPPS", pixelSize ) != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::readCamerasParams : Could not read pixel size!" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// pixel size @ VGA = pixel size @ SXGA x 2
|
|
pixelSize *= 2.0; // in mm
|
|
|
|
// focal length of IR camera in pixels for VGA resolution
|
|
XnUInt64 zeroPlanDistance; // in mm
|
|
if( depthGenerator.GetIntProperty( "ZPD", zeroPlanDistance ) != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::readCamerasParams : Could not read virtual plane distance!" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
if( depthGenerator.GetRealProperty( "LDDIS", baseline ) != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::readCamerasParams : Could not read base line!" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// baseline from cm -> mm
|
|
baseline *= 10;
|
|
|
|
// focal length from mm -> pixels (valid for 640x480)
|
|
depthFocalLength_VGA = (XnUInt64)((double)zeroPlanDistance / (double)pixelSize);
|
|
|
|
if( depthGenerator.GetIntProperty( "ShadowValue", shadowValue ) != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::readCamerasParams : Could not read property \"ShadowValue\"!" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
if( depthGenerator.GetIntProperty("NoSampleValue", noSampleValue ) != XN_STATUS_OK )
|
|
{
|
|
std::cerr << "CvCapture_OpenNI::readCamerasParams : Could not read property \"NoSampleValue\"!" <<std::endl;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
double CvCapture_OpenNI::getProperty( int propIdx )
|
|
{
|
|
double propValue = 0;
|
|
|
|
if( isOpened() )
|
|
{
|
|
int purePropIdx = propIdx & ~CV_CAP_OPENNI_GENERATORS_MASK;
|
|
|
|
if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_IMAGE_GENERATOR )
|
|
{
|
|
propValue = getImageGeneratorProperty( purePropIdx );
|
|
}
|
|
else if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_DEPTH_GENERATOR )
|
|
{
|
|
propValue = getDepthGeneratorProperty( purePropIdx );
|
|
}
|
|
else
|
|
{
|
|
propValue = getCommonProperty( purePropIdx );
|
|
}
|
|
}
|
|
|
|
return propValue;
|
|
}
|
|
|
|
bool CvCapture_OpenNI::setProperty( int propIdx, double propValue )
|
|
{
|
|
bool isSet = false;
|
|
if( isOpened() )
|
|
{
|
|
int purePropIdx = propIdx & ~CV_CAP_OPENNI_GENERATORS_MASK;
|
|
|
|
if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_IMAGE_GENERATOR )
|
|
{
|
|
isSet = setImageGeneratorProperty( purePropIdx, propValue );
|
|
}
|
|
else if( (propIdx & CV_CAP_OPENNI_GENERATORS_MASK) == CV_CAP_OPENNI_DEPTH_GENERATOR )
|
|
{
|
|
isSet = setDepthGeneratorProperty( purePropIdx, propValue );
|
|
}
|
|
else
|
|
{
|
|
isSet = setCommonProperty( purePropIdx, propValue );
|
|
}
|
|
}
|
|
|
|
return isSet;
|
|
}
|
|
|
|
double CvCapture_OpenNI::getCommonProperty( int propIdx )
|
|
{
|
|
double propValue = 0;
|
|
|
|
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 :
|
|
case CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH :
|
|
case CV_CAP_PROP_OPENNI_BASELINE :
|
|
case CV_CAP_PROP_OPENNI_FOCAL_LENGTH :
|
|
case CV_CAP_PROP_OPENNI_REGISTRATION :
|
|
propValue = getDepthGeneratorProperty( propIdx );
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_APPROX_FRAME_SYNC :
|
|
propValue = !approxSyncGrabber.empty() && approxSyncGrabber->isRun() ? 1. : 0.;
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_MAX_BUFFER_SIZE :
|
|
propValue = maxBufferSize;
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_CIRCLE_BUFFER :
|
|
propValue = isCircleBuffer ? 1. : 0.;
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_MAX_TIME_DURATION :
|
|
propValue = maxTimeDuration;
|
|
break;
|
|
default :
|
|
{
|
|
std::stringstream ss;
|
|
ss << "Such parameter (propIdx=" << propIdx << ") isn't supported for getting.\n";
|
|
CV_Error( CV_StsBadArg, ss.str().c_str() );
|
|
}
|
|
}
|
|
|
|
return propValue;
|
|
}
|
|
|
|
bool CvCapture_OpenNI::setCommonProperty( int propIdx, double propValue )
|
|
{
|
|
bool isSet = false;
|
|
|
|
switch( propIdx )
|
|
{
|
|
// There is a set of properties that correspond to depth generator by default
|
|
// (is they are pass without particular generator flag).
|
|
case CV_CAP_PROP_OPENNI_REGISTRATION:
|
|
isSet = setDepthGeneratorProperty( propIdx, propValue );
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_APPROX_FRAME_SYNC :
|
|
if( propValue && depthGenerator.IsValid() && imageGenerator.IsValid() )
|
|
{
|
|
// start synchronization
|
|
if( approxSyncGrabber.empty() )
|
|
{
|
|
approxSyncGrabber = new ApproximateSyncGrabber( context, depthGenerator, imageGenerator, maxBufferSize, isCircleBuffer, maxTimeDuration );
|
|
}
|
|
else
|
|
{
|
|
approxSyncGrabber->finish();
|
|
|
|
// update params
|
|
approxSyncGrabber->setMaxBufferSize(maxBufferSize);
|
|
approxSyncGrabber->setIsCircleBuffer(isCircleBuffer);
|
|
approxSyncGrabber->setMaxTimeDuration(maxTimeDuration);
|
|
}
|
|
approxSyncGrabber->start();
|
|
}
|
|
else if( !propValue && !approxSyncGrabber.empty() )
|
|
{
|
|
// finish synchronization
|
|
approxSyncGrabber->finish();
|
|
}
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_MAX_BUFFER_SIZE :
|
|
maxBufferSize = cvRound(propValue);
|
|
if( !approxSyncGrabber.empty() )
|
|
approxSyncGrabber->setMaxBufferSize(maxBufferSize);
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_CIRCLE_BUFFER :
|
|
if( !approxSyncGrabber.empty() )
|
|
approxSyncGrabber->setIsCircleBuffer(isCircleBuffer);
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_MAX_TIME_DURATION :
|
|
maxTimeDuration = cvRound(propValue);
|
|
if( !approxSyncGrabber.empty() )
|
|
approxSyncGrabber->setMaxTimeDuration(maxTimeDuration);
|
|
break;
|
|
default:
|
|
{
|
|
std::stringstream ss;
|
|
ss << "Such parameter (propIdx=" << propIdx << ") isn't supported for setting.\n";
|
|
CV_Error( CV_StsBadArg, ss.str().c_str() );
|
|
}
|
|
}
|
|
|
|
return isSet;
|
|
}
|
|
|
|
double CvCapture_OpenNI::getDepthGeneratorProperty( int propIdx )
|
|
{
|
|
double propValue = 0;
|
|
if( !depthGenerator.IsValid() )
|
|
return propValue;
|
|
|
|
XnMapOutputMode mode;
|
|
|
|
switch( propIdx )
|
|
{
|
|
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
|
|
CV_DbgAssert( depthGenerator.IsValid() );
|
|
propValue = 1.;
|
|
break;
|
|
case CV_CAP_PROP_FRAME_WIDTH :
|
|
if( depthGenerator.GetMapOutputMode(mode) == XN_STATUS_OK )
|
|
propValue = mode.nXRes;
|
|
break;
|
|
case CV_CAP_PROP_FRAME_HEIGHT :
|
|
if( depthGenerator.GetMapOutputMode(mode) == XN_STATUS_OK )
|
|
propValue = mode.nYRes;
|
|
break;
|
|
case CV_CAP_PROP_FPS :
|
|
if( depthGenerator.GetMapOutputMode(mode) == XN_STATUS_OK )
|
|
propValue = mode.nFPS;
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH :
|
|
propValue = depthGenerator.GetDeviceMaxDepth();
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_BASELINE :
|
|
propValue = baseline;
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_FOCAL_LENGTH :
|
|
propValue = (double)depthFocalLength_VGA;
|
|
break;
|
|
case CV_CAP_PROP_OPENNI_REGISTRATION :
|
|
propValue = depthGenerator.GetAlternativeViewPointCap().IsViewPointAs(imageGenerator) ? 1.0 : 0.0;
|
|
break;
|
|
default :
|
|
{
|
|
std::stringstream ss;
|
|
ss << "Depth generator does not support such parameter (propIdx=" << propIdx << ") for getting.\n";
|
|
CV_Error( CV_StsBadArg, ss.str().c_str() );
|
|
}
|
|
}
|
|
|
|
return propValue;
|
|
}
|
|
|
|
bool CvCapture_OpenNI::setDepthGeneratorProperty( int propIdx, double propValue )
|
|
{
|
|
bool isSet = false;
|
|
|
|
CV_Assert( depthGenerator.IsValid() );
|
|
|
|
switch( propIdx )
|
|
{
|
|
case CV_CAP_PROP_OPENNI_REGISTRATION:
|
|
{
|
|
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( imageGenerator.IsValid() )
|
|
{
|
|
if( !depthGenerator.GetAlternativeViewPointCap().IsViewPointAs(imageGenerator) )
|
|
{
|
|
if( depthGenerator.GetAlternativeViewPointCap().IsViewPointSupported(imageGenerator) )
|
|
{
|
|
XnStatus status = depthGenerator.GetAlternativeViewPointCap().SetViewPoint(imageGenerator);
|
|
if( status != XN_STATUS_OK )
|
|
std::cerr << "CvCapture_OpenNI::setDepthGeneratorProperty : " << xnGetStatusString(status) << std::endl;
|
|
else
|
|
isSet = true;
|
|
}
|
|
else
|
|
std::cerr << "CvCapture_OpenNI::setDepthGeneratorProperty : Unsupported viewpoint." << std::endl;
|
|
}
|
|
else
|
|
isSet = true;
|
|
}
|
|
}
|
|
else // "off"
|
|
{
|
|
XnStatus status = depthGenerator.GetAlternativeViewPointCap().ResetViewPoint();
|
|
if( status != XN_STATUS_OK )
|
|
std::cerr << "CvCapture_OpenNI::setDepthGeneratorProperty : " << xnGetStatusString(status) << std::endl;
|
|
else
|
|
isSet = true;
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
std::stringstream ss;
|
|
ss << "Depth generator does not support such parameter (propIdx=" << propIdx << ") for setting.\n";
|
|
CV_Error( CV_StsBadArg, ss.str().c_str() );
|
|
}
|
|
}
|
|
|
|
return isSet;
|
|
}
|
|
|
|
double CvCapture_OpenNI::getImageGeneratorProperty( int propIdx )
|
|
{
|
|
double propValue = 0.;
|
|
if( !imageGenerator.IsValid() )
|
|
return propValue;
|
|
|
|
XnMapOutputMode mode;
|
|
switch( propIdx )
|
|
{
|
|
case CV_CAP_PROP_OPENNI_GENERATOR_PRESENT :
|
|
CV_DbgAssert( imageGenerator.IsValid() );
|
|
propValue = 1.;
|
|
break;
|
|
case CV_CAP_PROP_FRAME_WIDTH :
|
|
if( imageGenerator.GetMapOutputMode(mode) == XN_STATUS_OK )
|
|
propValue = mode.nXRes;
|
|
break;
|
|
case CV_CAP_PROP_FRAME_HEIGHT :
|
|
if( imageGenerator.GetMapOutputMode(mode) == XN_STATUS_OK )
|
|
propValue = mode.nYRes;
|
|
break;
|
|
case CV_CAP_PROP_FPS :
|
|
if( imageGenerator.GetMapOutputMode(mode) == XN_STATUS_OK )
|
|
propValue = mode.nFPS;
|
|
break;
|
|
default :
|
|
{
|
|
std::stringstream ss;
|
|
ss << "Image generator does not support such parameter (propIdx=" << propIdx << ") for getting.\n";
|
|
CV_Error( CV_StsBadArg, ss.str().c_str() );
|
|
}
|
|
}
|
|
|
|
return propValue;
|
|
}
|
|
|
|
bool CvCapture_OpenNI::setImageGeneratorProperty( int propIdx, double propValue )
|
|
{
|
|
bool isSet = false;
|
|
if( !imageGenerator.IsValid() )
|
|
return isSet;
|
|
|
|
switch( propIdx )
|
|
{
|
|
case CV_CAP_PROP_OPENNI_OUTPUT_MODE :
|
|
{
|
|
XnMapOutputMode mode;
|
|
|
|
switch( cvRound(propValue) )
|
|
{
|
|
case CV_CAP_OPENNI_VGA_30HZ :
|
|
mode.nXRes = XN_VGA_X_RES;
|
|
mode.nYRes = XN_VGA_Y_RES;
|
|
mode.nFPS = 30;
|
|
break;
|
|
case CV_CAP_OPENNI_SXGA_15HZ :
|
|
mode.nXRes = XN_SXGA_X_RES;
|
|
mode.nYRes = XN_SXGA_Y_RES;
|
|
mode.nFPS = 15;
|
|
break;
|
|
case CV_CAP_OPENNI_SXGA_30HZ :
|
|
mode.nXRes = XN_SXGA_X_RES;
|
|
mode.nYRes = XN_SXGA_Y_RES;
|
|
mode.nFPS = 30;
|
|
break;
|
|
default :
|
|
CV_Error( CV_StsBadArg, "Unsupported image generator output mode.\n");
|
|
}
|
|
|
|
XnStatus status = imageGenerator.SetMapOutputMode( mode );
|
|
if( status != XN_STATUS_OK )
|
|
std::cerr << "CvCapture_OpenNI::setImageGeneratorProperty : " << xnGetStatusString(status) << std::endl;
|
|
else
|
|
isSet = true;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
std::stringstream ss;
|
|
ss << "Image generator does not support such parameter (propIdx=" << propIdx << ") for setting.\n";
|
|
CV_Error( CV_StsBadArg, ss.str().c_str() );
|
|
}
|
|
}
|
|
|
|
return isSet;
|
|
}
|
|
|
|
bool CvCapture_OpenNI::grabFrame()
|
|
{
|
|
if( !isOpened() )
|
|
return false;
|
|
|
|
bool isGrabbed = false;
|
|
if( !approxSyncGrabber.empty() && approxSyncGrabber->isRun() )
|
|
{
|
|
isGrabbed = approxSyncGrabber->grab( depthMetaData, imageMetaData );
|
|
}
|
|
else
|
|
{
|
|
XnStatus status = context.WaitAndUpdateAll();
|
|
if( status != XN_STATUS_OK )
|
|
return false;
|
|
|
|
if( depthGenerator.IsValid() )
|
|
depthGenerator.GetMetaData( depthMetaData );
|
|
if( imageGenerator.IsValid() )
|
|
imageGenerator.GetMetaData( imageMetaData );
|
|
isGrabbed = true;
|
|
}
|
|
|
|
return isGrabbed;
|
|
}
|
|
|
|
inline void getDepthMapFromMetaData( const xn::DepthMetaData& depthMetaData, cv::Mat& depthMap, XnUInt64 noSampleValue, XnUInt64 shadowValue )
|
|
{
|
|
int cols = depthMetaData.XRes();
|
|
int rows = depthMetaData.YRes();
|
|
|
|
depthMap.create( rows, cols, CV_16UC1 );
|
|
|
|
const XnDepthPixel* pDepthMap = depthMetaData.Data();
|
|
|
|
// CV_Assert( sizeof(unsigned short) == sizeof(XnDepthPixel) );
|
|
memcpy( depthMap.data, pDepthMap, cols*rows*sizeof(XnDepthPixel) );
|
|
|
|
cv::Mat badMask = (depthMap == (double)noSampleValue) | (depthMap == (double)shadowValue) | (depthMap == 0);
|
|
|
|
// mask the pixels with invalid depth
|
|
depthMap.setTo( cv::Scalar::all( CvCapture_OpenNI::INVALID_PIXEL_VAL ), badMask );
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveDepthMap()
|
|
{
|
|
if( !depthMetaData.Data() )
|
|
return 0;
|
|
|
|
getDepthMapFromMetaData( depthMetaData, outputMaps[CV_CAP_OPENNI_DEPTH_MAP].mat, noSampleValue, shadowValue );
|
|
|
|
return outputMaps[CV_CAP_OPENNI_DEPTH_MAP].getIplImagePtr();
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrievePointCloudMap()
|
|
{
|
|
if( !depthMetaData.Data() )
|
|
return 0;
|
|
|
|
cv::Mat depth;
|
|
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
|
|
|
|
const int badPoint = INVALID_PIXEL_VAL;
|
|
const float badCoord = INVALID_COORDINATE_VAL;
|
|
int cols = depthMetaData.XRes(), rows = depthMetaData.YRes();
|
|
cv::Mat pointCloud_XYZ( rows, cols, CV_32FC3, cv::Scalar::all(badPoint) );
|
|
|
|
cv::Ptr<XnPoint3D> proj = new XnPoint3D[cols*rows];
|
|
cv::Ptr<XnPoint3D> real = new XnPoint3D[cols*rows];
|
|
for( int y = 0; y < rows; y++ )
|
|
{
|
|
for( int x = 0; x < cols; x++ )
|
|
{
|
|
int ind = y*cols+x;
|
|
proj[ind].X = (float)x;
|
|
proj[ind].Y = (float)y;
|
|
proj[ind].Z = depth.at<unsigned short>(y, x);
|
|
}
|
|
}
|
|
depthGenerator.ConvertProjectiveToRealWorld(cols*rows, proj, real);
|
|
|
|
for( int y = 0; y < rows; y++ )
|
|
{
|
|
for( int x = 0; x < cols; x++ )
|
|
{
|
|
// Check for invalid measurements
|
|
if( depth.at<unsigned short>(y, x) == badPoint ) // not valid
|
|
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( badCoord, badCoord, badCoord );
|
|
else
|
|
{
|
|
int ind = y*cols+x;
|
|
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( real[ind].X*0.001f, real[ind].Y*0.001f, real[ind].Z*0.001f); // from mm to meters
|
|
}
|
|
}
|
|
}
|
|
|
|
outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].mat = pointCloud_XYZ;
|
|
|
|
return outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].getIplImagePtr();
|
|
}
|
|
|
|
static void computeDisparity_32F( const xn::DepthMetaData& depthMetaData, cv::Mat& disp, XnDouble baseline, XnUInt64 F,
|
|
XnUInt64 noSampleValue, XnUInt64 shadowValue )
|
|
{
|
|
cv::Mat depth;
|
|
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
|
|
CV_Assert( depth.type() == CV_16UC1 );
|
|
|
|
|
|
// disparity = baseline * F / z;
|
|
|
|
float mult = (float)(baseline /*mm*/ * F /*pixels*/);
|
|
|
|
disp.create( depth.size(), CV_32FC1);
|
|
disp = cv::Scalar::all( CvCapture_OpenNI::INVALID_PIXEL_VAL );
|
|
for( int y = 0; y < disp.rows; y++ )
|
|
{
|
|
for( int x = 0; x < disp.cols; x++ )
|
|
{
|
|
unsigned short curDepth = depth.at<unsigned short>(y,x);
|
|
if( curDepth != CvCapture_OpenNI::INVALID_PIXEL_VAL )
|
|
disp.at<float>(y,x) = mult / curDepth;
|
|
}
|
|
}
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveDisparityMap()
|
|
{
|
|
if( !depthMetaData.Data() )
|
|
return 0;
|
|
|
|
cv::Mat disp32;
|
|
computeDisparity_32F( depthMetaData, disp32, baseline, depthFocalLength_VGA, noSampleValue, shadowValue );
|
|
|
|
disp32.convertTo( outputMaps[CV_CAP_OPENNI_DISPARITY_MAP].mat, CV_8UC1 );
|
|
|
|
return outputMaps[CV_CAP_OPENNI_DISPARITY_MAP].getIplImagePtr();
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveDisparityMap_32F()
|
|
{
|
|
if( !depthMetaData.Data() )
|
|
return 0;
|
|
|
|
computeDisparity_32F( depthMetaData, outputMaps[CV_CAP_OPENNI_DISPARITY_MAP_32F].mat, baseline, depthFocalLength_VGA, noSampleValue, shadowValue );
|
|
|
|
return outputMaps[CV_CAP_OPENNI_DISPARITY_MAP_32F].getIplImagePtr();
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveValidDepthMask()
|
|
{
|
|
if( !depthMetaData.Data() )
|
|
return 0;
|
|
|
|
cv::Mat depth;
|
|
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
|
|
|
|
outputMaps[CV_CAP_OPENNI_VALID_DEPTH_MASK].mat = depth != CvCapture_OpenNI::INVALID_PIXEL_VAL;
|
|
|
|
return outputMaps[CV_CAP_OPENNI_VALID_DEPTH_MASK].getIplImagePtr();
|
|
}
|
|
|
|
inline void getBGRImageFromMetaData( const xn::ImageMetaData& imageMetaData, cv::Mat& bgrImage )
|
|
{
|
|
if( imageMetaData.PixelFormat() != XN_PIXEL_FORMAT_RGB24 )
|
|
CV_Error( CV_StsUnsupportedFormat, "Unsupported format of grabbed image\n" );
|
|
|
|
cv::Mat rgbImage( imageMetaData.YRes(), imageMetaData.XRes(), CV_8UC3 );
|
|
const XnRGB24Pixel* pRgbImage = imageMetaData.RGB24Data();
|
|
|
|
// CV_Assert( 3*sizeof(uchar) == sizeof(XnRGB24Pixel) );
|
|
memcpy( rgbImage.data, pRgbImage, rgbImage.total()*sizeof(XnRGB24Pixel) );
|
|
cv::cvtColor( rgbImage, bgrImage, CV_RGB2BGR );
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveBGRImage()
|
|
{
|
|
if( !imageMetaData.Data() )
|
|
return 0;
|
|
|
|
getBGRImageFromMetaData( imageMetaData, outputMaps[CV_CAP_OPENNI_BGR_IMAGE].mat );
|
|
|
|
return outputMaps[CV_CAP_OPENNI_BGR_IMAGE].getIplImagePtr();
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveGrayImage()
|
|
{
|
|
if( !imageMetaData.Data() )
|
|
return 0;
|
|
|
|
CV_Assert( imageMetaData.BytesPerPixel() == 3 ); // RGB
|
|
|
|
cv::Mat rgbImage;
|
|
getBGRImageFromMetaData( imageMetaData, rgbImage );
|
|
cv::cvtColor( rgbImage, outputMaps[CV_CAP_OPENNI_GRAY_IMAGE].mat, CV_BGR2GRAY );
|
|
|
|
return outputMaps[CV_CAP_OPENNI_GRAY_IMAGE].getIplImagePtr();
|
|
}
|
|
|
|
IplImage* CvCapture_OpenNI::retrieveFrame( int outputType )
|
|
{
|
|
IplImage* image = 0;
|
|
CV_Assert( outputType < outputMapsTypesCount && outputType >= 0);
|
|
|
|
if( outputType == CV_CAP_OPENNI_DEPTH_MAP )
|
|
{
|
|
image = retrieveDepthMap();
|
|
}
|
|
else if( outputType == CV_CAP_OPENNI_POINT_CLOUD_MAP )
|
|
{
|
|
image = retrievePointCloudMap();
|
|
}
|
|
else if( outputType == CV_CAP_OPENNI_DISPARITY_MAP )
|
|
{
|
|
image = retrieveDisparityMap();
|
|
}
|
|
else if( outputType == CV_CAP_OPENNI_DISPARITY_MAP_32F )
|
|
{
|
|
image = retrieveDisparityMap_32F();
|
|
}
|
|
else if( outputType == CV_CAP_OPENNI_VALID_DEPTH_MASK )
|
|
{
|
|
image = retrieveValidDepthMask();
|
|
}
|
|
else if( outputType == CV_CAP_OPENNI_BGR_IMAGE )
|
|
{
|
|
image = retrieveBGRImage();
|
|
}
|
|
else if( outputType == CV_CAP_OPENNI_GRAY_IMAGE )
|
|
{
|
|
image = retrieveGrayImage();
|
|
}
|
|
|
|
return image;
|
|
}
|
|
|
|
|
|
CvCapture* cvCreateCameraCapture_OpenNI( int index )
|
|
{
|
|
CvCapture_OpenNI* capture = new CvCapture_OpenNI( index );
|
|
|
|
if( capture->isOpened() )
|
|
return capture;
|
|
|
|
delete capture;
|
|
return 0;
|
|
}
|
|
|
|
CvCapture* cvCreateFileCapture_OpenNI( const char* filename )
|
|
{
|
|
CvCapture_OpenNI* capture = new CvCapture_OpenNI( filename );
|
|
|
|
if( capture->isOpened() )
|
|
return capture;
|
|
|
|
delete capture;
|
|
return 0;
|
|
}
|
|
|
|
#endif
|