diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index ebca91587d6..149093665ce 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,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; }; @@ -96,40 +98,36 @@ struct Scene struct SemisphereScene : 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 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) : - 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; - - 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); 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 +137,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); } @@ -152,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))); @@ -164,11 +162,117 @@ 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, _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 makePtr(sz, _intr, _depthFactor); + 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: @@ -188,11 +292,27 @@ 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(); } }; +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 = false; + PERF_TEST(Perf_TSDF, integrate) { Settings settings(false); @@ -220,6 +340,9 @@ PERF_TEST(Perf_TSDF, 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); } SANITY_CHECK_NOTHING(); } @@ -252,6 +375,9 @@ 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); } SANITY_CHECK_NOTHING(); } 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); 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]); 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 5a9b23afb95..31a137854c3 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -4,7 +4,8 @@ #include "test_precomp.hpp" -namespace opencv_test { namespace { +namespace opencv_test { +namespace { using namespace cv; @@ -32,12 +33,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 +66,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 +85,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; }; @@ -97,39 +100,35 @@ 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; 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; - - 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); 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 +138,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); } @@ -152,7 +151,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))); @@ -164,9 +163,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 @@ -242,7 +241,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im Vec4b color; - if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z) ) + if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z)) { color = Vec4b(0, 32, 0, 0); } @@ -277,6 +276,40 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im 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; @@ -288,29 +321,34 @@ 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"; } } } -void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) +int counterOfValid(Mat points) { - Ptr _params; - if (isHashTSDF) - _params = kinfu::Params::hashTSDFParams(true); - else - _params = kinfu::Params::coarseParams(); - - Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor); - std::vector poses = scene->getPoses(); - - Mat depth = scene->depth(poses[0]); - UMat _points, _normals, _tmpnormals; - UMat _newPoints, _newNormals; - Mat points, normals; - Mat image; - AccessFlag af = ACCESS_READ; + 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])) @@ -318,200 +356,167 @@ 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"; } }; - 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 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) { - 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) - { 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); - } + 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); + points = _newPoints.getMat(af); normalsCheck(normals); if (parallelCheck) - { normals.forEach(normalCheck); - } else - { normalsCheck(normals); - } - 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, settings.params->depthFactor, settings.params->lightPose); } 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; - if (isHashTSDF) - _params = kinfu::Params::hashTSDFParams(true); - else - _params = kinfu::Params::coarseParams(); + Settings settings(isHashTSDF, true); - Ptr scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor); - std::vector poses = scene->getPoses(); - - Mat depth = scene->depth(poses[0]); - UMat _points, _normals; - UMat _newPoints, _newNormals; + Mat depth = settings.scene->depth(settings.poses[0]); + UMat _points, _normals, _newPoints, _newNormals; + AccessFlag af = ACCESS_READ; Mat points, normals; - Mat image; 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); - volume->raycast(poses[0].matrix, _params->intr, _params->frameSize, _points, _normals); + 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); - } - - volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); + 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) - { - imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); - renderPointsNormals(points, normals, image, _params->lightPose); - imshow("render", image); - waitKey(2000); - } + displayImage(depth, points, normals, settings.params->depthFactor, settings.params->lightPose); + + // 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 << ")"; } -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_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); } - - -}} // namespace +#endif +} +} // namespace