generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

optimized retrievePointCloudMap (#1106)

Browse Source
This commit is contained in:
Maria Dimashova 2011-06-08 13:06:29 +00:00
parent 715b5d0c55
commit 4a1ccbeee6
1 changed files with 30 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
modules/highgui/src/cap_openni.cpp
View File

@ -86,6 +86,7 @@ class CvCapture_OpenNI : public CvCapture
{
public:
static const int INVALID_PIXEL_VAL = 0;
static const int INVALID_COORDINATE_VAL = 0;
CvCapture_OpenNI();
virtual ~CvCapture_OpenNI();
@ -482,25 +483,36 @@ IplImage* CvCapture_OpenNI::retrievePointCloudMap()
cv::Mat depth;
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
const float badPoint = 0;
const int badPoint = INVALID_PIXEL_VAL;
const float badCoord = INVALID_COORDINATE_VAL;
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 = x;
proj[ind].Y = 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++ )
{
unsigned short d = depth.at<unsigned short>(y, x);
// Check for invalid measurements
if( d == CvCapture_OpenNI::INVALID_PIXEL_VAL ) // not valid
continue;
XnPoint3D proj, real;
proj.X = x;
proj.Y = y;
proj.Z = d;
depthGenerator.ConvertProjectiveToRealWorld(1, &proj, &real);
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( real.X*0.001f, real.Y*0.001f, real.Z*0.001f); // from mm to meters
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
}
}
}