depthTo3d: fixed bug, added regression test

RgbdNormals: setMethod() removed as useless

RgbdNormals: tests + cross product, to be fixed

+ cross product

LINEMOD: diffThreshold param added + tests fixed

minor

diffThreshold fix

points3dToDepth16U fix

normals computer diffThreshold fix

random plane generation fixed + diffThreshold fix

Rendered normals test rewritten to GTest Params

random plane generation: scale

RGBD_Normals tests: thresholds tuned

Rendered normals tests: 64F support added

Random planes normal tests rewritten to GTest Params

LINEMOD and CrossProduct fix

SRI threshold raised

NormalsRandomPlanes: thresholds raised

assert on unknown alg; minor

fix

frame size reduced

TIFF replaced by YAML.GZ

depthTo3d test changed

cv::transform is used

fix warning

nanMask()

flipAxes()

absDotPixel() + forgotten code

helper functions removed

RGBDNormals: checkNormals() and compare LINEMOD's pts3d to depth input

Rendered: another criteria; thresholds; LINEMOD's pts3d to depth input comparison

thresholds raised a bit

SRI slightly optimized

assert change

normal tests refactored, parametrized, split

trailing namespace, thresholds raised

SRI caching optimized a lot

normal tests rewritten to fixture, no loop

minor

runCase() joined with testIt()

thresholds were put into GTest params

ternary operator

RgbdNormalsTest merged into NormalsRandomPlanes; RgbdPlanes moved closer to tests

normal test minor refactoring

plane finder tests refactored to GTest Params

skip tests

thresholds raised

plane test minor

plane tests: timers dropped, nPlanes put into GTest Params; refactoring

generated normals tests: minor refactoring

flipAxes() templated

rendered normals tests refactored: thresholds to GTest Params

CV_Error -> ASSERT_FALSE

Author: savuor

modules/3d/include/opencv2/3d/depth.hpp

@@ -30,7 +30,8 @@ class CV_EXPORTS_W RgbdNormals
     {
       RGBD_NORMALS_METHOD_FALS = 0,
       RGBD_NORMALS_METHOD_LINEMOD = 1,
-      RGBD_NORMALS_METHOD_SRI = 2
+      RGBD_NORMALS_METHOD_SRI = 2,
+      RGBD_NORMALS_METHOD_CROSS_PRODUCT = 3
     };
 
     RgbdNormals() { }
@@ -42,9 +43,11 @@ class CV_EXPORTS_W RgbdNormals
      * @param depth the depth of the normals (only CV_32F or CV_64F)
      * @param K the calibration matrix to use
      * @param window_size the window size to compute the normals: can only be 1,3,5 or 7
+     * @param diff_threshold threshold in depth difference, used in LINEMOD algirithm
      * @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS
      */
     CV_WRAP static Ptr<RgbdNormals> create(int rows = 0, int cols = 0, int depth = 0, InputArray K = noArray(), int window_size = 5,
+                                           float diff_threshold = 50.f,
                                            RgbdNormals::RgbdNormalsMethod method = RgbdNormals::RgbdNormalsMethod::RGBD_NORMALS_METHOD_FALS);
 
     /** Given a set of 3d points in a depth image, compute the normals at each point.
@@ -68,7 +71,6 @@ class CV_EXPORTS_W RgbdNormals
     CV_WRAP virtual void getK(OutputArray val) const = 0;
     CV_WRAP virtual void setK(InputArray val) = 0;
     CV_WRAP virtual RgbdNormals::RgbdNormalsMethod getMethod() const = 0;
-    CV_WRAP virtual void setMethod(RgbdNormals::RgbdNormalsMethod val) = 0;
 };
 
 
@@ -146,7 +148,7 @@ enum RgbdPlaneMethod
 
 /** Find the planes in a depth image
  * @param points3d the 3d points organized like the depth image: rows x cols with 3 channels
- * @param normals the normals for every point in the depth image
+ * @param normals the normals for every point in the depth image; optional, can be empty
  * @param mask An image where each pixel is labeled with the plane it belongs to
  *        and 255 if it does not belong to any plane
  * @param plane_coefficients the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0, norm(a,b,c)=1

modules/3d/src/rgbd/depth_to_3d.cpp

@@ -46,7 +46,7 @@ static void depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const
     coordinates[0] = coordinates[0].mul(z_mat);
     coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy);
     coordinates[2] = z_mat;
-    coordinates[3] = 0;
+    coordinates[3] = Mat(u_mat.size(), CV_32F, Scalar(0));
     cv::merge(coordinates, points3d);
 }
 

modules/3d/src/rgbd/normal.cpp

@@ -225,24 +225,7 @@ class RgbdNormalsImpl : public RgbdNormals
     {
         return method;
     }
-    virtual void setMethod(RgbdNormalsMethod val) CV_OVERRIDE
-    {
-        method = val; cacheIsDirty = true;
-    }
-
-    // Helper functions for apply()
-    virtual void assertOnBadArg(const Mat& points3d_ori) const = 0;
-    virtual void calcRadiusAnd3d(const Mat& points3d_ori, Mat& points3d, Mat& radius) const
-    {
-        // Make the points have the right depth
-        if (points3d_ori.depth() == dtype)
-            points3d = points3d_ori;
-        else
-            points3d_ori.convertTo(points3d, dtype);
 
-        // Compute the distance to the points
-        radius = computeRadius<T>(points3d);
-    }
     virtual void compute(const Mat& in, Mat& normals) const = 0;
 
     /** Given a set of 3d points in a depth image, compute the normals at each point
@@ -256,29 +239,66 @@ class RgbdNormalsImpl : public RgbdNormals
         CV_Assert(points3d_ori.dims == 2);
 
         // Either we have 3d points or a depth image
-        assertOnBadArg(points3d_ori);
+
+        bool ptsAre4F = (points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
+        bool ptsAreDepth = (points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
+        if (method == RGBD_NORMALS_METHOD_FALS || method == RGBD_NORMALS_METHOD_SRI || method == RGBD_NORMALS_METHOD_CROSS_PRODUCT)
+        {
+            if (!ptsAre4F)
+            {
+                CV_Error(Error::StsBadArg, "Input image should have 4 float-point channels");
+            }
+        }
+        else if (method == RGBD_NORMALS_METHOD_LINEMOD)
+        {
+            if (!ptsAre4F && !ptsAreDepth)
+            {
+                CV_Error(Error::StsBadArg, "Input image should have 4 float-point channels or have 1 ushort or float-point channel");
+            }
+        }
+        else
+        {
+            CV_Error(Error::StsInternal, "Unknown normal computer algorithm");
+        }
 
         // Initialize the pimpl
         cache();
 
         // Precompute something for RGBD_NORMALS_METHOD_SRI and RGBD_NORMALS_METHOD_FALS
-        Mat points3d, radius;
-        calcRadiusAnd3d(points3d_ori, points3d, radius);
+        Mat points3d;
+        if (method != RGBD_NORMALS_METHOD_LINEMOD)
+        {
+            // Make the points have the right depth
+            if (points3d_ori.depth() == dtype)
+                points3d = points3d_ori;
+            else
+                points3d_ori.convertTo(points3d, dtype);
+        }
 
         // Get the normals
         normals_out.create(points3d_ori.size(), CV_MAKETYPE(dtype, 4));
-        if (points3d_in.empty())
+        if (points3d_ori.empty())
             return;
 
         Mat normals = normals_out.getMat();
         if ((method == RGBD_NORMALS_METHOD_FALS) || (method == RGBD_NORMALS_METHOD_SRI))
         {
+            // Compute the distance to the points
+            Mat radius = computeRadius<T>(points3d);
             compute(radius, normals);
         }
-        else // LINEMOD
+        else if (method == RGBD_NORMALS_METHOD_LINEMOD)
         {
             compute(points3d_ori, normals);
         }
+        else if (method == RGBD_NORMALS_METHOD_CROSS_PRODUCT)
+        {
+            compute(points3d, normals);
+        }
+        else
+        {
+            CV_Error(Error::StsInternal, "Unknown normal computer algorithm");
+        }
     }
 
     int rows, cols;
@@ -406,13 +426,6 @@ class FALS : public RgbdNormalsImpl<T>
             }
     }
 
-    virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
-    {
-        //CV_Assert(points3d_ori.channels() == 3);
-        CV_Assert(points3d_ori.channels() == 4);
-        CV_Assert(points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
-    }
-
     // Cached data
     mutable Mat_<Vec3T> V_;
     mutable Mat_<Vec9T> M_inv_;
@@ -448,14 +461,17 @@ class LINEMOD : public RgbdNormalsImpl<T>
     typedef Vec<T, 3> Vec3T;
     typedef Matx<T, 3, 3> Mat33T;
 
-    LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K) :
-        RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
+    LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K, float _diffThr = 50.f) :
+        RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD),
+        differenceThreshold(_diffThr)
     { }
 
     /** Compute cached data
      */
     virtual void cache() const CV_OVERRIDE
-    { }
+    {
+        this->cacheIsDirty = false;
+    }
 
     /** Compute the normals
      * @param r
@@ -536,7 +552,9 @@ class LINEMOD : public RgbdNormalsImpl<T>
 
         Vec3T X1_minus_X, X2_minus_X;
 
-        ContainerDepth difference_threshold = 50;
+        ContainerDepth difference_threshold(differenceThreshold);
+        //TODO: fixit, difference threshold should not depend on input type
+        difference_threshold *= (std::is_same<DepthDepth, ushort>::value ? 1000.f : 1.f);
         normals.setTo(std::numeric_limits<DepthDepth>::quiet_NaN());
         for (int y = r; y < this->rows - r - 1; ++y)
         {
@@ -591,14 +609,7 @@ class LINEMOD : public RgbdNormalsImpl<T>
         return normals;
     }
 
-    virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
-    {
-        CV_Assert(((points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)) ||
-                  ((points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
-    }
-
-    virtual void calcRadiusAnd3d(const Mat& /*points3d_ori*/, Mat& /*points3d*/, Mat& /*radius*/) const CV_OVERRIDE
-    { }
+    float differenceThreshold;
 };
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -648,25 +659,28 @@ class SRI : public RgbdNormalsImpl<T>
         for (int phi_int = 0, k = 0; phi_int < this->rows; ++phi_int)
         {
             float phi = min_phi + phi_int * phi_step_;
+            float phi_sin = std::sin(phi), phi_cos = std::cos(phi);
             for (int theta_int = 0; theta_int < this->cols; ++theta_int, ++k)
             {
                 float theta = min_theta + theta_int * theta_step_;
+                float theta_sin = std::sin(theta), theta_cos = std::cos(theta);
                 // Store the 3d point to project it later
-                points3d[k] = Point3f(std::sin(theta) * std::cos(phi), std::sin(phi), std::cos(theta) * std::cos(phi));
+                Point3f pp(theta_sin * phi_cos, phi_sin, theta_cos * phi_cos);
+                points3d[k] = pp;
 
                 // Cache the rotation matrix and negate it
-                Mat_<T> mat =
-                    (Mat_ < T >(3, 3) << 0, 1, 0, 0, 0, 1, 1, 0, 0) *
-                    ((Mat_ < T >(3, 3) << std::cos(theta), -std::sin(theta), 0, std::sin(theta), std::cos(theta), 0, 0, 0, 1)) *
-                    ((Mat_ < T >(3, 3) << std::cos(phi), 0, -std::sin(phi), 0, 1, 0, std::sin(phi), 0, std::cos(phi)));
+                Matx<T, 3, 3> mat = Matx<T, 3, 3> (0, 1, 0,  0, 0, 1,  1, 0, 0) *
+                                    Matx<T, 3, 3> (theta_cos, -theta_sin, 0,  theta_sin, theta_cos, 0,  0, 0, 1) *
+                                    Matx<T, 3, 3> (phi_cos, 0, -phi_sin,  0, 1, 0,  phi_sin, 0, phi_cos);
+
                 for (unsigned char i = 0; i < 3; ++i)
-                    mat(i, 1) = mat(i, 1) / std::cos(phi);
+                    mat(i, 1) = mat(i, 1) / phi_cos;
                 // The second part of the matrix is never explained in the paper ... but look at the wikipedia normal article
-                mat(0, 0) = mat(0, 0) - 2 * std::cos(phi) * std::sin(theta);
-                mat(1, 0) = mat(1, 0) - 2 * std::sin(phi);
-                mat(2, 0) = mat(2, 0) - 2 * std::cos(phi) * std::cos(theta);
+                mat(0, 0) = mat(0, 0) - 2 * pp.x;
+                mat(1, 0) = mat(1, 0) - 2 * pp.y;
+                mat(2, 0) = mat(2, 0) - 2 * pp.z;
 
-                R_hat_(phi_int, theta_int) = Vec9T((T*)(mat.data));
+                R_hat_(phi_int, theta_int) = Vec9T(mat.val);
             }
         }
 
@@ -747,8 +761,8 @@ class SRI : public RgbdNormalsImpl<T>
                 T r_phi_over_r = (*r_phi_ptr) / (*r_ptr);
                 // R(1,1) is 0
                 signNormal((*R)(0, 0) + (*R)(0, 1) * r_theta_over_r + (*R)(0, 2) * r_phi_over_r,
-                    (*R)(1, 0) + (*R)(1, 2) * r_phi_over_r,
-                    (*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal);
+                           (*R)(1, 0) + (*R)(1, 2) * r_phi_over_r,
+                           (*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal);
             }
         }
 
@@ -759,11 +773,6 @@ class SRI : public RgbdNormalsImpl<T>
             signNormal((*normal)[0], (*normal)[1], (*normal)[2], *normal);
     }
 
-    virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
-    {
-        CV_Assert(((points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
-    }
-
     // Cached data
     /** Stores R */
     mutable Mat_<Vec9T> R_hat_;
@@ -781,14 +790,91 @@ class SRI : public RgbdNormalsImpl<T>
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K, int windowSize, RgbdNormalsMethod method)
+/* Uses the simpliest possible method for normals calculation: calculates cross product between two vectors
+(pointAt(x+1, y) - pointAt(x, y)) and (pointAt(x, y+1) - pointAt(x, y)) */
+
+template<typename DataType>
+class CrossProduct : public RgbdNormalsImpl<DataType>
+{
+public:
+    typedef Vec<DataType, 3> Vec3T;
+    typedef Vec<DataType, 4> Vec4T;
+    typedef Point3_<DataType> Point3T;
+
+    CrossProduct(int _rows, int _cols, int _windowSize, const Mat& _K) :
+        RgbdNormalsImpl<DataType>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT)
+    { }
+
+    /** Compute cached data
+     */
+    virtual void cache() const CV_OVERRIDE
+    {
+        this->cacheIsDirty = false;
+    }
+
+    static inline Point3T fromVec(Vec4T v)
+    {
+        return {v[0], v[1], v[2]};
+    }
+
+    static inline Vec4T toVec4(Point3T p)
+    {
+        return {p.x, p.y, p.z, 0};
+    }
+
+    static inline bool haveNaNs(Point3T p)
+    {
+        return cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z);
+    }
+
+    /** Compute the normals
+     * @param points reprojected depth points
+     * @param normals generated normals
+     * @return
+     */
+    virtual void compute(const Mat& points, Mat& normals) const CV_OVERRIDE
+    {
+        for(int y = 0; y < this->rows; y++)
+        {
+            const Vec4T* ptsRow0 = points.ptr<Vec4T>(y);
+            const Vec4T* ptsRow1 = (y < this->rows - 1) ? points.ptr<Vec4T>(y + 1) : nullptr;
+            Vec4T *normRow = normals.ptr<Vec4T>(y);
+
+            for (int x = 0; x < this->cols; x++)
+            {
+                Point3T v00 = fromVec(ptsRow0[x]);
+                const float qnan = std::numeric_limits<float>::quiet_NaN();
+                Point3T n(qnan, qnan, qnan);
+
+                if ((x < this->cols - 1) && (y < this->rows - 1) && !haveNaNs(v00))
+                {
+                    Point3T v01 = fromVec(ptsRow0[x + 1]);
+                    Point3T v10 = fromVec(ptsRow1[x]);
+
+                    if (!haveNaNs(v01) && !haveNaNs(v10))
+                    {
+                        Vec3T vec = (v10 - v00).cross(v01 - v00);
+                        n = normalize(vec);
+                    }
+                }
+
+                normRow[x] = toVec4(n);
+            }
+        }
+    }
+};
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K, int windowSize, float diffThreshold, RgbdNormalsMethod method)
 {
     CV_Assert(rows > 0 && cols > 0 && (depth == CV_32F || depth == CV_64F));
     CV_Assert(windowSize == 1 || windowSize == 3 || windowSize == 5 || windowSize == 7);
     CV_Assert(K.cols() == 3 && K.rows() == 3 && (K.depth() == CV_32F || K.depth() == CV_64F));
 
     Mat mK = K.getMat();
-    CV_Assert(method == RGBD_NORMALS_METHOD_FALS || method == RGBD_NORMALS_METHOD_LINEMOD || method == RGBD_NORMALS_METHOD_SRI);
     Ptr<RgbdNormals> ptr;
     switch (method)
     {
@@ -802,10 +888,11 @@ Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K
     }
     case (RGBD_NORMALS_METHOD_LINEMOD):
     {
+        CV_Assert(diffThreshold > 0);
         if (depth == CV_32F)
-            ptr = makePtr<LINEMOD<float> >(rows, cols, windowSize, mK);
+            ptr = makePtr<LINEMOD<float> >(rows, cols, windowSize, mK, diffThreshold);
         else
-            ptr = makePtr<LINEMOD<double>>(rows, cols, windowSize, mK);
+            ptr = makePtr<LINEMOD<double>>(rows, cols, windowSize, mK, diffThreshold);
         break;
     }
     case RGBD_NORMALS_METHOD_SRI:
@@ -816,6 +903,16 @@ Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K
             ptr = makePtr<SRI<double>>(rows, cols, windowSize, mK);
         break;
     }
+    case RGBD_NORMALS_METHOD_CROSS_PRODUCT:
+    {
+        if (depth == CV_32F)
+            ptr = makePtr<CrossProduct<float> >(rows, cols, windowSize, mK);
+        else
+            ptr = makePtr<CrossProduct<double>>(rows, cols, windowSize, mK);
+        break;
+    }
+    default:
+        CV_Error(Error::StsBadArg, "Unknown normals compute algorithm");
     }
 
     return ptr;