optimized retrievePointCloudMap (#1106)

modules/highgui/src/cap_openni.cpp

@@ -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);
 
-            unsigned short d = depth.at<unsigned short>(y, x);
+    for( int y = 0; y < rows; y++ )
+    {
+        for( int x = 0; x < cols; 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
+            }
         }
     }