From f286612607dded65ff9a5951985abfa8a7e77886 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 2 Feb 2021 15:11:22 +0300 Subject: [PATCH 01/18] add GPU and CPU versions of tests --- modules/rgbd/test/test_tsdf.cpp | 70 ++++++++++++++++++++++++++------- 1 file changed, 56 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 99da9579697..33f036ff888 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -470,44 +470,86 @@ void valid_points_test(bool isHashTSDF) ASSERT_LT(0.5 - percentValidity, 0.3); } -TEST(TSDF, raycast_normals) +#ifndef HAVE_OPENCL + +TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF, valid_points) { valid_points_test(false); } + +TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF, valid_points) { valid_points_test(true); } + +#else + +TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF_GPU, valid_points) { valid_points_test(false); } + +TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } + +TEST(TSDF_CPU, raycast_normals) { + cv::ocl::setUseOpenCL(false); normal_test(false, true, false, false); + cv::ocl::setUseOpenCL(true); } -TEST(HashTSDF, raycast_normals) +TEST(TSDF_CPU, fetch_points_normals) { - normal_test(true, true, false, false); + cv::ocl::setUseOpenCL(false); + normal_test(false, false, true, false); + cv::ocl::setUseOpenCL(true); } -TEST(TSDF, fetch_points_normals) +TEST(TSDF_CPU, fetch_normals) { - normal_test(false, false, true, false); + cv::ocl::setUseOpenCL(false); + normal_test(false, false, false, true); + cv::ocl::setUseOpenCL(true); } -TEST(HashTSDF, fetch_points_normals) +TEST(TSDF_CPU, valid_points) { - normal_test(true, false, true, false); + cv::ocl::setUseOpenCL(false); + valid_points_test(false); + cv::ocl::setUseOpenCL(true); } -TEST(TSDF, fetch_normals) +TEST(HashTSDF_CPU, raycast_normals) { - normal_test(false, false, false, true); + cv::ocl::setUseOpenCL(false); + normal_test(true, true, false, false); + cv::ocl::setUseOpenCL(true); } -TEST(HashTSDF, fetch_normals) +TEST(HashTSDF_CPU, fetch_points_normals) { - normal_test(true, false, false, true); + cv::ocl::setUseOpenCL(false); + normal_test(true, false, true, false); + cv::ocl::setUseOpenCL(true); } -TEST(TSDF, valid_points) +TEST(HashTSDF_CPU, fetch_normals) { - valid_points_test(false); + cv::ocl::setUseOpenCL(false); + normal_test(true, false, false, true); + cv::ocl::setUseOpenCL(true); } -TEST(HashTSDF, valid_points) +TEST(HashTSDF_CPU, valid_points) { + cv::ocl::setUseOpenCL(false); valid_points_test(true); + cv::ocl::setUseOpenCL(true); } +#endif + }} // namespace From 7c5f326dd976b6438798e4cb93e9b463940d5635 Mon Sep 17 00:00:00 2001 From: batters21 Date: Thu, 4 Feb 2021 12:55:28 +0300 Subject: [PATCH 02/18] reduce time for demo show --- modules/rgbd/test/test_tsdf.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 33f036ff888..1afdac85dfd 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -358,7 +358,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo points = _points.getMat(af); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } if (isRaycast) @@ -384,7 +384,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo points = _newPoints.getMat(af); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } } @@ -448,7 +448,7 @@ void valid_points_test(bool isHashTSDF) imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); @@ -463,7 +463,7 @@ void valid_points_test(bool isHashTSDF) imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } float percentValidity = float(profile) / float(anfas); From a54a2ec29e3ddd11b6b7278d9d5cc3bf1b7490c7 Mon Sep 17 00:00:00 2001 From: batters21 Date: Thu, 4 Feb 2021 14:05:48 +0300 Subject: [PATCH 03/18] add assert info and scene minor fix --- modules/rgbd/test/test_tsdf.cpp | 46 ++++++++++++++++++++------------- 1 file changed, 28 insertions(+), 18 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 1afdac85dfd..741fbc43985 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -32,12 +32,13 @@ template struct RenderInvoker : ParallelLoopBody { RenderInvoker(Mat_& _frame, Affine3f _pose, - Reprojector _reproj, - float _depthFactor) : ParallelLoopBody(), + Reprojector _reproj, float _depthFactor, bool _onlySemisphere) + : ParallelLoopBody(), frame(_frame), pose(_pose), reproj(_reproj), - depthFactor(_depthFactor) + depthFactor(_depthFactor), + onlySemisphere(_onlySemisphere) { } virtual void operator ()(const cv::Range& r) const @@ -64,7 +65,7 @@ struct RenderInvoker : ParallelLoopBody for (int step = 0; step < maxSteps && t < maxDepth; step++) { Point3f p = orig + dir * t; - float d = Scene::map(p); + float d = Scene::map(p, onlySemisphere); if (d < 0.000001f) { float depth = std::sqrt(t * t * xyt); @@ -83,12 +84,13 @@ struct RenderInvoker : ParallelLoopBody Affine3f pose; Reprojector reproj; float depthFactor; + bool onlySemisphere; }; struct Scene { virtual ~Scene() {} - static Ptr create(Size sz, Matx33f _intr, float _depthFactor); + static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); virtual Mat depth(Affine3f pose) = 0; virtual std::vector getPoses() = 0; }; @@ -102,12 +104,13 @@ struct SemisphereScene : Scene Size frameSize; Matx33f intr; float depthFactor; + bool onlySemisphere; - SemisphereScene(Size sz, Matx33f _intr, float _depthFactor) : - frameSize(sz), intr(_intr), depthFactor(_depthFactor) + SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) { } - static float map(Point3f p) + static float map(Point3f p, bool onlySemisphere) { float plane = p.y + 0.5f; @@ -129,7 +132,12 @@ struct SemisphereScene : Scene Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; - float res = min({sphereMinusBox, subSphere, plane}); + float res; + if (!onlySemisphere) + res = min({sphereMinusBox, subSphere, plane}); + else + res = sphereMinusBox; + return res; } @@ -139,7 +147,7 @@ struct SemisphereScene : Scene Reprojector reproj(intr); Range range(0, frame.rows); - parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor)); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); return std::move(frame); } @@ -164,9 +172,9 @@ struct SemisphereScene : Scene }; -Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor) +Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) { - return makePtr(sz, _intr, _depthFactor); + return makePtr(sz, _intr, _depthFactor, _onlySemisphere); } // this is a temporary solution @@ -274,7 +282,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = false; +static const bool display = true; static const bool parallelCheck = false; void normalsCheck(Mat normals) @@ -288,7 +296,7 @@ void normalsCheck(Mat normals) float length = vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f); + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; } } } @@ -301,7 +309,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo else _params = kinfu::Params::coarseParams(); - Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor); + Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor, false); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -318,7 +326,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo float length = vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f); + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; } }; @@ -421,7 +429,7 @@ void valid_points_test(bool isHashTSDF) else _params = kinfu::Params::coarseParams(); - Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor); + Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor, true); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -467,7 +475,9 @@ void valid_points_test(bool isHashTSDF) } float percentValidity = float(profile) / float(anfas); - ASSERT_LT(0.5 - percentValidity, 0.3); + ASSERT_NE(profile, 0) << "There is no points in profile"; + ASSERT_NE(anfas, 0) << "There is no points in anfas"; + ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; } #ifndef HAVE_OPENCL From c76942c34b83e0df1e9b8979ffae38d4a5183113 Mon Sep 17 00:00:00 2001 From: batters21 Date: Mon, 8 Feb 2021 17:08:53 +0300 Subject: [PATCH 04/18] add display to perf_test --- modules/rgbd/perf/perf_tsdf.cpp | 136 ++++++++++++++++++++++++++++++++ 1 file changed, 136 insertions(+) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index ebca91587d6..0ca8cbbee8c 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -169,6 +169,111 @@ Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor) return makePtr(sz, _intr, _depthFactor); } +// this is a temporary solution +// ---------------------------- + +typedef cv::Vec4f ptype; +typedef cv::Mat_< ptype > Points; +typedef Points Normals; +typedef Size2i Size; + +template +inline float specPow(float x) +{ + if (p % 2 == 0) + { + float v = specPow

(x); + return v * v; + } + else + { + float v = specPow<(p - 1) / 2>(x); + return v * v * x; + } +} + +template<> +inline float specPow<0>(float /*x*/) +{ + return 1.f; +} + +template<> +inline float specPow<1>(float x) +{ + return x; +} + +inline cv::Vec3f fromPtype(const ptype& x) +{ + return cv::Vec3f(x[0], x[1], x[2]); +} + +inline Point3f normalize(const Vec3f& v) +{ + double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + return v * (nv ? 1. / nv : 0.); +} + +void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose) +{ + Size sz = _points.size(); + image.create(sz, CV_8UC4); + + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + + Mat_ img = image.getMat(); + + Range range(0, sz.height); + const int nstripes = -1; + parallel_for_(range, [&](const Range&) + { + for (int y = range.start; y < range.end; y++) + { + Vec4b* imgRow = img[y]; + const ptype* ptsRow = points[y]; + const ptype* nrmRow = normals[y]; + + for (int x = 0; x < sz.width; x++) + { + Point3f p = fromPtype(ptsRow[x]); + Point3f n = fromPtype(nrmRow[x]); + + Vec4b color; + + if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z) ) + { + color = Vec4b(0, 32, 0, 0); + } + else + { + const float Ka = 0.3f; //ambient coeff + const float Kd = 0.5f; //diffuse coeff + const float Ks = 0.2f; //specular coeff + const int sp = 20; //specular power + + const float Ax = 1.f; //ambient color, can be RGB + const float Dx = 1.f; //diffuse color, can be RGB + const float Sx = 1.f; //specular color, can be RGB + const float Lx = 1.f; //light color + + Point3f l = normalize(lightPose.translation() - Vec3f(p)); + Point3f v = normalize(-Vec3f(p)); + Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l)); + + uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) + + Lx * Ks * Sx * specPow(max(0.f, r.dot(v)))) * 255.f); + color = Vec4b(ix, ix, ix, 0); + } + + imgRow[x] = color; + } + } + }, nstripes); +} +// ---------------------------- + class Settings { public: @@ -193,6 +298,8 @@ class Settings } }; +static const bool display = true; + PERF_TEST(Perf_TSDF, integrate) { Settings settings(false); @@ -213,6 +320,8 @@ PERF_TEST(Perf_TSDF, raycast) for (size_t i = 0; i < settings.poses.size(); i++) { UMat _points, _normals; + Mat points, normals, image; + AccessFlag af = ACCESS_READ; Matx44f pose = settings.poses[i].matrix; Mat depth = settings.scene->depth(pose); @@ -220,6 +329,18 @@ PERF_TEST(Perf_TSDF, raycast) startTimer(); settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); + normals = _normals.getMat(af); + points = _points.getMat(af); + patchNaNs(points); + + if (display) + { + imshow("depth", depth * (1.f / settings._params->depthFactor / 4.f)); + renderPointsNormals(points, normals, image, settings._params->lightPose); + imshow("render", image); + waitKey(2000); + } + } SANITY_CHECK_NOTHING(); } @@ -235,6 +356,8 @@ PERF_TEST(Perf_HashTSDF, integrate) startTimer(); settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr); stopTimer(); + + } SANITY_CHECK_NOTHING(); } @@ -245,6 +368,8 @@ PERF_TEST(Perf_HashTSDF, raycast) for (size_t i = 0; i < settings.poses.size(); i++) { UMat _points, _normals; + Mat points, normals, image; + AccessFlag af = ACCESS_READ; Matx44f pose = settings.poses[i].matrix; Mat depth = settings.scene->depth(pose); @@ -252,6 +377,17 @@ PERF_TEST(Perf_HashTSDF, raycast) startTimer(); settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); + + normals = _normals.getMat(af); + points = _points.getMat(af); + patchNaNs(points); + if (display) + { + imshow("depth", depth * (1.f / settings._params->depthFactor / 4.f)); + renderPointsNormals(points, normals, image, settings._params->lightPose); + imshow("render", image); + waitKey(2000); + } } SANITY_CHECK_NOTHING(); } From f12833a829896c0a8cd862bdc04a03fc74c7ba48 Mon Sep 17 00:00:00 2001 From: batters21 Date: Mon, 8 Feb 2021 17:26:28 +0300 Subject: [PATCH 05/18] replace extra code by function in perf_test --- modules/rgbd/perf/perf_tsdf.cpp | 37 ++++++++++++++------------------- 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index 0ca8cbbee8c..b8663cca430 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -298,6 +298,20 @@ class Settings } }; +void displayImage(Mat depth, UMat _points, UMat _normals, float depthFactor, Vec3f lightPose) +{ + Mat points, normals, image; + AccessFlag af = ACCESS_READ; + normals = _normals.getMat(af); + points = _points.getMat(af); + patchNaNs(points); + + imshow("depth", depth * (1.f / depthFactor / 4.f)); + renderPointsNormals(points, normals, image, lightPose); + imshow("render", image); + waitKey(2000); +} + static const bool display = true; PERF_TEST(Perf_TSDF, integrate) @@ -320,8 +334,6 @@ PERF_TEST(Perf_TSDF, raycast) for (size_t i = 0; i < settings.poses.size(); i++) { UMat _points, _normals; - Mat points, normals, image; - AccessFlag af = ACCESS_READ; Matx44f pose = settings.poses[i].matrix; Mat depth = settings.scene->depth(pose); @@ -329,18 +341,11 @@ PERF_TEST(Perf_TSDF, raycast) startTimer(); settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); - normals = _normals.getMat(af); - points = _points.getMat(af); - patchNaNs(points); if (display) { - imshow("depth", depth * (1.f / settings._params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, settings._params->lightPose); - imshow("render", image); - waitKey(2000); + displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose); } - } SANITY_CHECK_NOTHING(); } @@ -356,8 +361,6 @@ PERF_TEST(Perf_HashTSDF, integrate) startTimer(); settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr); stopTimer(); - - } SANITY_CHECK_NOTHING(); } @@ -368,8 +371,6 @@ PERF_TEST(Perf_HashTSDF, raycast) for (size_t i = 0; i < settings.poses.size(); i++) { UMat _points, _normals; - Mat points, normals, image; - AccessFlag af = ACCESS_READ; Matx44f pose = settings.poses[i].matrix; Mat depth = settings.scene->depth(pose); @@ -378,15 +379,9 @@ PERF_TEST(Perf_HashTSDF, raycast) settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); - normals = _normals.getMat(af); - points = _points.getMat(af); - patchNaNs(points); if (display) { - imshow("depth", depth * (1.f / settings._params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, settings._params->lightPose); - imshow("render", image); - waitKey(2000); + displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose); } } SANITY_CHECK_NOTHING(); From a0c90e8de2c2a3655798900cd9d5b4870ef894d3 Mon Sep 17 00:00:00 2001 From: batters21 Date: Tue, 9 Feb 2021 13:57:24 +0300 Subject: [PATCH 06/18] add display func for tests --- modules/rgbd/test/test_tsdf.cpp | 75 +++++++++++++++------------------ 1 file changed, 34 insertions(+), 41 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 741fbc43985..3ff47403a00 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -285,6 +285,15 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im static const bool display = true; static const bool parallelCheck = false; +void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) +{ + Mat image; + imshow("depth", depth * (1.f / depthFactor / 4.f)); + renderPointsNormals(points, normals, image, lightPose); + imshow("render", image); + waitKey(2000); +} + void normalsCheck(Mat normals) { Vec4f vector; @@ -301,6 +310,27 @@ void normalsCheck(Mat normals) } } +int counterOfValid(Mat points) +{ + Vec4f* v; + int i, j; + int count = 0; + for (i = 0; i < points.rows; ++i) + { + v = (points.ptr(i)); + for (j = 0; j < points.cols; ++j) + { + if ((v[j])[0] != 0 || + (v[j])[1] != 0 || + (v[j])[2] != 0) + { + count++; + } + } + } + return count; +} + void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) { Ptr _params; @@ -316,7 +346,6 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo UMat _points, _normals, _tmpnormals; UMat _newPoints, _newNormals; Mat points, normals; - Mat image; AccessFlag af = ACCESS_READ; auto normalCheck = [](Vec4f& vector, const int*) @@ -362,11 +391,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo if (isRaycast && display) { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - points = _points.getMat(af); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); + displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); } if (isRaycast) @@ -388,11 +413,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo if (display) { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - points = _newPoints.getMat(af); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); + displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); } } @@ -400,27 +421,6 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo points.release(); normals.release(); } -int counterOfValid(Mat points) -{ - Vec4f* v; - int i, j; - int count = 0; - for (i = 0; i < points.rows; ++i) - { - v = (points.ptr(i)); - for (j = 0; j < points.cols; ++j) - { - if ((v[j])[0] != 0 || - (v[j])[1] != 0 || - (v[j])[2] != 0) - { - count++; - } - } - } - return count; -} - void valid_points_test(bool isHashTSDF) { Ptr _params; @@ -436,7 +436,6 @@ void valid_points_test(bool isHashTSDF) UMat _points, _normals; UMat _newPoints, _newNormals; Mat points, normals; - Mat image; int anfas, profile; AccessFlag af = ACCESS_READ; @@ -453,10 +452,7 @@ void valid_points_test(bool isHashTSDF) if (display) { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); + displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); } volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); @@ -468,10 +464,7 @@ void valid_points_test(bool isHashTSDF) if (display) { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); + displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); } float percentValidity = float(profile) / float(anfas); From 8a8a8828f07ecf9ac5f9c8f9bcee974cde1b0eb5 Mon Sep 17 00:00:00 2001 From: batters21 Date: Tue, 9 Feb 2021 14:26:53 +0300 Subject: [PATCH 07/18] add settings class for test --- modules/rgbd/test/test_tsdf.cpp | 83 +++++++++++++++++---------------- 1 file changed, 44 insertions(+), 39 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 3ff47403a00..9de369a82f9 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -285,9 +285,34 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im static const bool display = true; static const bool parallelCheck = false; +class Settings +{ +public: + Ptr params; + Ptr volume; + Ptr scene; + std::vector poses; + + Settings(bool useHashTSDF, bool onlySemisphere) + { + if (useHashTSDF) + params = kinfu::Params::hashTSDFParams(true); + else + params = kinfu::Params::coarseParams(); + + volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, + params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, + params->truncateThreshold, params->volumeDims); + + scene = Scene::create(params->frameSize, params->intr, params->depthFactor, onlySemisphere); + poses = scene->getPoses(); + } +}; + void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) { Mat image; + patchNaNs(points); imshow("depth", depth * (1.f / depthFactor / 4.f)); renderPointsNormals(points, normals, image, lightPose); imshow("render", image); @@ -333,16 +358,9 @@ int counterOfValid(Mat points) void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) { - Ptr _params; - if (isHashTSDF) - _params = kinfu::Params::hashTSDFParams(true); - else - _params = kinfu::Params::coarseParams(); - - Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor, false); - std::vector poses = scene->getPoses(); - - Mat depth = scene->depth(poses[0]); + Settings settings(isHashTSDF, false); + + Mat depth = settings.scene->depth(settings.poses[0]); UMat _points, _normals, _tmpnormals; UMat _newPoints, _newNormals; Mat points, normals; @@ -359,26 +377,24 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo } }; - Ptr volume = kinfu::makeVolume(_params->volumeType, _params->voxelSize, _params->volumePose.matrix, - _params->raycast_step_factor, _params->tsdf_trunc_dist, _params->tsdf_max_weight, - _params->truncateThreshold, _params->volumeDims); - volume->integrate(depth, _params->depthFactor, poses[0].matrix, _params->intr); + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); if (isRaycast) { - volume->raycast(poses[0].matrix, _params->intr, _params->frameSize, _points, _normals); + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); } if (isFetchPointsNormals) { - volume->fetchPointsNormals(_points, _normals); + settings.volume->fetchPointsNormals(_points, _normals); } if (isFetchNormals) { - volume->fetchPointsNormals(_points, _tmpnormals); - volume->fetchNormals(_points, _normals); + settings.volume->fetchPointsNormals(_points, _tmpnormals); + settings.volume->fetchNormals(_points, _normals); } normals = _normals.getMat(af); + points = _points.getMat(af); if (parallelCheck) { @@ -391,12 +407,12 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo if (isRaycast && display) { - displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); } if (isRaycast) { - volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); normals = _newNormals.getMat(af); normalsCheck(normals); @@ -410,10 +426,9 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo normalsCheck(normals); } - if (display) { - displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); } } @@ -423,28 +438,18 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo void valid_points_test(bool isHashTSDF) { - Ptr _params; - if (isHashTSDF) - _params = kinfu::Params::hashTSDFParams(true); - else - _params = kinfu::Params::coarseParams(); - - Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor, true); - std::vector poses = scene->getPoses(); + Settings settings(isHashTSDF, false); - Mat depth = scene->depth(poses[0]); + Mat depth = settings.scene->depth(settings.poses[0]); UMat _points, _normals; UMat _newPoints, _newNormals; Mat points, normals; int anfas, profile; AccessFlag af = ACCESS_READ; - Ptr volume = kinfu::makeVolume(_params->volumeType, _params->voxelSize, _params->volumePose.matrix, - _params->raycast_step_factor, _params->tsdf_trunc_dist, _params->tsdf_max_weight, - _params->truncateThreshold, _params->volumeDims); - volume->integrate(depth, _params->depthFactor, poses[0].matrix, _params->intr); + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); - volume->raycast(poses[0].matrix, _params->intr, _params->frameSize, _points, _normals); + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); normals = _normals.getMat(af); points = _points.getMat(af); patchNaNs(points); @@ -452,10 +457,10 @@ void valid_points_test(bool isHashTSDF) if (display) { - displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); } - volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); normals = _newNormals.getMat(af); points = _newPoints.getMat(af); @@ -464,7 +469,7 @@ void valid_points_test(bool isHashTSDF) if (display) { - displayImage(depth, points, normals, _params->depthFactor, _params->lightPose); + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); } float percentValidity = float(profile) / float(anfas); From 187445098b327c8fe0b5ca2a6cf0bae5cba67cb3 Mon Sep 17 00:00:00 2001 From: batters21 Date: Tue, 9 Feb 2021 15:20:20 +0300 Subject: [PATCH 08/18] remove extra code --- modules/rgbd/perf/perf_tsdf.cpp | 4 --- modules/rgbd/test/test_tsdf.cpp | 48 ++++++++++----------------------- 2 files changed, 14 insertions(+), 38 deletions(-) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index b8663cca430..3849ad0f6ea 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -343,9 +343,7 @@ PERF_TEST(Perf_TSDF, raycast) stopTimer(); if (display) - { displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose); - } } SANITY_CHECK_NOTHING(); } @@ -380,9 +378,7 @@ PERF_TEST(Perf_HashTSDF, raycast) stopTimer(); if (display) - { displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose); - } } SANITY_CHECK_NOTHING(); } diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 9de369a82f9..4e95c8c3d8e 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -358,14 +358,6 @@ int counterOfValid(Mat points) void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) { - Settings settings(isHashTSDF, false); - - Mat depth = settings.scene->depth(settings.poses[0]); - UMat _points, _normals, _tmpnormals; - UMat _newPoints, _newNormals; - Mat points, normals; - AccessFlag af = ACCESS_READ; - auto normalCheck = [](Vec4f& vector, const int*) { if (!cvIsNaN(vector[0])) @@ -377,6 +369,14 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo } }; + Settings settings(isHashTSDF, false); + + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _tmpnormals; + UMat _newPoints, _newNormals; + Mat points, normals; + AccessFlag af = ACCESS_READ; + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); if (isRaycast) @@ -397,40 +397,27 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo points = _points.getMat(af); if (parallelCheck) - { normals.forEach(normalCheck); - } else - { normalsCheck(normals); - } - + if (isRaycast && display) - { displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - } if (isRaycast) { settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); - normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); normalsCheck(normals); if (parallelCheck) - { normals.forEach(normalCheck); - } else - { normalsCheck(normals); - } if (display) - { displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - } - } points.release(); normals.release(); @@ -441,36 +428,29 @@ void valid_points_test(bool isHashTSDF) Settings settings(isHashTSDF, false); Mat depth = settings.scene->depth(settings.poses[0]); - UMat _points, _normals; - UMat _newPoints, _newNormals; + UMat _points, _normals, _newPoints, _newNormals; + AccessFlag af = ACCESS_READ; Mat points, normals; int anfas, profile; - AccessFlag af = ACCESS_READ; settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); - settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); normals = _normals.getMat(af); - points = _points.getMat(af); + points = _points.getMat(af); patchNaNs(points); anfas = counterOfValid(points); if (display) - { displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - } settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); - normals = _newNormals.getMat(af); - points = _newPoints.getMat(af); + points = _newPoints.getMat(af); patchNaNs(points); profile = counterOfValid(points); if (display) - { displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - } float percentValidity = float(profile) / float(anfas); ASSERT_NE(profile, 0) << "There is no points in profile"; From 6febe9c0e8672c79d46b911dca9bcd00238adc06 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 16 Mar 2021 16:06:08 +0300 Subject: [PATCH 09/18] replace scene in perf test --- modules/rgbd/perf/perf_tsdf.cpp | 70 +++++++++++++++++---------------- 1 file changed, 37 insertions(+), 33 deletions(-) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index 3849ad0f6ea..c8d8f88fc16 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -93,43 +93,40 @@ struct Scene virtual std::vector getPoses() = 0; }; -struct SemisphereScene : Scene +struct RotatingScene : Scene { - const int framesPerCycle = 72; - const float nCycles = 1.0f; - const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -1.5f)); + const int framesPerCycle = 32; + const float nCycles = 0.5f; + const Affine3f startPose = Affine3f(Vec3f(-1.f, 0.f, 0.f), Vec3f(1.5f, 2.f, -1.5f)); - Size frameSize; - Matx33f intr; - float depthFactor; - - SemisphereScene(Size sz, Matx33f _intr, float _depthFactor) : + RotatingScene(Size sz, Matx33f _intr, float _depthFactor) : frameSize(sz), intr(_intr), depthFactor(_depthFactor) - { } + { + cv::RNG rng(0); + rng.fill(randTexture, cv::RNG::UNIFORM, 0.f, 1.f); + } static float map(Point3f p) { - float plane = p.y + 0.5f; - - Point3f boxPose = p - Point3f(-0.0f, 0.3f, 0.5f); - float boxSize = 0.5f; - float roundness = 0.08f; - Point3f boxTmp; - boxTmp.x = max(abs(boxPose.x) - boxSize, 0.0f); - boxTmp.y = max(abs(boxPose.y) - boxSize, 0.0f); - boxTmp.z = max(abs(boxPose.z) - boxSize, 0.0f); - float roundBox = (float)cv::norm(boxTmp) - roundness; - - Point3f spherePose = p - Point3f(-0.0f, 0.3f, 0.0f); - float sphereRadius = 0.5f; - float sphere = (float)cv::norm(spherePose) - sphereRadius; - float sphereMinusBox = max(sphere, -roundBox); - - float subSphereRadius = 0.05f; - Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); - float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; - - float res = min({ sphereMinusBox, subSphere, plane }); + const Point3f torPlace(0.f, 0.f, 0.f); + Point3f torPos(p - torPlace); + const Point2f torusParams(1.f, 0.2f); + Point2f torq(std::sqrt(torPos.x * torPos.x + torPos.z * torPos.z) - torusParams.x, torPos.y); + float torus = (float)cv::norm(torq) - torusParams.y; + + const Point3f cylShift(0.25f, 0.25f, 0.25f); + + Point3f cylPos = Point3f(abs(std::fmod(p.x - 0.1f, cylShift.x)), + p.y, + abs(std::fmod(p.z - 0.2f, cylShift.z))) - cylShift * 0.5f; + + const Point2f cylParams(0.1f, + 0.1f + 0.1f * sin(p.x * p.y * 5.f /* +std::log(1.f+abs(p.x*0.1f)) */)); + Point2f cyld = Point2f(abs(std::sqrt(cylPos.x * cylPos.x + cylPos.z * cylPos.z)), abs(cylPos.y)) - cylParams; + float pins = min(max(cyld.x, cyld.y), 0.0f) + (float)cv::norm(Point2f(max(cyld.x, 0.f), max(cyld.y, 0.f))); + + float res = max(-pins, torus); + return res; } @@ -139,7 +136,7 @@ struct SemisphereScene : Scene Reprojector reproj(intr); Range range(0, frame.rows); - parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor)); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor)); return std::move(frame); } @@ -162,11 +159,17 @@ struct SemisphereScene : Scene return poses; } + Size frameSize; + Matx33f intr; + float depthFactor; + static cv::Mat_ randTexture; }; +Mat_ RotatingScene::randTexture(256, 256); + Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor) { - return makePtr(sz, _intr, _depthFactor); + return makePtr(sz, _intr, _depthFactor); } // this is a temporary solution @@ -272,6 +275,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } }, nstripes); } + // ---------------------------- class Settings From 6176828328a84d823c2271d83c68a4dbfe7dae30 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Mar 2021 14:32:52 +0300 Subject: [PATCH 10/18] main dug fixed --- modules/rgbd/test/test_tsdf.cpp | 26 +++++++++----------------- 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 4e95c8c3d8e..cb5b0aefe1e 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -99,7 +99,7 @@ struct SemisphereScene : Scene { const int framesPerCycle = 72; const float nCycles = 0.25f; - const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.3f)); + const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); Size frameSize; Matx33f intr; @@ -113,20 +113,10 @@ struct SemisphereScene : Scene static float map(Point3f p, bool onlySemisphere) { float plane = p.y + 0.5f; - - Point3f boxPose = p - Point3f(-0.0f, 0.3f, 0.5f); - float boxSize = 0.5f; - float roundness = 0.08f; - Point3f boxTmp; - boxTmp.x = max(abs(boxPose.x) - boxSize, 0.0f); - boxTmp.y = max(abs(boxPose.y) - boxSize, 0.0f); - boxTmp.z = max(abs(boxPose.z) - boxSize, 0.0f); - float roundBox = (float)cv::norm(boxTmp) - roundness; - - Point3f spherePose = p - Point3f(-0.0f, 0.3f, 0.0f); + Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); float sphereRadius = 0.5f; float sphere = (float)cv::norm(spherePose) - sphereRadius; - float sphereMinusBox = max(sphere, -roundBox); + float sphereMinusBox = sphere; float subSphereRadius = 0.05f; Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); @@ -160,7 +150,7 @@ struct SemisphereScene : Scene float angle = (float)(CV_2PI * i / framesPerCycle); Affine3f pose; pose = pose.rotate(startPose.rotation()); - pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle); + pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), startPose.translation()[1], startPose.translation()[2] * cos(angle))); @@ -282,7 +272,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = true; +static const bool display = false; static const bool parallelCheck = false; class Settings @@ -425,7 +415,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo void valid_points_test(bool isHashTSDF) { - Settings settings(isHashTSDF, false); + Settings settings(isHashTSDF, true); Mat depth = settings.scene->depth(settings.poses[0]); UMat _points, _normals, _newPoints, _newNormals; @@ -452,7 +442,9 @@ void valid_points_test(bool isHashTSDF) if (display) displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - float percentValidity = float(profile) / float(anfas); + // TODO: why profile == 2*anfas ? + float percentValidity = float(anfas) / float(profile); + ASSERT_NE(profile, 0) << "There is no points in profile"; ASSERT_NE(anfas, 0) << "There is no points in anfas"; ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; From 36c899dbe518e5815947b9ba63a1f5a8d28df779 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Mar 2021 14:45:03 +0300 Subject: [PATCH 11/18] fix the same bug --- modules/rgbd/perf/perf_tsdf.cpp | 87 ++++++++++++++++----------------- 1 file changed, 41 insertions(+), 46 deletions(-) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index c8d8f88fc16..e6677ed9c0f 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -32,12 +32,13 @@ template struct RenderInvoker : ParallelLoopBody { RenderInvoker(Mat_& _frame, Affine3f _pose, - Reprojector _reproj, - float _depthFactor) : ParallelLoopBody(), + Reprojector _reproj, float _depthFactor, bool _onlySemisphere) + : ParallelLoopBody(), frame(_frame), pose(_pose), reproj(_reproj), - depthFactor(_depthFactor) + depthFactor(_depthFactor), + onlySemisphere(_onlySemisphere) { } virtual void operator ()(const cv::Range& r) const @@ -64,7 +65,7 @@ struct RenderInvoker : ParallelLoopBody for (int step = 0; step < maxSteps && t < maxDepth; step++) { Point3f p = orig + dir * t; - float d = Scene::map(p); + float d = Scene::map(p, onlySemisphere); if (d < 0.000001f) { float depth = std::sqrt(t * t * xyt); @@ -83,49 +84,49 @@ struct RenderInvoker : ParallelLoopBody Affine3f pose; Reprojector reproj; float depthFactor; + bool onlySemisphere; }; struct Scene { virtual ~Scene() {} - static Ptr create(Size sz, Matx33f _intr, float _depthFactor); + static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); virtual Mat depth(Affine3f pose) = 0; virtual std::vector getPoses() = 0; }; -struct RotatingScene : Scene +struct SemisphereScene : Scene { - const int framesPerCycle = 32; - const float nCycles = 0.5f; - const Affine3f startPose = Affine3f(Vec3f(-1.f, 0.f, 0.f), Vec3f(1.5f, 2.f, -1.5f)); + const int framesPerCycle = 72; + const float nCycles = 0.25f; + const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); - RotatingScene(Size sz, Matx33f _intr, float _depthFactor) : - frameSize(sz), intr(_intr), depthFactor(_depthFactor) - { - cv::RNG rng(0); - rng.fill(randTexture, cv::RNG::UNIFORM, 0.f, 1.f); - } - - static float map(Point3f p) - { - const Point3f torPlace(0.f, 0.f, 0.f); - Point3f torPos(p - torPlace); - const Point2f torusParams(1.f, 0.2f); - Point2f torq(std::sqrt(torPos.x * torPos.x + torPos.z * torPos.z) - torusParams.x, torPos.y); - float torus = (float)cv::norm(torq) - torusParams.y; - - const Point3f cylShift(0.25f, 0.25f, 0.25f); - - Point3f cylPos = Point3f(abs(std::fmod(p.x - 0.1f, cylShift.x)), - p.y, - abs(std::fmod(p.z - 0.2f, cylShift.z))) - cylShift * 0.5f; + Size frameSize; + Matx33f intr; + float depthFactor; + bool onlySemisphere; - const Point2f cylParams(0.1f, - 0.1f + 0.1f * sin(p.x * p.y * 5.f /* +std::log(1.f+abs(p.x*0.1f)) */)); - Point2f cyld = Point2f(abs(std::sqrt(cylPos.x * cylPos.x + cylPos.z * cylPos.z)), abs(cylPos.y)) - cylParams; - float pins = min(max(cyld.x, cyld.y), 0.0f) + (float)cv::norm(Point2f(max(cyld.x, 0.f), max(cyld.y, 0.f))); + SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) + { } - float res = max(-pins, torus); + static float map(Point3f p, bool onlySemisphere) + { + float plane = p.y + 0.5f; + Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); + float sphereRadius = 0.5f; + float sphere = (float)cv::norm(spherePose) - sphereRadius; + float sphereMinusBox = sphere; + + float subSphereRadius = 0.05f; + Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); + float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; + + float res; + if (!onlySemisphere) + res = min({ sphereMinusBox, subSphere, plane }); + else + res = sphereMinusBox; return res; } @@ -136,7 +137,7 @@ struct RotatingScene : Scene Reprojector reproj(intr); Range range(0, frame.rows); - parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor)); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); return std::move(frame); } @@ -149,7 +150,7 @@ struct RotatingScene : Scene float angle = (float)(CV_2PI * i / framesPerCycle); Affine3f pose; pose = pose.rotate(startPose.rotation()); - pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle); + pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), startPose.translation()[1], startPose.translation()[2] * cos(angle))); @@ -159,17 +160,11 @@ struct RotatingScene : Scene return poses; } - Size frameSize; - Matx33f intr; - float depthFactor; - static cv::Mat_ randTexture; }; -Mat_ RotatingScene::randTexture(256, 256); - -Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor) +Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) { - return makePtr(sz, _intr, _depthFactor); + return makePtr(sz, _intr, _depthFactor, _onlySemisphere); } // this is a temporary solution @@ -297,7 +292,7 @@ class Settings _params->raycast_step_factor, _params->tsdf_trunc_dist, _params->tsdf_max_weight, _params->truncateThreshold, _params->volumeDims); - scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor); + scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor, true); poses = scene->getPoses(); } }; @@ -316,7 +311,7 @@ void displayImage(Mat depth, UMat _points, UMat _normals, float depthFactor, Vec waitKey(2000); } -static const bool display = true; +static const bool display = false; PERF_TEST(Perf_TSDF, integrate) { From 4c13406891637ed6d1b502264bcf4c104d637592 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Mar 2021 15:03:47 +0300 Subject: [PATCH 12/18] fix --- modules/rgbd/test/test_tsdf.cpp | 968 ++++++++++++++++---------------- 1 file changed, 475 insertions(+), 493 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 137ea01046e..394bacc5288 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -4,552 +4,534 @@ #include "test_precomp.hpp" -namespace opencv_test { namespace { - -using namespace cv; - -/** Reprojects screen point to camera space given z coord. */ -struct Reprojector -{ - Reprojector() {} - inline Reprojector(Matx33f intr) - { - fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1); - cx = intr(0, 2), cy = intr(1, 2); - } - template - inline cv::Point3_ operator()(cv::Point3_ p) const - { - T x = p.z * (p.x - cx) * fxinv; - T y = p.z * (p.y - cy) * fyinv; - return cv::Point3_(x, y, p.z); - } +namespace opencv_test { + namespace { + + using namespace cv; + + /** Reprojects screen point to camera space given z coord. */ + struct Reprojector + { + Reprojector() {} + inline Reprojector(Matx33f intr) + { + fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1); + cx = intr(0, 2), cy = intr(1, 2); + } + template + inline cv::Point3_ operator()(cv::Point3_ p) const + { + T x = p.z * (p.x - cx) * fxinv; + T y = p.z * (p.y - cy) * fyinv; + return cv::Point3_(x, y, p.z); + } - float fxinv, fyinv, cx, cy; -}; - -template -struct RenderInvoker : ParallelLoopBody -{ - RenderInvoker(Mat_& _frame, Affine3f _pose, - Reprojector _reproj, float _depthFactor, bool _onlySemisphere) - : ParallelLoopBody(), - frame(_frame), - pose(_pose), - reproj(_reproj), - depthFactor(_depthFactor), - onlySemisphere(_onlySemisphere) - { } - - virtual void operator ()(const cv::Range& r) const - { - for (int y = r.start; y < r.end; y++) + float fxinv, fyinv, cx, cy; + }; + + template + struct RenderInvoker : ParallelLoopBody { - float* frameRow = frame[y]; - for (int x = 0; x < frame.cols; x++) + RenderInvoker(Mat_& _frame, Affine3f _pose, + Reprojector _reproj, float _depthFactor, bool _onlySemisphere) + : ParallelLoopBody(), + frame(_frame), + pose(_pose), + reproj(_reproj), + depthFactor(_depthFactor), + onlySemisphere(_onlySemisphere) + { } + + virtual void operator ()(const cv::Range& r) const { - float pix = 0; - - Point3f orig = pose.translation(); - // direction through pixel - Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); - float xyt = 1.f / (screenVec.x * screenVec.x + - screenVec.y * screenVec.y + 1.f); - Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); - // screen space axis - dir.y = -dir.y; - - const float maxDepth = 20.f; - const float maxSteps = 256; - float t = 0.f; - for (int step = 0; step < maxSteps && t < maxDepth; step++) + for (int y = r.start; y < r.end; y++) { - Point3f p = orig + dir * t; - float d = Scene::map(p, onlySemisphere); - if (d < 0.000001f) + float* frameRow = frame[y]; + for (int x = 0; x < frame.cols; x++) { - float depth = std::sqrt(t * t * xyt); - pix = depth * depthFactor; - break; + float pix = 0; + + Point3f orig = pose.translation(); + // direction through pixel + Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); + float xyt = 1.f / (screenVec.x * screenVec.x + + screenVec.y * screenVec.y + 1.f); + Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); + // screen space axis + dir.y = -dir.y; + + const float maxDepth = 20.f; + const float maxSteps = 256; + float t = 0.f; + for (int step = 0; step < maxSteps && t < maxDepth; step++) + { + Point3f p = orig + dir * t; + float d = Scene::map(p, onlySemisphere); + if (d < 0.000001f) + { + float depth = std::sqrt(t * t * xyt); + pix = depth * depthFactor; + break; + } + t += d; + } + + frameRow[x] = pix; } - t += d; } + } + + Mat_& frame; + Affine3f pose; + Reprojector reproj; + float depthFactor; + bool onlySemisphere; + }; + + struct Scene + { + virtual ~Scene() {} + static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); + virtual Mat depth(Affine3f pose) = 0; + virtual std::vector getPoses() = 0; + }; + + struct SemisphereScene : Scene + { + const int framesPerCycle = 72; + const float nCycles = 0.25f; + const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); + + Size frameSize; + Matx33f intr; + float depthFactor; + bool onlySemisphere; - frameRow[x] = pix; + SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) + { } + + static float map(Point3f p, bool onlySemisphere) + { + float plane = p.y + 0.5f; + Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); + float sphereRadius = 0.5f; + float sphere = (float)cv::norm(spherePose) - sphereRadius; + float sphereMinusBox = sphere; + + float subSphereRadius = 0.05f; + Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); + float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; + + float res; + if (!onlySemisphere) + res = min({ sphereMinusBox, subSphere, plane }); + else + res = sphereMinusBox; + + return res; } - } - } - Mat_& frame; - Affine3f pose; - Reprojector reproj; - float depthFactor; - bool onlySemisphere; -}; - -struct Scene -{ - virtual ~Scene() {} - static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); - virtual Mat depth(Affine3f pose) = 0; - virtual std::vector getPoses() = 0; -}; - -struct SemisphereScene : Scene -{ - const int framesPerCycle = 72; - const float nCycles = 0.25f; - const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); - - Size frameSize; - Matx33f intr; - float depthFactor; - bool onlySemisphere; - - SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : - frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) - { } - - static float map(Point3f p, bool onlySemisphere) - { - float plane = p.y + 0.5f; - Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); - float sphereRadius = 0.5f; - float sphere = (float)cv::norm(spherePose) - sphereRadius; - float sphereMinusBox = sphere; - - float subSphereRadius = 0.05f; - Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); - float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; - - float res; - if (!onlySemisphere) - res = min({sphereMinusBox, subSphere, plane}); - else - res = sphereMinusBox; - - return res; - } + Mat depth(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); - Mat depth(Affine3f pose) override - { - Mat_ frame(frameSize); - Reprojector reproj(intr); + Range range(0, frame.rows); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); - Range range(0, frame.rows); - parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); + return std::move(frame); + } - return std::move(frame); - } + std::vector getPoses() override + { + std::vector poses; + for (int i = 0; i < framesPerCycle * nCycles; i++) + { + float angle = (float)(CV_2PI * i / framesPerCycle); + Affine3f pose; + pose = pose.rotate(startPose.rotation()); + pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); + pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), + startPose.translation()[1], + startPose.translation()[2] * cos(angle))); + poses.push_back(pose); + } + + return poses; + } - std::vector getPoses() override - { - std::vector poses; - for (int i = 0; i < framesPerCycle * nCycles; i++) + }; + + Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) { - float angle = (float)(CV_2PI * i / framesPerCycle); - Affine3f pose; - pose = pose.rotate(startPose.rotation()); - pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); - pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), - startPose.translation()[1], - startPose.translation()[2] * cos(angle))); - poses.push_back(pose); + return makePtr(sz, _intr, _depthFactor, _onlySemisphere); } - return poses; - } + // this is a temporary solution + // ---------------------------- + + typedef cv::Vec4f ptype; + typedef cv::Mat_< ptype > Points; + typedef Points Normals; + typedef Size2i Size; -}; + template + inline float specPow(float x) + { + if (p % 2 == 0) + { + float v = specPow

(x); + return v * v; + } + else + { + float v = specPow<(p - 1) / 2>(x); + return v * v * x; + } + } -Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) -{ - return makePtr(sz, _intr, _depthFactor, _onlySemisphere); -} + template<> + inline float specPow<0>(float /*x*/) + { + return 1.f; + } -// this is a temporary solution -// ---------------------------- + template<> + inline float specPow<1>(float x) + { + return x; + } -typedef cv::Vec4f ptype; -typedef cv::Mat_< ptype > Points; -typedef Points Normals; -typedef Size2i Size; + inline cv::Vec3f fromPtype(const ptype& x) + { + return cv::Vec3f(x[0], x[1], x[2]); + } -template -inline float specPow(float x) -{ - if (p % 2 == 0) - { - float v = specPow

(x); - return v * v; - } - else - { - float v = specPow<(p - 1) / 2>(x); - return v * v * x; - } -} - -template<> -inline float specPow<0>(float /*x*/) -{ - return 1.f; -} - -template<> -inline float specPow<1>(float x) -{ - return x; -} - -inline cv::Vec3f fromPtype(const ptype& x) -{ - return cv::Vec3f(x[0], x[1], x[2]); -} - -inline Point3f normalize(const Vec3f& v) -{ - double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); - return v * (nv ? 1. / nv : 0.); -} - -void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose) -{ - Size sz = _points.size(); - image.create(sz, CV_8UC4); - - Points points = _points.getMat(); - Normals normals = _normals.getMat(); - - Mat_ img = image.getMat(); - - Range range(0, sz.height); - const int nstripes = -1; - parallel_for_(range, [&](const Range&) + inline Point3f normalize(const Vec3f& v) { - for (int y = range.start; y < range.end; y++) - { - Vec4b* imgRow = img[y]; - const ptype* ptsRow = points[y]; - const ptype* nrmRow = normals[y]; + double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + return v * (nv ? 1. / nv : 0.); + } - for (int x = 0; x < sz.width; x++) - { - Point3f p = fromPtype(ptsRow[x]); - Point3f n = fromPtype(nrmRow[x]); + void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose) + { + Size sz = _points.size(); + image.create(sz, CV_8UC4); - Vec4b color; + Points points = _points.getMat(); + Normals normals = _normals.getMat(); - if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z) ) - { - color = Vec4b(0, 32, 0, 0); - } - else + Mat_ img = image.getMat(); + + Range range(0, sz.height); + const int nstripes = -1; + parallel_for_(range, [&](const Range&) + { + for (int y = range.start; y < range.end; y++) { - const float Ka = 0.3f; //ambient coeff - const float Kd = 0.5f; //diffuse coeff - const float Ks = 0.2f; //specular coeff - const int sp = 20; //specular power - - const float Ax = 1.f; //ambient color, can be RGB - const float Dx = 1.f; //diffuse color, can be RGB - const float Sx = 1.f; //specular color, can be RGB - const float Lx = 1.f; //light color - - Point3f l = normalize(lightPose.translation() - Vec3f(p)); - Point3f v = normalize(-Vec3f(p)); - Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l)); - - uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) + - Lx * Ks * Sx * specPow(max(0.f, r.dot(v)))) * 255.f); - color = Vec4b(ix, ix, ix, 0); + Vec4b* imgRow = img[y]; + const ptype* ptsRow = points[y]; + const ptype* nrmRow = normals[y]; + + for (int x = 0; x < sz.width; x++) + { + Point3f p = fromPtype(ptsRow[x]); + Point3f n = fromPtype(nrmRow[x]); + + Vec4b color; + + if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z)) + { + color = Vec4b(0, 32, 0, 0); + } + else + { + const float Ka = 0.3f; //ambient coeff + const float Kd = 0.5f; //diffuse coeff + const float Ks = 0.2f; //specular coeff + const int sp = 20; //specular power + + const float Ax = 1.f; //ambient color, can be RGB + const float Dx = 1.f; //diffuse color, can be RGB + const float Sx = 1.f; //specular color, can be RGB + const float Lx = 1.f; //light color + + Point3f l = normalize(lightPose.translation() - Vec3f(p)); + Point3f v = normalize(-Vec3f(p)); + Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l)); + + uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) + + Lx * Ks * Sx * specPow(max(0.f, r.dot(v)))) * 255.f); + color = Vec4b(ix, ix, ix, 0); + } + + imgRow[x] = color; + } } + }, nstripes); + } + // ---------------------------- - imgRow[x] = color; - } + static const bool display = false; + static const bool parallelCheck = false; + + class Settings + { + public: + Ptr params; + Ptr volume; + Ptr scene; + std::vector poses; + + Settings(bool useHashTSDF, bool onlySemisphere) + { + if (useHashTSDF) + params = kinfu::Params::hashTSDFParams(true); + else + params = kinfu::Params::coarseParams(); + + volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, + params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, + params->truncateThreshold, params->volumeDims); + + scene = Scene::create(params->frameSize, params->intr, params->depthFactor, onlySemisphere); + poses = scene->getPoses(); } - }, nstripes); -} -// ---------------------------- - -static const bool display = false; -static const bool parallelCheck = false; - -class Settings -{ -public: - Ptr params; - Ptr volume; - Ptr scene; - std::vector poses; - - Settings(bool useHashTSDF, bool onlySemisphere) - { - if (useHashTSDF) - params = kinfu::Params::hashTSDFParams(true); - else - params = kinfu::Params::coarseParams(); - - volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, - params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, - params->truncateThreshold, params->volumeDims); - - scene = Scene::create(params->frameSize, params->intr, params->depthFactor, onlySemisphere); - poses = scene->getPoses(); - } -}; - -void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) -{ - Mat image; - patchNaNs(points); - imshow("depth", depth * (1.f / depthFactor / 4.f)); - renderPointsNormals(points, normals, image, lightPose); - imshow("render", image); - waitKey(2000); -} - -void normalsCheck(Mat normals) -{ - Vec4f vector; - for (auto pvector = normals.begin(); pvector < normals.end(); pvector++) - { - vector = *pvector; - if (!cvIsNaN(vector[0])) + }; + + void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) { - float length = vector[0] * vector[0] + - vector[1] * vector[1] + - vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + Mat image; + patchNaNs(points); + imshow("depth", depth * (1.f / depthFactor / 4.f)); + renderPointsNormals(points, normals, image, lightPose); + imshow("render", image); + waitKey(2000); } - } -} - -int counterOfValid(Mat points) -{ - Vec4f* v; - int i, j; - int count = 0; - for (i = 0; i < points.rows; ++i) - { - v = (points.ptr(i)); - for (j = 0; j < points.cols; ++j) + + void normalsCheck(Mat normals) { - if ((v[j])[0] != 0 || - (v[j])[1] != 0 || - (v[j])[2] != 0) + Vec4f vector; + for (auto pvector = normals.begin(); pvector < normals.end(); pvector++) { - count++; + vector = *pvector; + if (!cvIsNaN(vector[0])) + { + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + } } } - } - return count; -} - -void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) -{ - auto normalCheck = [](Vec4f& vector, const int*) - { - if (!cvIsNaN(vector[0])) + + int counterOfValid(Mat points) { - float length = vector[0] * vector[0] + - vector[1] * vector[1] + - vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + Vec4f* v; + int i, j; + int count = 0; + for (i = 0; i < points.rows; ++i) + { + v = (points.ptr(i)); + for (j = 0; j < points.cols; ++j) + { + if ((v[j])[0] != 0 || + (v[j])[1] != 0 || + (v[j])[2] != 0) + { + count++; + } + } + } + return count; } - }; - Settings settings(isHashTSDF, false); - - Mat depth = settings.scene->depth(settings.poses[0]); - UMat _points, _normals, _tmpnormals; - UMat _newPoints, _newNormals; - Mat points, normals; - AccessFlag af = ACCESS_READ; + void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) + { + auto normalCheck = [](Vec4f& vector, const int*) + { + if (!cvIsNaN(vector[0])) + { + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + } + }; - settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); + Settings settings(isHashTSDF, false); - if (isRaycast) - { - settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); - } - if (isFetchPointsNormals) - { - settings.volume->fetchPointsNormals(_points, _normals); - } - if (isFetchNormals) - { - settings.volume->fetchPointsNormals(_points, _tmpnormals); - settings.volume->fetchNormals(_points, _normals); - } + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _tmpnormals; + UMat _newPoints, _newNormals; + Mat points, normals; + AccessFlag af = ACCESS_READ; - normals = _normals.getMat(af); - points = _points.getMat(af); - - if (parallelCheck) - normals.forEach(normalCheck); - else - normalsCheck(normals); - - if (isRaycast && display) - { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - points = _points.getMat(af); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); - } - if (isRaycast) - { - settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); - normals = _newNormals.getMat(af); - points = _newPoints.getMat(af); - normalsCheck(normals); - - if (parallelCheck) - normals.forEach(normalCheck); - else - normalsCheck(normals); - - if (display) + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); + + if (isRaycast) + { + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); + } + if (isFetchPointsNormals) + { + settings.volume->fetchPointsNormals(_points, _normals); + } + if (isFetchNormals) + { + settings.volume->fetchPointsNormals(_points, _tmpnormals); + settings.volume->fetchNormals(_points, _normals); + } + + normals = _normals.getMat(af); + points = _points.getMat(af); + + if (parallelCheck) + normals.forEach(normalCheck); + else + normalsCheck(normals); + + if (isRaycast && display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + + if (isRaycast) + { + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); + normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); + normalsCheck(normals); + + if (parallelCheck) + normals.forEach(normalCheck); + else + normalsCheck(normals); + + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + } + + points.release(); normals.release(); + } + + void valid_points_test(bool isHashTSDF) { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); + Settings settings(isHashTSDF, true); + + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _newPoints, _newNormals; + AccessFlag af = ACCESS_READ; + Mat points, normals; + int anfas, profile; + + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); + normals = _normals.getMat(af); + points = _points.getMat(af); + patchNaNs(points); + anfas = counterOfValid(points); + + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); + normals = _newNormals.getMat(af); points = _newPoints.getMat(af); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); - } - } + patchNaNs(points); + profile = counterOfValid(points); - points.release(); normals.release(); -} - -void valid_points_test(bool isHashTSDF) -{ - Settings settings(isHashTSDF, true); - - Mat depth = settings.scene->depth(settings.poses[0]); - UMat _points, _normals, _newPoints, _newNormals; - AccessFlag af = ACCESS_READ; - Mat points, normals; - int anfas, profile; - - settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); - settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); - normals = _normals.getMat(af); - points = _points.getMat(af); - patchNaNs(points); - anfas = counterOfValid(points); - - if (display) - { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); - } + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); - settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); - normals = _newNormals.getMat(af); - points = _newPoints.getMat(af); - patchNaNs(points); - profile = counterOfValid(points); - - if (display) - { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); - } + // TODO: why profile == 2*anfas ? + float percentValidity = float(anfas) / float(profile); - float percentValidity; - if (profile == 0) percentValidity = -0.5; - else if (anfas == 0) percentValidity = 0; - else percentValidity = float(profile) / float(anfas); - ASSERT_LT(0.5 - percentValidity, 0.3); -} + ASSERT_NE(profile, 0) << "There is no points in profile"; + ASSERT_NE(anfas, 0) << "There is no points in anfas"; + ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; + } #ifndef HAVE_OPENCL -TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } -TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } -TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } -TEST(TSDF, valid_points) { valid_points_test(false); } + TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } + TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } + TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } + TEST(TSDF, valid_points) { valid_points_test(false); } -TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } -TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } -TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } -TEST(HashTSDF, valid_points) { valid_points_test(true); } + TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } + TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } + TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } + TEST(HashTSDF, valid_points) { valid_points_test(true); } #else -TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } -TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } -TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } -TEST(TSDF_GPU, valid_points) { valid_points_test(false); } - -TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } -TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } -TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } -TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } - -TEST(TSDF_CPU, raycast_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(false, true, false, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(false, false, true, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(false, false, false, true); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, valid_points) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test(false); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, raycast_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(true, true, false, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(true, false, true, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(true, false, false, true); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, valid_points) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test(true); - cv::ocl::setUseOpenCL(true); -} + TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } + TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } + TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } + TEST(TSDF_GPU, valid_points) { valid_points_test(false); } + + TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } + TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } + TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } + TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } + + TEST(TSDF_CPU, raycast_normals) + { + cv::ocl::setUseOpenCL(false); + normal_test(false, true, false, false); + cv::ocl::setUseOpenCL(true); + } + + TEST(TSDF_CPU, fetch_points_normals) + { + cv::ocl::setUseOpenCL(false); + normal_test(false, false, true, false); + cv::ocl::setUseOpenCL(true); + } + + TEST(TSDF_CPU, fetch_normals) + { + cv::ocl::setUseOpenCL(false); + normal_test(false, false, false, true); + cv::ocl::setUseOpenCL(true); + } + + TEST(TSDF_CPU, valid_points) + { + cv::ocl::setUseOpenCL(false); + valid_points_test(false); + cv::ocl::setUseOpenCL(true); + } + + TEST(HashTSDF_CPU, raycast_normals) + { + cv::ocl::setUseOpenCL(false); + normal_test(true, true, false, false); + cv::ocl::setUseOpenCL(true); + } + + TEST(HashTSDF_CPU, fetch_points_normals) + { + cv::ocl::setUseOpenCL(false); + normal_test(true, false, true, false); + cv::ocl::setUseOpenCL(true); + } + + TEST(HashTSDF_CPU, fetch_normals) + { + cv::ocl::setUseOpenCL(false); + normal_test(true, false, false, true); + cv::ocl::setUseOpenCL(true); + } + + TEST(HashTSDF_CPU, valid_points) + { + cv::ocl::setUseOpenCL(false); + valid_points_test(true); + cv::ocl::setUseOpenCL(true); + } + #endif -}} // namespace + } +} // namespace From 3cfe29fce04512aa8e031f45b79ba4e9f811b875 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Mar 2021 15:09:08 +0300 Subject: [PATCH 13/18] docs fix --- modules/rgbd/perf/perf_tsdf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index e6677ed9c0f..149093665ce 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -375,7 +375,7 @@ PERF_TEST(Perf_HashTSDF, raycast) startTimer(); settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); - + if (display) displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose); } From 21e4e699cb109bd4cf1b3281d2b80b9649d26488 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 29 Mar 2021 12:45:11 +0300 Subject: [PATCH 14/18] minor fix --- modules/rgbd/src/opencl/tsdf.cl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/opencl/tsdf.cl b/modules/rgbd/src/opencl/tsdf.cl index 3a13d3b8336..4f9841b291d 100644 --- a/modules/rgbd/src/opencl/tsdf.cl +++ b/modules/rgbd/src/opencl/tsdf.cl @@ -227,8 +227,8 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePt float vaz[8]; for(int i = 0; i < 8; i++) - vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf - - volumePtr[nco[i] - dim].tsdf); + vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf) - + tsdfToFloat(volumePtr[nco[i] - dim].tsdf); float8 vz = vload8(0, vaz); From 9791b0b79d91ff20bcb32a2d0c8651d35ba07cd0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 29 Mar 2021 13:38:37 +0300 Subject: [PATCH 15/18] namespace fix --- modules/rgbd/test/test_tsdf.cpp | 928 ++++++++++++++++---------------- 1 file changed, 464 insertions(+), 464 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 394bacc5288..2b26eeb2b37 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -5,533 +5,533 @@ #include "test_precomp.hpp" namespace opencv_test { - namespace { - - using namespace cv; - - /** Reprojects screen point to camera space given z coord. */ - struct Reprojector - { - Reprojector() {} - inline Reprojector(Matx33f intr) - { - fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1); - cx = intr(0, 2), cy = intr(1, 2); - } - template - inline cv::Point3_ operator()(cv::Point3_ p) const - { - T x = p.z * (p.x - cx) * fxinv; - T y = p.z * (p.y - cy) * fyinv; - return cv::Point3_(x, y, p.z); - } - - float fxinv, fyinv, cx, cy; - }; +namespace { + +using namespace cv; + +/** Reprojects screen point to camera space given z coord. */ +struct Reprojector +{ + Reprojector() {} + inline Reprojector(Matx33f intr) + { + fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1); + cx = intr(0, 2), cy = intr(1, 2); + } + template + inline cv::Point3_ operator()(cv::Point3_ p) const + { + T x = p.z * (p.x - cx) * fxinv; + T y = p.z * (p.y - cy) * fyinv; + return cv::Point3_(x, y, p.z); + } - template - struct RenderInvoker : ParallelLoopBody + float fxinv, fyinv, cx, cy; +}; + +template +struct RenderInvoker : ParallelLoopBody +{ + RenderInvoker(Mat_& _frame, Affine3f _pose, + Reprojector _reproj, float _depthFactor, bool _onlySemisphere) + : ParallelLoopBody(), + frame(_frame), + pose(_pose), + reproj(_reproj), + depthFactor(_depthFactor), + onlySemisphere(_onlySemisphere) + { } + + virtual void operator ()(const cv::Range& r) const + { + for (int y = r.start; y < r.end; y++) { - RenderInvoker(Mat_& _frame, Affine3f _pose, - Reprojector _reproj, float _depthFactor, bool _onlySemisphere) - : ParallelLoopBody(), - frame(_frame), - pose(_pose), - reproj(_reproj), - depthFactor(_depthFactor), - onlySemisphere(_onlySemisphere) - { } - - virtual void operator ()(const cv::Range& r) const + float* frameRow = frame[y]; + for (int x = 0; x < frame.cols; x++) { - for (int y = r.start; y < r.end; y++) + float pix = 0; + + Point3f orig = pose.translation(); + // direction through pixel + Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); + float xyt = 1.f / (screenVec.x * screenVec.x + + screenVec.y * screenVec.y + 1.f); + Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); + // screen space axis + dir.y = -dir.y; + + const float maxDepth = 20.f; + const float maxSteps = 256; + float t = 0.f; + for (int step = 0; step < maxSteps && t < maxDepth; step++) { - float* frameRow = frame[y]; - for (int x = 0; x < frame.cols; x++) + Point3f p = orig + dir * t; + float d = Scene::map(p, onlySemisphere); + if (d < 0.000001f) { - float pix = 0; - - Point3f orig = pose.translation(); - // direction through pixel - Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); - float xyt = 1.f / (screenVec.x * screenVec.x + - screenVec.y * screenVec.y + 1.f); - Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); - // screen space axis - dir.y = -dir.y; - - const float maxDepth = 20.f; - const float maxSteps = 256; - float t = 0.f; - for (int step = 0; step < maxSteps && t < maxDepth; step++) - { - Point3f p = orig + dir * t; - float d = Scene::map(p, onlySemisphere); - if (d < 0.000001f) - { - float depth = std::sqrt(t * t * xyt); - pix = depth * depthFactor; - break; - } - t += d; - } - - frameRow[x] = pix; + float depth = std::sqrt(t * t * xyt); + pix = depth * depthFactor; + break; } + t += d; } - } - - Mat_& frame; - Affine3f pose; - Reprojector reproj; - float depthFactor; - bool onlySemisphere; - }; - - struct Scene - { - virtual ~Scene() {} - static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); - virtual Mat depth(Affine3f pose) = 0; - virtual std::vector getPoses() = 0; - }; - struct SemisphereScene : Scene - { - const int framesPerCycle = 72; - const float nCycles = 0.25f; - const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); - - Size frameSize; - Matx33f intr; - float depthFactor; - bool onlySemisphere; - - SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : - frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) - { } - - static float map(Point3f p, bool onlySemisphere) - { - float plane = p.y + 0.5f; - Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); - float sphereRadius = 0.5f; - float sphere = (float)cv::norm(spherePose) - sphereRadius; - float sphereMinusBox = sphere; - - float subSphereRadius = 0.05f; - Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); - float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; - - float res; - if (!onlySemisphere) - res = min({ sphereMinusBox, subSphere, plane }); - else - res = sphereMinusBox; - - return res; + frameRow[x] = pix; } + } + } - Mat depth(Affine3f pose) override - { - Mat_ frame(frameSize); - Reprojector reproj(intr); - - Range range(0, frame.rows); - parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); - - return std::move(frame); - } + Mat_& frame; + Affine3f pose; + Reprojector reproj; + float depthFactor; + bool onlySemisphere; +}; + +struct Scene +{ + virtual ~Scene() {} + static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); + virtual Mat depth(Affine3f pose) = 0; + virtual std::vector getPoses() = 0; +}; + +struct SemisphereScene : Scene +{ + const int framesPerCycle = 72; + const float nCycles = 0.25f; + const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); + + Size frameSize; + Matx33f intr; + float depthFactor; + bool onlySemisphere; + + SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) + { } + + static float map(Point3f p, bool onlySemisphere) + { + float plane = p.y + 0.5f; + Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); + float sphereRadius = 0.5f; + float sphere = (float)cv::norm(spherePose) - sphereRadius; + float sphereMinusBox = sphere; + + float subSphereRadius = 0.05f; + Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); + float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; + + float res; + if (!onlySemisphere) + res = min({ sphereMinusBox, subSphere, plane }); + else + res = sphereMinusBox; + + return res; + } - std::vector getPoses() override - { - std::vector poses; - for (int i = 0; i < framesPerCycle * nCycles; i++) - { - float angle = (float)(CV_2PI * i / framesPerCycle); - Affine3f pose; - pose = pose.rotate(startPose.rotation()); - pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); - pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), - startPose.translation()[1], - startPose.translation()[2] * cos(angle))); - poses.push_back(pose); - } + Mat depth(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); - return poses; - } + Range range(0, frame.rows); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); - }; + return std::move(frame); + } - Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) + std::vector getPoses() override + { + std::vector poses; + for (int i = 0; i < framesPerCycle * nCycles; i++) { - return makePtr(sz, _intr, _depthFactor, _onlySemisphere); + float angle = (float)(CV_2PI * i / framesPerCycle); + Affine3f pose; + pose = pose.rotate(startPose.rotation()); + pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); + pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), + startPose.translation()[1], + startPose.translation()[2] * cos(angle))); + poses.push_back(pose); } - // this is a temporary solution - // ---------------------------- - - typedef cv::Vec4f ptype; - typedef cv::Mat_< ptype > Points; - typedef Points Normals; - typedef Size2i Size; - - template - inline float specPow(float x) - { - if (p % 2 == 0) - { - float v = specPow

(x); - return v * v; - } - else - { - float v = specPow<(p - 1) / 2>(x); - return v * v * x; - } - } + return poses; + } - template<> - inline float specPow<0>(float /*x*/) - { - return 1.f; - } +}; - template<> - inline float specPow<1>(float x) - { - return x; - } +Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) +{ + return makePtr(sz, _intr, _depthFactor, _onlySemisphere); +} - inline cv::Vec3f fromPtype(const ptype& x) - { - return cv::Vec3f(x[0], x[1], x[2]); - } +// this is a temporary solution +// ---------------------------- - inline Point3f normalize(const Vec3f& v) - { - double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); - return v * (nv ? 1. / nv : 0.); - } +typedef cv::Vec4f ptype; +typedef cv::Mat_< ptype > Points; +typedef Points Normals; +typedef Size2i Size; - void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose) +template +inline float specPow(float x) +{ + if (p % 2 == 0) + { + float v = specPow

(x); + return v * v; + } + else + { + float v = specPow<(p - 1) / 2>(x); + return v * v * x; + } +} + +template<> +inline float specPow<0>(float /*x*/) +{ + return 1.f; +} + +template<> +inline float specPow<1>(float x) +{ + return x; +} + +inline cv::Vec3f fromPtype(const ptype& x) +{ + return cv::Vec3f(x[0], x[1], x[2]); +} + +inline Point3f normalize(const Vec3f& v) +{ + double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + return v * (nv ? 1. / nv : 0.); +} + +void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose) +{ + Size sz = _points.size(); + image.create(sz, CV_8UC4); + + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + + Mat_ img = image.getMat(); + + Range range(0, sz.height); + const int nstripes = -1; + parallel_for_(range, [&](const Range&) { - Size sz = _points.size(); - image.create(sz, CV_8UC4); + for (int y = range.start; y < range.end; y++) + { + Vec4b* imgRow = img[y]; + const ptype* ptsRow = points[y]; + const ptype* nrmRow = normals[y]; - Points points = _points.getMat(); - Normals normals = _normals.getMat(); + for (int x = 0; x < sz.width; x++) + { + Point3f p = fromPtype(ptsRow[x]); + Point3f n = fromPtype(nrmRow[x]); - Mat_ img = image.getMat(); + Vec4b color; - Range range(0, sz.height); - const int nstripes = -1; - parallel_for_(range, [&](const Range&) - { - for (int y = range.start; y < range.end; y++) + if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z)) { - Vec4b* imgRow = img[y]; - const ptype* ptsRow = points[y]; - const ptype* nrmRow = normals[y]; - - for (int x = 0; x < sz.width; x++) - { - Point3f p = fromPtype(ptsRow[x]); - Point3f n = fromPtype(nrmRow[x]); - - Vec4b color; - - if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z)) - { - color = Vec4b(0, 32, 0, 0); - } - else - { - const float Ka = 0.3f; //ambient coeff - const float Kd = 0.5f; //diffuse coeff - const float Ks = 0.2f; //specular coeff - const int sp = 20; //specular power - - const float Ax = 1.f; //ambient color, can be RGB - const float Dx = 1.f; //diffuse color, can be RGB - const float Sx = 1.f; //specular color, can be RGB - const float Lx = 1.f; //light color - - Point3f l = normalize(lightPose.translation() - Vec3f(p)); - Point3f v = normalize(-Vec3f(p)); - Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l)); - - uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) + - Lx * Ks * Sx * specPow(max(0.f, r.dot(v)))) * 255.f); - color = Vec4b(ix, ix, ix, 0); - } - - imgRow[x] = color; - } + color = Vec4b(0, 32, 0, 0); + } + else + { + const float Ka = 0.3f; //ambient coeff + const float Kd = 0.5f; //diffuse coeff + const float Ks = 0.2f; //specular coeff + const int sp = 20; //specular power + + const float Ax = 1.f; //ambient color, can be RGB + const float Dx = 1.f; //diffuse color, can be RGB + const float Sx = 1.f; //specular color, can be RGB + const float Lx = 1.f; //light color + + Point3f l = normalize(lightPose.translation() - Vec3f(p)); + Point3f v = normalize(-Vec3f(p)); + Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l)); + + uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) + + Lx * Ks * Sx * specPow(max(0.f, r.dot(v)))) * 255.f); + color = Vec4b(ix, ix, ix, 0); } - }, nstripes); - } - // ---------------------------- - - static const bool display = false; - static const bool parallelCheck = false; - - class Settings - { - public: - Ptr params; - Ptr volume; - Ptr scene; - std::vector poses; - - Settings(bool useHashTSDF, bool onlySemisphere) - { - if (useHashTSDF) - params = kinfu::Params::hashTSDFParams(true); - else - params = kinfu::Params::coarseParams(); - - volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, - params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, - params->truncateThreshold, params->volumeDims); - scene = Scene::create(params->frameSize, params->intr, params->depthFactor, onlySemisphere); - poses = scene->getPoses(); + imgRow[x] = color; + } } - }; - - void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) + }, nstripes); +} +// ---------------------------- + +static const bool display = false; +static const bool parallelCheck = false; + +class Settings +{ +public: + Ptr params; + Ptr volume; + Ptr scene; + std::vector poses; + + Settings(bool useHashTSDF, bool onlySemisphere) + { + if (useHashTSDF) + params = kinfu::Params::hashTSDFParams(true); + else + params = kinfu::Params::coarseParams(); + + volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, + params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, + params->truncateThreshold, params->volumeDims); + + scene = Scene::create(params->frameSize, params->intr, params->depthFactor, onlySemisphere); + poses = scene->getPoses(); + } +}; + +void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) +{ + Mat image; + patchNaNs(points); + imshow("depth", depth * (1.f / depthFactor / 4.f)); + renderPointsNormals(points, normals, image, lightPose); + imshow("render", image); + waitKey(2000); +} + +void normalsCheck(Mat normals) +{ + Vec4f vector; + for (auto pvector = normals.begin(); pvector < normals.end(); pvector++) + { + vector = *pvector; + if (!cvIsNaN(vector[0])) { - Mat image; - patchNaNs(points); - imshow("depth", depth * (1.f / depthFactor / 4.f)); - renderPointsNormals(points, normals, image, lightPose); - imshow("render", image); - waitKey(2000); + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; } - - void normalsCheck(Mat normals) + } +} + +int counterOfValid(Mat points) +{ + Vec4f* v; + int i, j; + int count = 0; + for (i = 0; i < points.rows; ++i) + { + v = (points.ptr(i)); + for (j = 0; j < points.cols; ++j) { - Vec4f vector; - for (auto pvector = normals.begin(); pvector < normals.end(); pvector++) + if ((v[j])[0] != 0 || + (v[j])[1] != 0 || + (v[j])[2] != 0) { - vector = *pvector; - if (!cvIsNaN(vector[0])) - { - float length = vector[0] * vector[0] + - vector[1] * vector[1] + - vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; - } + count++; } } - - int counterOfValid(Mat points) + } + return count; +} + +void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) +{ + auto normalCheck = [](Vec4f& vector, const int*) + { + if (!cvIsNaN(vector[0])) { - Vec4f* v; - int i, j; - int count = 0; - for (i = 0; i < points.rows; ++i) - { - v = (points.ptr(i)); - for (j = 0; j < points.cols; ++j) - { - if ((v[j])[0] != 0 || - (v[j])[1] != 0 || - (v[j])[2] != 0) - { - count++; - } - } - } - return count; + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; } + }; - void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) - { - auto normalCheck = [](Vec4f& vector, const int*) - { - if (!cvIsNaN(vector[0])) - { - float length = vector[0] * vector[0] + - vector[1] * vector[1] + - vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; - } - }; + Settings settings(isHashTSDF, false); - Settings settings(isHashTSDF, false); + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _tmpnormals; + UMat _newPoints, _newNormals; + Mat points, normals; + AccessFlag af = ACCESS_READ; - Mat depth = settings.scene->depth(settings.poses[0]); - UMat _points, _normals, _tmpnormals; - UMat _newPoints, _newNormals; - Mat points, normals; - AccessFlag af = ACCESS_READ; + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); - settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); - - if (isRaycast) - { - settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); - } - if (isFetchPointsNormals) - { - settings.volume->fetchPointsNormals(_points, _normals); - } - if (isFetchNormals) - { - settings.volume->fetchPointsNormals(_points, _tmpnormals); - settings.volume->fetchNormals(_points, _normals); - } + if (isRaycast) + { + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); + } + if (isFetchPointsNormals) + { + settings.volume->fetchPointsNormals(_points, _normals); + } + if (isFetchNormals) + { + settings.volume->fetchPointsNormals(_points, _tmpnormals); + settings.volume->fetchNormals(_points, _normals); + } - normals = _normals.getMat(af); - points = _points.getMat(af); + normals = _normals.getMat(af); + points = _points.getMat(af); - if (parallelCheck) - normals.forEach(normalCheck); - else - normalsCheck(normals); + if (parallelCheck) + normals.forEach(normalCheck); + else + normalsCheck(normals); - if (isRaycast && display) - displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + if (isRaycast && display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - if (isRaycast) - { - settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); - normals = _newNormals.getMat(af); - points = _newPoints.getMat(af); - normalsCheck(normals); - - if (parallelCheck) - normals.forEach(normalCheck); - else - normalsCheck(normals); - - if (display) - displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - } + if (isRaycast) + { + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); + normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); + normalsCheck(normals); - points.release(); normals.release(); - } + if (parallelCheck) + normals.forEach(normalCheck); + else + normalsCheck(normals); - void valid_points_test(bool isHashTSDF) - { - Settings settings(isHashTSDF, true); - - Mat depth = settings.scene->depth(settings.poses[0]); - UMat _points, _normals, _newPoints, _newNormals; - AccessFlag af = ACCESS_READ; - Mat points, normals; - int anfas, profile; - - settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); - settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); - normals = _normals.getMat(af); - points = _points.getMat(af); - patchNaNs(points); - anfas = counterOfValid(points); - - if (display) - displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - - settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); - normals = _newNormals.getMat(af); - points = _newPoints.getMat(af); - patchNaNs(points); - profile = counterOfValid(points); - - if (display) - displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - - // TODO: why profile == 2*anfas ? - float percentValidity = float(anfas) / float(profile); - - ASSERT_NE(profile, 0) << "There is no points in profile"; - ASSERT_NE(anfas, 0) << "There is no points in anfas"; - ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; - } + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + } -#ifndef HAVE_OPENCL + points.release(); normals.release(); +} - TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } - TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } - TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } - TEST(TSDF, valid_points) { valid_points_test(false); } +void valid_points_test(bool isHashTSDF) +{ + Settings settings(isHashTSDF, true); - TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } - TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } - TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } - TEST(HashTSDF, valid_points) { valid_points_test(true); } + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _newPoints, _newNormals; + AccessFlag af = ACCESS_READ; + Mat points, normals; + int anfas, profile; -#else + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); + normals = _normals.getMat(af); + points = _points.getMat(af); + patchNaNs(points); + anfas = counterOfValid(points); - TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } - TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } - TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } - TEST(TSDF_GPU, valid_points) { valid_points_test(false); } + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } - TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } - TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } - TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); + normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); + patchNaNs(points); + profile = counterOfValid(points); - TEST(TSDF_CPU, raycast_normals) - { - cv::ocl::setUseOpenCL(false); - normal_test(false, true, false, false); - cv::ocl::setUseOpenCL(true); - } + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); - TEST(TSDF_CPU, fetch_points_normals) - { - cv::ocl::setUseOpenCL(false); - normal_test(false, false, true, false); - cv::ocl::setUseOpenCL(true); - } + // TODO: why profile == 2*anfas ? + float percentValidity = float(anfas) / float(profile); - TEST(TSDF_CPU, fetch_normals) - { - cv::ocl::setUseOpenCL(false); - normal_test(false, false, false, true); - cv::ocl::setUseOpenCL(true); - } + ASSERT_NE(profile, 0) << "There is no points in profile"; + ASSERT_NE(anfas, 0) << "There is no points in anfas"; + ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; +} - TEST(TSDF_CPU, valid_points) - { - cv::ocl::setUseOpenCL(false); - valid_points_test(false); - cv::ocl::setUseOpenCL(true); - } +#ifndef HAVE_OPENCL - TEST(HashTSDF_CPU, raycast_normals) - { - cv::ocl::setUseOpenCL(false); - normal_test(true, true, false, false); - cv::ocl::setUseOpenCL(true); - } +TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF, valid_points) { valid_points_test(false); } - TEST(HashTSDF_CPU, fetch_points_normals) - { - cv::ocl::setUseOpenCL(false); - normal_test(true, false, true, false); - cv::ocl::setUseOpenCL(true); - } +TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF, valid_points) { valid_points_test(true); } - TEST(HashTSDF_CPU, fetch_normals) - { - cv::ocl::setUseOpenCL(false); - normal_test(true, false, false, true); - cv::ocl::setUseOpenCL(true); - } +#else - TEST(HashTSDF_CPU, valid_points) - { - cv::ocl::setUseOpenCL(false); - valid_points_test(true); - cv::ocl::setUseOpenCL(true); - } +TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF_GPU, valid_points) { valid_points_test(false); } + +TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } + +TEST(TSDF_CPU, raycast_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(false, true, false, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(TSDF_CPU, fetch_points_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(false, false, true, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(TSDF_CPU, fetch_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(false, false, false, true); + cv::ocl::setUseOpenCL(true); +} + +TEST(TSDF_CPU, valid_points) +{ + cv::ocl::setUseOpenCL(false); + valid_points_test(false); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, raycast_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(true, true, false, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, fetch_points_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(true, false, true, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, fetch_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(true, false, false, true); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, valid_points) +{ + cv::ocl::setUseOpenCL(false); + valid_points_test(true); + cv::ocl::setUseOpenCL(true); +} #endif - } +} } // namespace From 4b2baf9db04f8e00af36dc342d7d862337a3e14e Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 29 Mar 2021 14:52:59 +0300 Subject: [PATCH 16/18] tsdf cpu getnormal fix --- modules/rgbd/src/tsdf.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 0a0fcfacf05..991d525b9d6 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -259,13 +259,13 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const const int dim = volDims[c]; float& nv = an[c]; - TsdfType vx[8]; + float vx[8]; for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; + vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1*dim].tsdf) - + tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1*dim].tsdf); - v_float32x4 v0246 = tsdfToFloat_INTR(v_int32x4(vx[0], vx[2], vx[4], vx[6])); - v_float32x4 v1357 = tsdfToFloat_INTR(v_int32x4(vx[1], vx[3], vx[5], vx[7])); + v_float32x4 v0246 (vx[0], vx[2], vx[4], vx[6]); + v_float32x4 v1357 (vx[1], vx[3], vx[5], vx[7]); v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); v_float32x4 v00_10 = vxx; @@ -312,9 +312,9 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(const Point3f& p) const float& nv = an[c]; float vx[8]; - for(int i = 0; i < 8; i++) - vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1*dim].tsdf); + for (int i = 0; i < 8; i++) + vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) - + tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf); float v00 = vx[0] + tz*(vx[1] - vx[0]); float v01 = vx[2] + tz*(vx[3] - vx[2]); From 01c93d7ca17bfde5fce805a63e5c4fe8a38924b9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 29 Mar 2021 17:09:30 +0300 Subject: [PATCH 17/18] add folder ocl with simple tests --- modules/rgbd/test/ocl/test_tsdf.cpp | 470 ++++++++++++++++++++++++++++ modules/rgbd/test/test_tsdf.cpp | 26 -- 2 files changed, 470 insertions(+), 26 deletions(-) create mode 100644 modules/rgbd/test/ocl/test_tsdf.cpp diff --git a/modules/rgbd/test/ocl/test_tsdf.cpp b/modules/rgbd/test/ocl/test_tsdf.cpp new file mode 100644 index 00000000000..1c55e1f4001 --- /dev/null +++ b/modules/rgbd/test/ocl/test_tsdf.cpp @@ -0,0 +1,470 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "../test_precomp.hpp" +#include "opencv2/ts/ocl_test.hpp" + +#ifdef HAVE_OPENCL + +namespace opencv_test { +namespace { + +using namespace cv; + +/** Reprojects screen point to camera space given z coord. */ +struct Reprojector +{ + Reprojector() {} + inline Reprojector(Matx33f intr) + { + fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1); + cx = intr(0, 2), cy = intr(1, 2); + } + template + inline cv::Point3_ operator()(cv::Point3_ p) const + { + T x = p.z * (p.x - cx) * fxinv; + T y = p.z * (p.y - cy) * fyinv; + return cv::Point3_(x, y, p.z); + } + + float fxinv, fyinv, cx, cy; +}; + +template +struct RenderInvoker : ParallelLoopBody +{ + RenderInvoker(Mat_& _frame, Affine3f _pose, + Reprojector _reproj, float _depthFactor, bool _onlySemisphere) + : ParallelLoopBody(), + frame(_frame), + pose(_pose), + reproj(_reproj), + depthFactor(_depthFactor), + onlySemisphere(_onlySemisphere) + { } + + virtual void operator ()(const cv::Range& r) const + { + for (int y = r.start; y < r.end; y++) + { + float* frameRow = frame[y]; + for (int x = 0; x < frame.cols; x++) + { + float pix = 0; + + Point3f orig = pose.translation(); + // direction through pixel + Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); + float xyt = 1.f / (screenVec.x * screenVec.x + + screenVec.y * screenVec.y + 1.f); + Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); + // screen space axis + dir.y = -dir.y; + + const float maxDepth = 20.f; + const float maxSteps = 256; + float t = 0.f; + for (int step = 0; step < maxSteps && t < maxDepth; step++) + { + Point3f p = orig + dir * t; + float d = Scene::map(p, onlySemisphere); + if (d < 0.000001f) + { + float depth = std::sqrt(t * t * xyt); + pix = depth * depthFactor; + break; + } + t += d; + } + + frameRow[x] = pix; + } + } + } + + Mat_& frame; + Affine3f pose; + Reprojector reproj; + float depthFactor; + bool onlySemisphere; +}; + +struct Scene +{ + virtual ~Scene() {} + static Ptr create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere); + virtual Mat depth(Affine3f pose) = 0; + virtual std::vector getPoses() = 0; +}; + +struct SemisphereScene : Scene +{ + const int framesPerCycle = 72; + const float nCycles = 0.25f; + const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f)); + + Size frameSize; + Matx33f intr; + float depthFactor; + bool onlySemisphere; + + SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere) + { } + + static float map(Point3f p, bool onlySemisphere) + { + float plane = p.y + 0.5f; + Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f); + float sphereRadius = 0.5f; + float sphere = (float)cv::norm(spherePose) - sphereRadius; + float sphereMinusBox = sphere; + + float subSphereRadius = 0.05f; + Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f); + float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius; + + float res; + if (!onlySemisphere) + res = min({ sphereMinusBox, subSphere, plane }); + else + res = sphereMinusBox; + + return res; + } + + Mat depth(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, onlySemisphere)); + + return std::move(frame); + } + + std::vector getPoses() override + { + std::vector poses; + for (int i = 0; i < framesPerCycle * nCycles; i++) + { + float angle = (float)(CV_2PI * i / framesPerCycle); + Affine3f pose; + pose = pose.rotate(startPose.rotation()); + pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); + pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), + startPose.translation()[1], + startPose.translation()[2] * cos(angle))); + poses.push_back(pose); + } + + return poses; + } + +}; + +Ptr Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) +{ + return makePtr(sz, _intr, _depthFactor, _onlySemisphere); +} + +// this is a temporary solution +// ---------------------------- + +typedef cv::Vec4f ptype; +typedef cv::Mat_< ptype > Points; +typedef Points Normals; +typedef Size2i Size; + +template +inline float specPow(float x) +{ + if (p % 2 == 0) + { + float v = specPow

(x); + return v * v; + } + else + { + float v = specPow<(p - 1) / 2>(x); + return v * v * x; + } +} + +template<> +inline float specPow<0>(float /*x*/) +{ + return 1.f; +} + +template<> +inline float specPow<1>(float x) +{ + return x; +} + +inline cv::Vec3f fromPtype(const ptype& x) +{ + return cv::Vec3f(x[0], x[1], x[2]); +} + +inline Point3f normalize(const Vec3f& v) +{ + double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + return v * (nv ? 1. / nv : 0.); +} + +void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose) +{ + Size sz = _points.size(); + image.create(sz, CV_8UC4); + + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + + Mat_ img = image.getMat(); + + Range range(0, sz.height); + const int nstripes = -1; + parallel_for_(range, [&](const Range&) + { + for (int y = range.start; y < range.end; y++) + { + Vec4b* imgRow = img[y]; + const ptype* ptsRow = points[y]; + const ptype* nrmRow = normals[y]; + + for (int x = 0; x < sz.width; x++) + { + Point3f p = fromPtype(ptsRow[x]); + Point3f n = fromPtype(nrmRow[x]); + + Vec4b color; + + if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z)) + { + color = Vec4b(0, 32, 0, 0); + } + else + { + const float Ka = 0.3f; //ambient coeff + const float Kd = 0.5f; //diffuse coeff + const float Ks = 0.2f; //specular coeff + const int sp = 20; //specular power + + const float Ax = 1.f; //ambient color, can be RGB + const float Dx = 1.f; //diffuse color, can be RGB + const float Sx = 1.f; //specular color, can be RGB + const float Lx = 1.f; //light color + + Point3f l = normalize(lightPose.translation() - Vec3f(p)); + Point3f v = normalize(-Vec3f(p)); + Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l)); + + uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) + + Lx * Ks * Sx * specPow(max(0.f, r.dot(v)))) * 255.f); + color = Vec4b(ix, ix, ix, 0); + } + + imgRow[x] = color; + } + } + }, nstripes); +} +// ---------------------------- + +static const bool display = false; +static const bool parallelCheck = false; + +class Settings +{ +public: + Ptr params; + Ptr volume; + Ptr scene; + std::vector poses; + + Settings(bool useHashTSDF, bool onlySemisphere) + { + if (useHashTSDF) + params = kinfu::Params::hashTSDFParams(true); + else + params = kinfu::Params::coarseParams(); + + volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, + params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, + params->truncateThreshold, params->volumeDims); + + scene = Scene::create(params->frameSize, params->intr, params->depthFactor, onlySemisphere); + poses = scene->getPoses(); + } +}; + +void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose) +{ + Mat image; + patchNaNs(points); + imshow("depth", depth * (1.f / depthFactor / 4.f)); + renderPointsNormals(points, normals, image, lightPose); + imshow("render", image); + waitKey(2000); +} + +void normalsCheck(Mat normals) +{ + Vec4f vector; + for (auto pvector = normals.begin(); pvector < normals.end(); pvector++) + { + vector = *pvector; + if (!cvIsNaN(vector[0])) + { + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + } + } +} + +int counterOfValid(Mat points) +{ + Vec4f* v; + int i, j; + int count = 0; + for (i = 0; i < points.rows; ++i) + { + v = (points.ptr(i)); + for (j = 0; j < points.cols; ++j) + { + if ((v[j])[0] != 0 || + (v[j])[1] != 0 || + (v[j])[2] != 0) + { + count++; + } + } + } + return count; +} + +void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) +{ + auto normalCheck = [](Vec4f& vector, const int*) + { + if (!cvIsNaN(vector[0])) + { + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + } + }; + + Settings settings(isHashTSDF, false); + + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _tmpnormals; + UMat _newPoints, _newNormals; + Mat points, normals; + AccessFlag af = ACCESS_READ; + + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); + + if (isRaycast) + { + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); + } + if (isFetchPointsNormals) + { + settings.volume->fetchPointsNormals(_points, _normals); + } + if (isFetchNormals) + { + settings.volume->fetchPointsNormals(_points, _tmpnormals); + settings.volume->fetchNormals(_points, _normals); + } + + normals = _normals.getMat(af); + points = _points.getMat(af); + + if (parallelCheck) + normals.forEach(normalCheck); + else + normalsCheck(normals); + + if (isRaycast && display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + + if (isRaycast) + { + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); + normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); + normalsCheck(normals); + + if (parallelCheck) + normals.forEach(normalCheck); + else + normalsCheck(normals); + + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + } + + points.release(); normals.release(); +} + +void valid_points_test(bool isHashTSDF) +{ + Settings settings(isHashTSDF, true); + + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _newPoints, _newNormals; + AccessFlag af = ACCESS_READ; + Mat points, normals; + int anfas, profile; + + settings.volume->integrate(depth, settings.params->depthFactor, settings.poses[0].matrix, settings.params->intr); + settings.volume->raycast(settings.poses[0].matrix, settings.params->intr, settings.params->frameSize, _points, _normals); + normals = _normals.getMat(af); + points = _points.getMat(af); + patchNaNs(points); + anfas = counterOfValid(points); + + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + + settings.volume->raycast(settings.poses[17].matrix, settings.params->intr, settings.params->frameSize, _newPoints, _newNormals); + normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); + patchNaNs(points); + profile = counterOfValid(points); + + if (display) + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + + // TODO: why profile == 2*anfas ? + float percentValidity = float(anfas) / float(profile); + + ASSERT_NE(profile, 0) << "There is no points in profile"; + ASSERT_NE(anfas, 0) << "There is no points in anfas"; + ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; +} + +TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF_GPU, valid_points) { valid_points_test(false); } + +TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } + +} +} // namespace + +#endif diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 2b26eeb2b37..de6d12f80c6 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -451,30 +451,6 @@ void valid_points_test(bool isHashTSDF) ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; } -#ifndef HAVE_OPENCL - -TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } -TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } -TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } -TEST(TSDF, valid_points) { valid_points_test(false); } - -TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } -TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } -TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } -TEST(HashTSDF, valid_points) { valid_points_test(true); } - -#else - -TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } -TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } -TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } -TEST(TSDF_GPU, valid_points) { valid_points_test(false); } - -TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } -TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } -TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } -TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } - TEST(TSDF_CPU, raycast_normals) { cv::ocl::setUseOpenCL(false); @@ -531,7 +507,5 @@ TEST(HashTSDF_CPU, valid_points) cv::ocl::setUseOpenCL(true); } -#endif - } } // namespace From c5ba998c1d994b5a13862ce98b07ce1080453be2 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 30 Mar 2021 15:25:20 +0300 Subject: [PATCH 18/18] build error fix --- modules/rgbd/test/test_tsdf.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index de6d12f80c6..31a137854c3 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -451,6 +451,17 @@ void valid_points_test(bool isHashTSDF) ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; } +#ifndef HAVE_OPENCL +TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF, valid_points) { valid_points_test(false); } + +TEST(HashTSDF, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF, valid_points) { valid_points_test(true); } +#else TEST(TSDF_CPU, raycast_normals) { cv::ocl::setUseOpenCL(false); @@ -506,6 +517,6 @@ TEST(HashTSDF_CPU, valid_points) valid_points_test(true); cv::ocl::setUseOpenCL(true); } - +#endif } } // namespace