Reintroduce GrayCodePattern::getProjPixel to allow for tests to pass, fixup NaN values in projector mapping

This commit reintroduces a faster and improved version of getProjPixel to allow for tests that relied on it to pass, and it also fixes an issue with NaN values appearing in projector mapping when a pixel wasn't seen by either one of the cameras.

Author: Atari2

modules/structured_light/include/opencv2/structured_light/graycodepattern.hpp

@@ -128,6 +128,19 @@ class CV_EXPORTS_W GrayCodePattern : public StructuredLightPattern
    */
   CV_WRAP
   virtual void getImagesForShadowMasks( InputOutputArray blackImage, InputOutputArray whiteImage ) const = 0;
+
+  /** @brief For a (x,y) pixel of a camera returns the corresponding projector pixel.
+   *
+   *  The function decodes each pixel in the pattern images acquired by a camera into their corresponding decimal numbers representing the projector's column and row,
+   *  providing a mapping between camera's and projector's pixel.
+   *
+   *  @param patternImages The pattern images acquired by the camera, stored in a grayscale vector < Mat >.
+   *  @param x x coordinate of the image pixel.
+   *  @param y y coordinate of the image pixel.
+   *  @param projPix Projector's pixel corresponding to the camera's pixel: projPix.x and projPix.y are the image coordinates of the projector's pixel corresponding to the pixel being decoded in a camera.
+   */
+  CV_WRAP
+  virtual bool getProjPixel(InputArrayOfArrays patternImages, int x, int y, CV_OUT Point& projPix) const = 0;
 };
 
 //! @}

modules/structured_light/src/graycodepattern.cpp

@@ -71,6 +71,8 @@ class CV_EXPORTS_W GrayCodePattern_Impl CV_FINAL : public GrayCodePattern
   // Generates the images needed for shadowMasks computation
   void getImagesForShadowMasks( InputOutputArray blackImage, InputOutputArray whiteImage ) const CV_OVERRIDE;
 
+  bool getProjPixel(InputArrayOfArrays patternImages, int x, int y, Point& projPix) const CV_OVERRIDE;
+
  private:
   // Parameters
   Params params;
@@ -245,8 +247,10 @@ bool GrayCodePattern_Impl::decode(const std::vector< std::vector<Mat> >& pattern
         cv::divide(sumX[0], counts_64F[0], avgX[0]);
         cv::divide(sumX[1], counts_64F[1], avgX[1]);
 
+        // Handle invalid pixels (e.g. not seen by one of the cameras) by setting them to zero.
+        cv::Mat invalidPixels = counts[0] == 0 | counts[1] == 0;
+
         Mat projectorDisparity = avgX[1] - avgX[0];
-        Mat validProjectorPixels = (counts[0] > 0) & (counts[1] > 0);
 
         Mat& disparityMap_ = *(Mat*)disparityMap.getObj();
         Mat map_x(cam_height, cam_width, CV_32F);
@@ -257,16 +261,11 @@ bool GrayCodePattern_Impl::decode(const std::vector< std::vector<Mat> >& pattern
         projMapChannels[0].convertTo(map_x, CV_32F); // Projector x-coordinates
         projMapChannels[1].convertTo(map_y, CV_32F); // Projector y-coordinates
 
-        cv::remap(projectorDisparity, disparityMap_, map_x, map_y,
-            INTER_NEAREST, BORDER_CONSTANT, Scalar(0));
+        projectorDisparity.setTo(Scalar(0.0), invalidPixels);
 
-        // Remap the validity mask and invert it to get the final invalid mask.
-        Mat validCameraPixels;
-        cv::remap(validProjectorPixels, validCameraPixels, map_x, map_y,
+        cv::remap(projectorDisparity, disparityMap_, map_x, map_y,
             INTER_NEAREST, BORDER_CONSTANT, Scalar(0));
 
-        Mat invalidMask;
-        cv::bitwise_not(validCameraPixels, invalidMask);
         return true;
     }
 
@@ -353,6 +352,95 @@ void GrayCodePattern_Impl::populateFastPatternImages(const std::vector<std::vect
     }
 }
 
+static int grayToDec(const std::vector<uchar>& gray)
+{
+    if (gray.empty())
+    {
+        return 0;
+    }
+
+    int dec = 0;
+    uchar prev_binary_bit = 0;
+    for (size_t i = 0; i < gray.size(); ++i)
+    {
+        uchar current_binary_bit = prev_binary_bit ^ gray[i];
+        dec = (dec << 1) | current_binary_bit;
+        prev_binary_bit = current_binary_bit;
+    }
+    return dec;
+}
+
+bool GrayCodePattern_Impl::getProjPixel(InputArrayOfArrays patternImages, int x, int y, Point& projPix) const
+{
+    std::vector<Mat>& patternImagesVec = *(std::vector<Mat>*)patternImages.getObj();
+    const int rows = patternImagesVec[0].rows;
+    const int cols = patternImagesVec[0].cols;
+
+    std::vector<uchar> grayCol(numOfColImgs);
+    std::vector<uchar> grayRow(numOfRowImgs);
+
+    auto decode_pixel = [&](auto dummy_type_ptr) -> bool
+        {
+            using T = typename std::remove_pointer<decltype(dummy_type_ptr)>::type;
+            bool error = false;
+
+            for (size_t k = 0; k < numOfColImgs; ++k)
+            {
+                T val1 = patternImagesVec[k * 2].at<T>(y, x);
+                T val2 = patternImagesVec[k * 2 + 1].at<T>(y, x);
+
+                if (std::abs(static_cast<double>(val1) - static_cast<double>(val2)) < whiteThreshold)
+                {
+                    error = true;
+                }
+                grayCol[k] = (val1 > val2);
+            }
+
+            size_t base_idx = 2 * numOfColImgs;
+            for (size_t k = 0; k < numOfRowImgs; ++k)
+            {
+                T val1 = patternImagesVec[base_idx + k * 2].at<T>(y, x);
+                T val2 = patternImagesVec[base_idx + k * 2 + 1].at<T>(y, x);
+
+                if (std::abs(static_cast<double>(val1) - static_cast<double>(val2)) < whiteThreshold)
+                {
+                    error = true;
+                }
+                grayRow[k] = (val1 > val2);
+            }
+            return error;
+        };
+
+    bool error = false;
+    int depth = patternImagesVec[0].depth();
+
+    switch (depth)
+    {
+    case CV_8U:
+        error = decode_pixel((uchar*)nullptr);
+        break;
+    case CV_16U:
+        error = decode_pixel((ushort*)nullptr);
+        break;
+    default:
+        CV_Error(Error::StsUnsupportedFormat, "Image depth not supported, only CV_8U and CV_16U");
+    }
+
+    int xDec = grayToDec(grayCol);
+    int yDec = grayToDec(grayRow);
+
+    if (yDec >= params.height || xDec >= params.width)
+    {
+        error = true;
+    }
+
+    projPix.x = xDec;
+    projPix.y = yDec;
+
+    return error;
+}
+
+
 // For a (x,y) pixel of the camera returns the corresponding projector's pixel
 bool GrayCodePattern_Impl::getProjPixelFast( const std::vector<Mat>& fastColImages, const std::vector<Mat>& fastRowImages, int x, int y, Point &projPix) const
 {