diff --git a/modules/3d/include/opencv2/3d/depth.hpp b/modules/3d/include/opencv2/3d/depth.hpp index 0279a26f03..54b5853232 100644 --- a/modules/3d/include/opencv2/3d/depth.hpp +++ b/modules/3d/include/opencv2/3d/depth.hpp @@ -30,7 +30,8 @@ public: { 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 @@ public: * @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 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 @@ public: 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 diff --git a/modules/3d/src/rgbd/depth_to_3d.cpp b/modules/3d/src/rgbd/depth_to_3d.cpp index ec7545362f..48c48f322e 100644 --- a/modules/3d/src/rgbd/depth_to_3d.cpp +++ b/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); } diff --git a/modules/3d/src/rgbd/normal.cpp b/modules/3d/src/rgbd/normal.cpp index 886c953f59..69e062bfb0 100644 --- a/modules/3d/src/rgbd/normal.cpp +++ b/modules/3d/src/rgbd/normal.cpp @@ -225,24 +225,7 @@ public: { 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(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 @@ public: 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(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 @@ public: } } - 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_ V_; mutable Mat_ M_inv_; @@ -448,14 +461,17 @@ public: typedef Vec Vec3T; typedef Matx Mat33T; - LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K) : - RgbdNormalsImpl(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD) + LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K, float _diffThr = 50.f) : + RgbdNormalsImpl(_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 @@ public: 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::value ? 1000.f : 1.f); normals.setTo(std::numeric_limits::quiet_NaN()); for (int y = r; y < this->rows - r - 1; ++y) { @@ -591,14 +609,7 @@ public: 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 @@ public: 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_ 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))); - for (unsigned char i = 0; i < 3; ++i) - mat(i, 1) = mat(i, 1) / std::cos(phi); - // 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); + Matx mat = Matx (0, 1, 0, 0, 0, 1, 1, 0, 0) * + Matx (theta_cos, -theta_sin, 0, theta_sin, theta_cos, 0, 0, 0, 1) * + Matx (phi_cos, 0, -phi_sin, 0, 1, 0, phi_sin, 0, phi_cos); - R_hat_(phi_int, theta_int) = Vec9T((T*)(mat.data)); + for (unsigned char i = 0; i < 3; ++i) + 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 * 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(mat.val); } } @@ -747,8 +761,8 @@ public: 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 @@ public: 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_ R_hat_; @@ -781,14 +790,91 @@ public: //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -Ptr 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 +class CrossProduct : public RgbdNormalsImpl +{ +public: + typedef Vec Vec3T; + typedef Vec Vec4T; + typedef Point3_ Point3T; + + CrossProduct(int _rows, int _cols, int _windowSize, const Mat& _K) : + RgbdNormalsImpl(_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(y); + const Vec4T* ptsRow1 = (y < this->rows - 1) ? points.ptr(y + 1) : nullptr; + Vec4T *normRow = normals.ptr(y); + + for (int x = 0; x < this->cols; x++) + { + Point3T v00 = fromVec(ptsRow0[x]); + const float qnan = std::numeric_limits::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::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 ptr; switch (method) { @@ -802,10 +888,11 @@ Ptr 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 >(rows, cols, windowSize, mK); + ptr = makePtr >(rows, cols, windowSize, mK, diffThreshold); else - ptr = makePtr>(rows, cols, windowSize, mK); + ptr = makePtr>(rows, cols, windowSize, mK, diffThreshold); break; } case RGBD_NORMALS_METHOD_SRI: @@ -816,6 +903,16 @@ Ptr RgbdNormals::create(int rows, int cols, int depth, InputArray K ptr = makePtr>(rows, cols, windowSize, mK); break; } + case RGBD_NORMALS_METHOD_CROSS_PRODUCT: + { + if (depth == CV_32F) + ptr = makePtr >(rows, cols, windowSize, mK); + else + ptr = makePtr>(rows, cols, windowSize, mK); + break; + } + default: + CV_Error(Error::StsBadArg, "Unknown normals compute algorithm"); } return ptr; diff --git a/modules/3d/test/test_normal.cpp b/modules/3d/test/test_normal.cpp index af176a5a42..0ef419f26a 100644 --- a/modules/3d/test/test_normal.cpp +++ b/modules/3d/test/test_normal.cpp @@ -4,40 +4,10 @@ #include "test_precomp.hpp" #include +#include namespace opencv_test { namespace { -#if 0 -Point3f -rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const Mat_& Kinv) -{ - Matx33d dKinv(Kinv); - Vec3d dNormal(normal); - return rayPlaneIntersection(Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv); -} -#endif - -Vec4f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec4d& normal, const Matx33d& Kinv) -{ - Matx31d L = Kinv * uv1; //a ray passing through camera optical center - //and uv. - L = L * (1.0 / cv::norm(L)); - double LdotNormal = L.dot(Vec3d(normal[0], normal[1], normal[2])); - double d; - if (std::fabs(LdotNormal) > 1e-9) - { - d = centroid_dot_normal / LdotNormal; - } - else - { - d = 1.0; - std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl; - std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl; - } - Vec4f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)), 0); - return xyz; -} - const int W = 640; const int H = 480; //int window_size = 5; @@ -63,61 +33,104 @@ void points3dToDepth16U(const Mat_& points3d, Mat& depthMap) Vec3f T(0.0, 0.0, 0.0); cv::projectPoints(points3dvec, R, T, K, Mat(), img_points); + float maxv = 0.f; int index = 0; for (int i = 0; i < H; i++) { for (int j = 0; j < W; j++) { - float value = (points3d.at(i, j))[2]; // value is the z + float value = (points3d(i, j))[2]; // value is the z depthMap.at(cvRound(img_points[index].y), cvRound(img_points[index].x)) = value; + maxv = std::max(maxv, value); index++; } } - depthMap.convertTo(depthMap, CV_16U, 1000); + + double scale = ((1 << 16) - 1) / maxv; + depthMap.convertTo(depthMap, CV_16U, scale); } -static RNG rng; + struct Plane { - Vec4d n, p; - double p_dot_n; - Plane() +public: + Vec4d nd; + + Plane() : nd(1, 0, 0, 0) { } + + static Plane generate(RNG& rng) { - n[0] = rng.uniform(-0.5, 0.5); - n[1] = rng.uniform(-0.5, 0.5); - n[2] = -0.3; //rng.uniform(-1.f, 0.5f); - n[3] = 0.; - n = n / cv::norm(n); - set_d((float)rng.uniform(-2.0, 0.6)); + // Gaussian 3D distribution is separable and spherically symmetrical + // Being normalized, its points represent uniformly distributed points on a sphere (i.e. normal directions) + double sigma = 1.0; + Vec3d ngauss; + ngauss[0] = rng.gaussian(sigma); + ngauss[1] = rng.gaussian(sigma); + ngauss[2] = rng.gaussian(sigma); + ngauss = ngauss * (1.0 / cv::norm(ngauss)); + + double d = rng.uniform(-2.0, 2.0); + Plane p; + p.nd = Vec4d(ngauss[0], ngauss[1], ngauss[2], d); + return p; } - void - set_d(float d) + Vec3d pixelIntersection(double u, double v, const Matx33d& K_inv) { - p = Vec4d(0, 0, d / n[2], 0); - p_dot_n = p.dot(n); - } + Vec3d uv1(u, v, 1); + // pixel reprojected to camera space + Matx31d pspace = K_inv * uv1; - Vec4f - intersection(float u, float v, const Matx33f& Kinv_in) const - { - return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in); + double d = this->nd[3]; + double dotp = pspace.ddot({this->nd[0], this->nd[1], this->nd[2]}); + double d_over_dotp = d / dotp; + if (std::fabs(dotp) <= 1e-9) + { + d_over_dotp = 1.0; + CV_LOG_INFO(NULL, "warning, dotp nearly 0! " << dotp); + } + + Matx31d pmeet = pspace * (- d_over_dotp); + return {pmeet(0, 0), pmeet(1, 0), pmeet(2, 0)}; } }; void gen_points_3d(std::vector& planes_out, Mat_ &plane_mask, Mat& points3d, Mat& normals, - int n_planes) + int n_planes, float scale, RNG& rng) { + const double minGoodZ = 0.0001; + const double maxGoodZ = 1000.0; + std::vector planes; for (int i = 0; i < n_planes; i++) { - Plane px; - for (int j = 0; j < 1; j++) + bool found = false; + for (int j = 0; j < 100; j++) { - px.set_d(rng.uniform(-3.f, -0.5f)); - planes.push_back(px); + Plane px = Plane::generate(rng); + + // Check that area corners have good z values + // So that they won't break rendering + double x0 = double(i) * double(W) / double(n_planes); + double x1 = double(i+1) * double(W) / double(n_planes); + std::vector corners = {{x0, 0}, {x0, H - 1}, {x1, 0}, {x1, H - 1}}; + double minz = std::numeric_limits::max(); + double maxz = 0.0; + for (auto p : corners) + { + Vec3d v = px.pixelIntersection(p.x, p.y, Kinv); + minz = std::min(minz, v[2]); + maxz = std::max(maxz, v[2]); + } + if (minz > minGoodZ && maxz < maxGoodZ) + { + planes.push_back(px); + found = true; + break; + } } + ASSERT_TRUE(found) << "Failed to generate proper random plane" << std::endl; } Mat_ < Vec4f > outp(H, W); Mat_ < Vec4f > outn(H, W); @@ -134,8 +147,9 @@ void gen_points_3d(std::vector& planes_out, Mat_ &plane_ma { unsigned int plane_index = (unsigned int)((u / float(W)) * planes.size()); Plane plane = planes[plane_index]; - outp(v, u) = plane.intersection((float)u, (float)v, Kinv); - outn(v, u) = plane.n; + Vec3f pt = Vec3f(plane.pixelIntersection((double)u, (double)v, Kinv) * scale); + outp(v, u) = {pt[0], pt[1], pt[2], 0}; + outn(v, u) = {(float)plane.nd[0], (float)plane.nd[1], (float)plane.nd[2], 0}; plane_mask(v, u) = (uchar)plane_index; } } @@ -146,269 +160,555 @@ void gen_points_3d(std::vector& planes_out, Mat_ &plane_ma //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -class RgbdNormalsTest +CV_ENUM(NormalComputers, RgbdNormals::RGBD_NORMALS_METHOD_FALS, + RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, + RgbdNormals::RGBD_NORMALS_METHOD_SRI, + RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT); +typedef std::tuple NormalsTestData; +typedef std::tuple NormalsTestParams; + +const double threshold3d1d = 1e-12; +// Right angle is the maximum angle possible between two normals +const double hpi = CV_PI / 2.0; +const int nTestCasesNormals = 5; + +class NormalsRandomPlanes : public ::testing::TestWithParam { -public: - RgbdNormalsTest() { } - ~RgbdNormalsTest() { } - - void run() +protected: + void SetUp() override { - Mat_ plane_mask; - for (unsigned char i = 0; i < 3; ++i) - { - RgbdNormals::RgbdNormalsMethod method = RgbdNormals::RGBD_NORMALS_METHOD_FALS;; - // inner vector: whether it's 1 plane or 3 planes - // outer vector: float or double - std::vector > errors(2); - errors[0].resize(4); - errors[1].resize(4); - switch (i) - { - case 0: - method = RgbdNormals::RGBD_NORMALS_METHOD_FALS; - CV_LOG_INFO(NULL, "*** FALS"); - errors[0][0] = 0.006f; - errors[0][1] = 0.03f; - errors[1][0] = 0.0001f; - errors[1][1] = 0.02f; - break; - case 1: - method = RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD; - CV_LOG_INFO(NULL, "*** LINEMOD"); - errors[0][0] = 0.04f; - errors[0][1] = 0.07f; - errors[0][2] = 0.04f; // depth 16U 1 plane - errors[0][3] = 0.07f; // depth 16U 3 planes + p = GetParam(); + depth = std::get<0>(std::get<0>(p)); + alg = static_cast(int(std::get<1>(std::get<0>(p)))); + scale = std::get<2>(std::get<0>(p)); + int idx = std::get<1>(p); - errors[1][0] = 0.05f; - errors[1][1] = 0.08f; - errors[1][2] = 0.05f; // depth 16U 1 plane - errors[1][3] = 0.08f; // depth 16U 3 planes - break; - case 2: - method = RgbdNormals::RGBD_NORMALS_METHOD_SRI; - CV_LOG_INFO(NULL, "*** SRI"); - errors[0][0] = 0.02f; - errors[0][1] = 0.04f; - errors[1][0] = 0.02f; - errors[1][1] = 0.04f; - break; - } + rng = cvtest::TS::ptr()->get_rng(); + rng.state += idx + nTestCasesNormals*int(scale) + alg*16 + depth*64; - for (unsigned char j = 0; j < 2; ++j) - { - int depth = (j % 2 == 0) ? CV_32F : CV_64F; - if (depth == CV_32F) - { - CV_LOG_INFO(NULL, " * float"); - } - else - { - CV_LOG_INFO(NULL, " * double"); - } - - Ptr normals_computer = RgbdNormals::create(H, W, depth, K, 5, method); - normals_computer->cache(); - - std::vector plane_params; - Mat points3d, ground_normals; - // 1 plane, continuous scene, very low error.. - CV_LOG_INFO(NULL, "1 plane - input 3d points"); - float err_mean = 0; - for (int ii = 0; ii < 5; ++ii) - { - gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1); - err_mean += testit(points3d, ground_normals, normals_computer); - } - CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5)); - EXPECT_LE(err_mean / 5, errors[j][0]); - - // 3 discontinuities, more error expected. - CV_LOG_INFO(NULL, "3 planes"); - err_mean = 0; - for (int ii = 0; ii < 5; ++ii) - { - gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3); - err_mean += testit(points3d, ground_normals, normals_computer); - } - CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5)); - EXPECT_LE(err_mean / 5, errors[j][1]); - - if (method == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD) - { - // depth 16U test - CV_LOG_INFO(NULL, "** depth 16U - 1 plane"); - err_mean = 0; - for (int ii = 0; ii < 5; ++ii) - { - gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1); - Mat depthMap; - points3dToDepth16U(points3d, depthMap); - err_mean += testit(depthMap, ground_normals, normals_computer); - } - CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5)); - EXPECT_LE(err_mean / 5, errors[j][2]); - - CV_LOG_INFO(NULL, "** depth 16U - 3 plane"); - err_mean = 0; - for (int ii = 0; ii < 5; ++ii) - { - gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3); - Mat depthMap; - points3dToDepth16U(points3d, depthMap); - err_mean += testit(depthMap, ground_normals, normals_computer); - } - CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5)); - EXPECT_LE(err_mean / 5, errors[j][3]); - } - } - } - - //TODO test NaNs in data + float diffThreshold = scale ? 100000.f : 50.f; + normalsComputer = RgbdNormals::create(H, W, depth, K, 5, diffThreshold, alg); + normalsComputer->cache(); } - float testit(const Mat& points3d, const Mat& in_ground_normals, const Ptr& normals_computer) + struct NormalsCompareResult { - TickMeter tm; - tm.start(); - Mat in_normals; - if (normals_computer->getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && points3d.channels() == 3) - { - std::vector channels; - split(points3d, channels); - normals_computer->apply(channels[2], in_normals); - } - else - normals_computer->apply(points3d, in_normals); - tm.stop(); + double meanErr; + double maxErr; + }; - Mat_ normals, ground_normals; - in_normals.convertTo(normals, CV_32FC4); - in_ground_normals.convertTo(ground_normals, CV_32FC4); - - float err = 0; + static NormalsCompareResult checkNormals(Mat_ normals, Mat_ ground_normals) + { + double meanErr = 0, maxErr = 0; for (int y = 0; y < normals.rows; ++y) + { for (int x = 0; x < normals.cols; ++x) { Vec4f vec1 = normals(y, x), vec2 = ground_normals(y, x); vec1 = vec1 / cv::norm(vec1); vec2 = vec2 / cv::norm(vec2); - float dot = vec1.dot(vec2); + double dot = vec1.ddot(vec2); + // Just for rounding errors + double err = std::abs(dot) < 1.0 ? std::min(std::acos(dot), std::acos(-dot)) : 0.0; + meanErr += err; + maxErr = std::max(maxErr, err); + } + } + meanErr /= normals.rows * normals.cols; + return { meanErr, maxErr }; + } + + void runCase(bool scaleUp, int nPlanes, bool makeDepth, + double meanThreshold, double maxThreshold, double threshold3d) + { + std::vector plane_params; + Mat_ plane_mask; + Mat points3d, ground_normals; + + gen_points_3d(plane_params, plane_mask, points3d, ground_normals, nPlanes, scaleUp ? 5000.f : 1.f, rng); + + Mat in; + if (makeDepth) + { + points3dToDepth16U(points3d, in); + } + else + { + in = points3d; + } + + TickMeter tm; + tm.start(); + Mat in_normals, normals3d; + //TODO: check other methods when 16U input is implemented for them + if (normalsComputer->getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && in.channels() == 3) + { + std::vector channels; + split(in, channels); + normalsComputer->apply(channels[2], in_normals); + + normalsComputer->apply(in, normals3d); + } + else + normalsComputer->apply(in, in_normals); + tm.stop(); + + CV_LOG_INFO(NULL, "Speed: " << tm.getTimeMilli() << " ms"); + + Mat_ normals; + in_normals.convertTo(normals, CV_32FC4); + + NormalsCompareResult res = checkNormals(normals, ground_normals); + double err3d = 0.0; + if (!normals3d.empty()) + { + Mat_ cvtNormals3d; + normals3d.convertTo(cvtNormals3d, CV_32FC4); + err3d = checkNormals(cvtNormals3d, ground_normals).maxErr; + } + + EXPECT_LE(res.meanErr, meanThreshold); + EXPECT_LE(res.maxErr, maxThreshold); + EXPECT_LE(err3d, threshold3d); + } + + NormalsTestParams p; + int depth; + RgbdNormals::RgbdNormalsMethod alg; + bool scale; + + RNG rng; + Ptr normalsComputer; +}; + +//TODO Test NaNs in data + +TEST_P(NormalsRandomPlanes, check1plane) +{ + double meanErr = std::get<3>(std::get<0>(p)); + double maxErr = std::get<4>(std::get<0>(p)); + + // 1 plane, continuous scene, very low error.. + runCase(scale, 1, false, meanErr, maxErr, threshold3d1d); +} + +TEST_P(NormalsRandomPlanes, check3planes) +{ + double meanErr = std::get<5>(std::get<0>(p)); + double maxErr = hpi; + + // 3 discontinuities, more error expected + runCase(scale, 3, false, meanErr, maxErr, threshold3d1d); +} + +TEST_P(NormalsRandomPlanes, check1plane16u) +{ + // TODO: check other algos as soon as they support 16U depth inputs + if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && scale) + { + double meanErr = std::get<6>(std::get<0>(p)); + double maxErr = hpi; + + runCase(false, 1, true, meanErr, maxErr, threshold3d1d); + } + else + { + throw SkipTestException("Not implemented for anything except LINEMOD with scale"); + } +} + +TEST_P(NormalsRandomPlanes, check3planes16u) +{ + // TODO: check other algos as soon as they support 16U depth inputs + if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && scale) + { + double meanErr = std::get<7>(std::get<0>(p)); + double maxErr = hpi; + + runCase(false, 3, true, meanErr, maxErr, threshold3d1d); + } + else + { + throw SkipTestException("Not implemented for anything except LINEMOD with scale"); + } +} + +INSTANTIATE_TEST_CASE_P(RGBD_Normals, NormalsRandomPlanes, +::testing::Combine(::testing::Values( + // 3 normal computer params + 5 thresholds: + //depth, alg, scale, 1plane mean, 1plane max, 3planes mean, 1plane16u mean, 3planes16 mean + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, true, 0.00362, 0.08881, 0.02175, 0, 0}, + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, false, 0.00374, 0.10309, 0.01902, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, true, 0.00023, 0.00037, 0.01805, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, false, 0.00023, 0.00037, 0.01805, 0, 0}, + + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, true, 0.00186, 0.08974, 0.04528, 0.21220, 0.17314}, + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, false, 0.00157, 0.01225, 0.04528, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, true, 0.00160, 0.06526, 0.04371, 0.28837, 0.28918}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, false, 0.00154, 0.06877, 0.04323, 0, 0}, + + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, true, 0.01987, hpi, 0.03463, 0, 0}, + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01962, hpi, 0.03546, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, true, 0.01958, hpi, 0.03546, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01995, hpi, 0.03474, 0, 0}, + + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, true, 0.000230, 0.00038, 0.00450, 0, 0}, + NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, false, 0.000230, 0.00038, 0.00478, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, true, 0.000221, 0.00038, 0.00469, 0, 0}, + NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, false, 0.000238, 0.00038, 0.00477, 0, 0} +), ::testing::Range(0, nTestCasesNormals))); + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +typedef std::tuple> NormalComputerThresholds; +struct RenderedNormals: public ::testing::TestWithParam> +{ + static Mat readYaml(std::string fname) + { + Mat img; + FileStorage fs(fname, FileStorage::Mode::READ); + if (fs.isOpened() && fs.getFirstTopLevelNode().name() == "testImg") + { + fs["testImg"] >> img; + } + return img; + }; + + static Mat nanMask(Mat img) + { + int depth = img.depth(); + Mat mask(img.size(), CV_8U); + for (int y = 0; y < img.rows; y++) + { + uchar* maskRow = mask.ptr(y); + if (depth == CV_32F) + { + Vec3f *imgrow = img.ptr(y); + for (int x = 0; x < img.cols; x++) + { + maskRow[x] = (imgrow[x] == imgrow[x])*255; + } + } + else if (depth == CV_64F) + { + Vec3d *imgrow = img.ptr(y); + for (int x = 0; x < img.cols; x++) + { + maskRow[x] = (imgrow[x] == imgrow[x])*255; + } + } + } + return mask; + } + + template + static Mat flipAxesT(Mat pts, int flip) + { + Mat flipped(pts.size(), pts.type()); + for (int y = 0; y < pts.rows; y++) + { + VT *inrow = pts.ptr(y); + VT *outrow = flipped.ptr(y); + for (int x = 0; x < pts.cols; x++) + { + VT n = inrow[x]; + n[0] = (flip & FLIP_X) ? -n[0] : n[0]; + n[1] = (flip & FLIP_Y) ? -n[1] : n[1]; + n[2] = (flip & FLIP_Z) ? -n[2] : n[2]; + outrow[x] = n; + } + } + return flipped; + } + + static const int FLIP_X = 1; + static const int FLIP_Y = 2; + static const int FLIP_Z = 4; + static Mat flipAxes(Mat pts, int flip) + { + int depth = pts.depth(); + if (depth == CV_32F) + { + return flipAxesT(pts, flip); + } + else if (depth == CV_64F) + { + return flipAxesT(pts, flip); + } + else + { + return Mat(); + } + } + + template + static Mat_ normalsErrorT(Mat_ srcNormals, Mat_ dstNormals) + { + typedef typename VT::value_type Val; + Mat out(srcNormals.size(), cv::traits::Depth::value, Scalar(0)); + for (int y = 0; y < srcNormals.rows; y++) + { + + VT *srcrow = srcNormals[y]; + VT *dstrow = dstNormals[y]; + Val *outrow = out.ptr(y); + for (int x = 0; x < srcNormals.cols; x++) + { + VT sn = srcrow[x]; + VT dn = dstrow[x]; + + Val dot = sn.dot(dn); + Val v(0.0); // Just for rounding errors if (std::abs(dot) < 1) - err += std::min(std::acos(dot), std::acos(-dot)); - } + v = std::min(std::acos(dot), std::acos(-dot)); - err /= normals.rows * normals.cols; - CV_LOG_INFO(NULL, "Average error: " << err << " Speed: " << tm.getTimeMilli() << " ms"); - return err; + outrow[x] = v; + } + } + return out; + } + + static Mat normalsError(Mat srcNormals, Mat dstNormals) + { + int depth = srcNormals.depth(); + int channels = srcNormals.channels(); + + if (depth == CV_32F) + { + if (channels == 3) + { + return normalsErrorT(srcNormals, dstNormals); + } + else if (channels == 4) + { + return normalsErrorT(srcNormals, dstNormals); + } + } + else if (depth == CV_64F) + { + if (channels == 3) + { + return normalsErrorT(srcNormals, dstNormals); + } + else if (channels == 4) + { + return normalsErrorT(srcNormals, dstNormals); + } + } + else + { + CV_Error(Error::StsInternal, "This type is unsupported"); + } + return Mat(); } }; + +TEST_P(RenderedNormals, check) +{ + auto p = GetParam(); + int depth = std::get<0>(p); + auto alg = static_cast(int(std::get<0>(std::get<1>(p)))); + bool scale = std::get<2>(p); + + std::string dataPath = cvtest::TS::ptr()->get_data_path(); + // The depth rendered from scene OPENCV_TEST_DATA_PATH + "/cv/rgbd/normals_check/normals_scene.blend" + std::string srcDepthFilename = dataPath + "/cv/rgbd/normals_check/depth.yaml.gz"; + std::string srcNormalsFilename = dataPath + "/cv/rgbd/normals_check/normals%d.yaml.gz"; + Mat srcDepth = readYaml(srcDepthFilename); + + ASSERT_FALSE(srcDepth.empty()) << "Failed to load depth data"; + + Size depthSize = srcDepth.size(); + + Mat srcNormals; + std::array srcNormalsCh; + for (int i = 0; i < 3; i++) + { + Mat m = readYaml(cv::format(srcNormalsFilename.c_str(), i)); + + ASSERT_FALSE(m.empty()) << "Failed to load normals data"; + + if (depth == CV_64F) + { + Mat c; + m.convertTo(c, CV_64F); + m = c; + } + + srcNormalsCh[i] = m; + } + cv::merge(srcNormalsCh, srcNormals); + + // Convert saved normals from [0; 1] range to [-1; 1] + srcNormals = srcNormals * 2.0 - 1.0; + + // Data obtained from Blender scene + Matx33f intr(666.6667f, 0.f, 320.f, + 0.f, 666.6667f, 240.f, + 0.f, 0.f, 1.f); + // Inverted camera rotation + Matx33d rotm = cv::Quatd(0.7805, 0.4835, 0.2087, 0.3369).conjugate().toRotMat3x3(); + cv::transform(srcNormals, srcNormals, rotm); + + Mat srcMask = srcDepth > 0; + + float diffThreshold = 50.f; + if (scale) + { + srcDepth = srcDepth * 5000.0; + diffThreshold = 100000.f; + } + + Mat srcCloud; + // The function with mask produces 1x(w*h) vector, this is not what we need + // depthTo3d(srcDepth, intr, srcCloud, srcMask); + depthTo3d(srcDepth, intr, srcCloud); + Scalar qnan = Scalar::all(std::numeric_limits::quiet_NaN()); + srcCloud.setTo(qnan, ~srcMask); + srcDepth.setTo(qnan, ~srcMask); + + // For further result comparison + srcNormals.setTo(qnan, ~srcMask); + + Ptr normalsComputer = RgbdNormals::create(depthSize.height, depthSize.width, depth, intr, 5, diffThreshold, alg); + normalsComputer->cache(); + + Mat dstNormals, dstNormalsOrig, dstNormalsDepth; + normalsComputer->apply(srcCloud, dstNormals); + //TODO: add for other methods too when it's implemented + if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD) + { + normalsComputer->apply(srcDepth, dstNormalsDepth); + dstNormalsOrig = dstNormals.clone(); + } + + // Remove 4th channel from dstNormals + Mat newDstNormals; + std::vector dstNormalsCh; + split(dstNormals, dstNormalsCh); + dstNormalsCh.resize(3); + merge(dstNormalsCh, newDstNormals); + dstNormals = newDstNormals; + + Mat dstMask = nanMask(dstNormals); + // div by 8 because uchar is 8-bit + double maskl2 = cv::norm(dstMask, srcMask, NORM_HAMMING) / 8; + + // Flipping Y and Z to correspond to srcNormals + Mat flipped = flipAxes(dstNormals, FLIP_Y | FLIP_Z); + dstNormals = flipped; + + Mat absdot = normalsError(srcNormals, dstNormals); + + Mat cmpMask = srcMask & dstMask; + + EXPECT_GT(countNonZero(cmpMask), 0); + + double nrml2 = cv::norm(absdot, NORM_L2, cmpMask); + + if (!dstNormalsDepth.empty()) + { + Mat abs3d = normalsError(dstNormalsOrig, dstNormalsDepth); + double errInf = cv::norm(abs3d, NORM_INF, cmpMask); + double errL2 = cv::norm(abs3d, NORM_L2, cmpMask); + EXPECT_LE(errInf, 0.00085); + EXPECT_LE(errL2, 0.07718); + } + + auto th = std::get<1>(std::get<1>(p)); + EXPECT_LE(nrml2, th.first); + EXPECT_LE(maskl2, th.second); +} + +INSTANTIATE_TEST_CASE_P(RGBD_Normals, RenderedNormals, ::testing::Combine(::testing::Values(CV_32F, CV_64F), + ::testing::Values( + NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_FALS, { 81.8210, 0}}, + NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, { 107.2710, 29168}}, + NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_SRI, { 73.2027, 17693}}, + NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, { 57.9832, 2531}}), + ::testing::Values(true, false))); + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -class RgbdPlaneTest +class RgbdPlaneGenerate : public ::testing::TestWithParam> { -public: - RgbdPlaneTest() { } - ~RgbdPlaneTest() { } - - void run() +protected: + void SetUp() override { - std::vector planes; - Mat points3d, ground_normals; - Mat_ plane_mask; - gen_points_3d(planes, plane_mask, points3d, ground_normals, 1); - testit(planes, plane_mask, points3d); // 1 plane, continuous scene, very low error.. - for (int ii = 0; ii < 10; ii++) - { - gen_points_3d(planes, plane_mask, points3d, ground_normals, 3); //three planes - testit(planes, plane_mask, points3d); // 3 discontinuities, more error expected. - } + auto p = GetParam(); + idx = std::get<0>(p); + checkNormals = std::get<1>(p); + nPlanes = std::get<2>(p); } - void testit(const std::vector& gt_planes, const Mat& gt_plane_mask, const Mat& points3d) - { - for (char i_test = 0; i_test < 2; ++i_test) - { - TickMeter tm1, tm2; - Mat plane_mask; - std::vector plane_coefficients; - - if (i_test == 0) - { - tm1.start(); - // First, get the normals - int depth = CV_32F; - Ptr normals_computer = RgbdNormals::create(H, W, depth, K, 5, RgbdNormals::RGBD_NORMALS_METHOD_FALS); - Mat normals; - normals_computer->apply(points3d, normals); - tm1.stop(); - - tm2.start(); - findPlanes(points3d, normals, plane_mask, plane_coefficients); - tm2.stop(); - } - else - { - tm2.start(); - findPlanes(points3d, noArray(), plane_mask, plane_coefficients); - tm2.stop(); - } - - // Compare each found plane to each ground truth plane - int n_planes = (int)plane_coefficients.size(); - int n_gt_planes = (int)gt_planes.size(); - Mat_ matching(n_gt_planes, n_planes); - for (int j = 0; j < n_gt_planes; ++j) - { - Mat gt_mask = gt_plane_mask == j; - int n_gt = countNonZero(gt_mask); - int n_max = 0, i_max = 0; - for (int i = 0; i < n_planes; ++i) - { - Mat dst; - bitwise_and(gt_mask, plane_mask == i, dst); - matching(j, i) = countNonZero(dst); - if (matching(j, i) > n_max) - { - n_max = matching(j, i); - i_max = i; - } - } - // Get the best match - ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001); - // Compare the normals - Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]); - Vec4d n = gt_planes[j].n; - ASSERT_GE(std::abs(Vec3d(n[0], n[1], n[2]).dot(normal)), 0.95); - } - - CV_LOG_INFO(NULL, "Speed: "); - if (i_test == 0) - CV_LOG_INFO(NULL, "normals " << tm1.getTimeMilli() << " ms and "); - CV_LOG_INFO(NULL, "plane " << tm2.getTimeMilli() << " ms"); - } - } + int idx; + bool checkNormals; + int nPlanes; }; -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -TEST(RGBD_Normals, compute) +TEST_P(RgbdPlaneGenerate, compute) { - RgbdNormalsTest test; - test.run(); + RNG &rng = cvtest::TS::ptr()->get_rng(); + rng.state += idx; + + std::vector planes; + Mat points3d, ground_normals; + Mat_ gt_plane_mask; + gen_points_3d(planes, gt_plane_mask, points3d, ground_normals, nPlanes, 1.f, rng); + + Mat plane_mask; + std::vector plane_coefficients; + + Mat normals; + if (checkNormals) + { + // First, get the normals + int depth = CV_32F; + Ptr normalsComputer = RgbdNormals::create(H, W, depth, K, 5, 50.f, RgbdNormals::RGBD_NORMALS_METHOD_FALS); + normalsComputer->apply(points3d, normals); + } + + findPlanes(points3d, normals, plane_mask, plane_coefficients); + + // Compare each found plane to each ground truth plane + int n_planes = (int)plane_coefficients.size(); + int n_gt_planes = (int)planes.size(); + Mat_ matching(n_gt_planes, n_planes); + for (int j = 0; j < n_gt_planes; ++j) + { + Mat gt_mask = gt_plane_mask == j; + int n_gt = countNonZero(gt_mask); + int n_max = 0, i_max = 0; + for (int i = 0; i < n_planes; ++i) + { + Mat dst; + bitwise_and(gt_mask, plane_mask == i, dst); + matching(j, i) = countNonZero(dst); + if (matching(j, i) > n_max) + { + n_max = matching(j, i); + i_max = i; + } + } + // Get the best match + ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001); + // Compare the normals + Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]); + Vec4d nd = planes[j].nd; + ASSERT_GE(std::abs(Vec3d(nd[0], nd[1], nd[2]).dot(normal)), 0.95); + } } -TEST(RGBD_Plane, compute) -{ - RgbdPlaneTest test; - test.run(); -} +// 1 plane, continuous scene, very low error +// 3 planes, 3 discontinuities, more error expected +INSTANTIATE_TEST_CASE_P(RGBD_Plane, RgbdPlaneGenerate, ::testing::Combine(::testing::Range(0, 10), + ::testing::Values(false, true), + ::testing::Values(1, 3))); -TEST(RGBD_Plane, regression_2309_valgrind_check) +TEST(RGBD_Plane, regression2309ValgrindCheck) { Mat points(640, 480, CV_32FC3, Scalar::all(0)); // Note, 640%9 is 1 and 480%9 is 3 diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index bd9fa38d35..d1193dbdd8 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -627,4 +627,31 @@ TEST(RGBD_Odometry_WarpFrame, bigScale) ASSERT_LE(lidiff, 0.99951172); } +TEST(RGBD_DepthTo3D, mask) +{ + std::string dataPath = cvtest::TS::ptr()->get_data_path(); + std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png"; + + Mat srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED); + ASSERT_FALSE(srcDepth.empty()) << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl; + ASSERT_TRUE(srcDepth.type() == CV_16UC1); + + Mat srcMask = srcDepth > 0; + + // test data used to generate warped depth and rgb + // the script used to generate is in opencv_extra repo + // at testdata/cv/rgbd/warped_depth_generator/warp_test.py + double fx = 525.0, fy = 525.0, + cx = 319.5, cy = 239.5; + Matx33d intr(fx, 0, cx, + 0, fy, cy, + 0, 0, 1); + + Mat srcCloud; + depthTo3d(srcDepth, intr, srcCloud, srcMask); + size_t npts = countNonZero(srcMask); + + ASSERT_EQ(npts, srcCloud.total()); +} + }} // namespace