From a423b07149414ce910cf8c7c84300af212d8eb07 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 24 Nov 2022 15:16:18 +0100 Subject: [PATCH] Merge pull request #22845 from savuor:volume_interface_etc Corresponding contrib PR: #3382@contrib Changes - Volume::raycast(): camera intrinsics can be explicitly passed to the function. If not, the ones from current volume settings are used - getVolumeDimensions() renamed to getVolumeStrides() because they are strides actually - TSDF tests: OpenCLStatusRevert and parametrized fixture - ColorTSDF::integrate(): extra RGB projector is redundant, removed - Minor changes --- .../3d/include/opencv2/3d/detail/submap.hpp | 8 +- modules/3d/include/opencv2/3d/volume.hpp | 9 +- .../3d/include/opencv2/3d/volume_settings.hpp | 16 +- modules/3d/perf/perf_tsdf.cpp | 115 ++-- modules/3d/src/rgbd/color_tsdf_functions.cpp | 98 +-- modules/3d/src/rgbd/color_tsdf_functions.hpp | 33 +- modules/3d/src/rgbd/hash_tsdf_functions.cpp | 36 +- modules/3d/src/rgbd/hash_tsdf_functions.hpp | 4 +- modules/3d/src/rgbd/tsdf_functions.cpp | 57 +- modules/3d/src/rgbd/tsdf_functions.hpp | 4 +- modules/3d/src/rgbd/volume_impl.cpp | 32 +- modules/3d/src/rgbd/volume_impl.hpp | 14 +- modules/3d/src/rgbd/volume_settings_impl.cpp | 65 +- modules/3d/test/ocl/test_tsdf.cpp | 36 +- modules/3d/test/test_tsdf.cpp | 646 ++++++++++-------- 15 files changed, 616 insertions(+), 557 deletions(-) diff --git a/modules/3d/include/opencv2/3d/detail/submap.hpp b/modules/3d/include/opencv2/3d/detail/submap.hpp index 2a0dd37868..87a738afb4 100644 --- a/modules/3d/include/opencv2/3d/detail/submap.hpp +++ b/modules/3d/include/opencv2/3d/detail/submap.hpp @@ -52,7 +52,7 @@ public: virtual ~Submap() = default; virtual void integrate(InputArray _depth, const int currframeId); - virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, + virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, cv::Matx33f K, OutputArray points = noArray(), OutputArray normals = noArray()); virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); }; @@ -112,14 +112,14 @@ void Submap::integrate(InputArray _depth, const int currFrameId) } template -void Submap::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, +void Submap::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, cv::Matx33f K, OutputArray points, OutputArray normals) { if (!points.needed() && !normals.needed()) { MatType pts, nrm; //TODO: get depth instead of pts from raycast - volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, pts, nrm); + volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, pts, nrm); std::vector pch(3); split(pts, pch); @@ -130,7 +130,7 @@ void Submap::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSiz } else { - volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, points, normals); + volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, points, normals); } } diff --git a/modules/3d/include/opencv2/3d/volume.hpp b/modules/3d/include/opencv2/3d/volume.hpp index 4e687d559e..3c9bf8ebe8 100644 --- a/modules/3d/include/opencv2/3d/volume.hpp +++ b/modules/3d/include/opencv2/3d/volume.hpp @@ -22,7 +22,7 @@ public: * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF]. * @param settings the custom settings for volume. */ - Volume(VolumeType vtype, const VolumeSettings& settings); + Volume(VolumeType vtype, VolumeSettings settings); virtual ~Volume(); /** @brief Integrates the input data to the volume. @@ -73,13 +73,14 @@ public: Rendered image size and camera intrinsics are taken from volume settings structure. * @param cameraPose the pose of camera in global coordinates. - * @param height the height of result image. - * @param width the width of result image. + * @param height the height of result image + * @param width the width of result image + * @param K camera raycast intrinsics * @param points image to store rendered points. * @param normals image to store rendered normals corresponding to points. * @param colors image to store rendered colors corresponding to points (only for ColorTSDF). */ - void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const; + void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const; /** @brief Extract the all data from volume. * @param points the input exist point. diff --git a/modules/3d/include/opencv2/3d/volume_settings.hpp b/modules/3d/include/opencv2/3d/volume_settings.hpp index f5477d2ee0..b57e647756 100644 --- a/modules/3d/include/opencv2/3d/volume_settings.hpp +++ b/modules/3d/include/opencv2/3d/volume_settings.hpp @@ -54,21 +54,21 @@ public: int getIntegrateHeight() const; - /** @brief Sets the width of the raycasted image. + /** @brief Sets the width of the raycasted image, used when user does not provide it at raycast() call. * @param val input value. */ void setRaycastWidth(int val); - /** @brief Returns the width of the raycasted image. + /** @brief Returns the width of the raycasted image, used when user does not provide it at raycast() call. */ int getRaycastWidth() const; - /** @brief Sets the height of the raycasted image. + /** @brief Sets the height of the raycasted image, used when user does not provide it at raycast() call. * @param val input value. */ void setRaycastHeight(int val); - /** @brief Returns the height of the raycasted image. + /** @brief Returns the height of the raycasted image, used when user does not provide it at raycast() call. */ int getRaycastHeight() const; @@ -159,10 +159,10 @@ public: void getVolumeResolution(OutputArray val) const; /** @brief Returns 3 integers representing strides by x, y and z dimension. - Can be used to iterate over volume unit raw data. + Can be used to iterate over raw volume unit data. * @param val output value. */ - void getVolumeDimensions(OutputArray val) const; + void getVolumeStrides(OutputArray val) const; /** @brief Sets intrinsics of camera for integrations. * Format of input: @@ -184,7 +184,7 @@ public: */ void getCameraIntegrateIntrinsics(OutputArray val) const; - /** @brief Sets intrinsics of camera for raycast image. + /** @brief Sets camera intrinsics for raycast image which, used when user does not provide them at raycast() call. * Format of input: * [ fx 0 cx ] * [ 0 fy cy ] @@ -194,7 +194,7 @@ public: */ void setCameraRaycastIntrinsics(InputArray val); - /** @brief Returns intrinsics of camera for raycast image. + /** @brief Returns camera intrinsics for raycast image, used when user does not provide them at raycast() call. * Format of output: * [ fx 0 cx ] * [ 0 fy cy ] diff --git a/modules/3d/perf/perf_tsdf.cpp b/modules/3d/perf/perf_tsdf.cpp index 86cfda991b..99847a4d18 100644 --- a/modules/3d/perf/perf_tsdf.cpp +++ b/modules/3d/perf/perf_tsdf.cpp @@ -428,11 +428,12 @@ PERF_TEST(Perf_TSDF, integrate_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -460,11 +461,12 @@ PERF_TEST(Perf_TSDF, integrate_frame) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -492,12 +494,13 @@ PERF_TEST(Perf_TSDF, raycast_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -509,7 +512,7 @@ PERF_TEST(Perf_TSDF, raycast_mat) volume.integrate(depth, pose); startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, points, normals); + volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals); stopTimer(); if (display) @@ -531,11 +534,12 @@ PERF_TEST(Perf_TSDF_CPU, integrate_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); @@ -564,11 +568,12 @@ PERF_TEST(Perf_TSDF_CPU, integrate_frame) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -597,12 +602,13 @@ PERF_TEST(Perf_TSDF_CPU, raycast_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -614,7 +620,7 @@ PERF_TEST(Perf_TSDF_CPU, raycast_mat) volume.integrate(depth, pose); startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, points, normals); + volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals); stopTimer(); if (display) @@ -640,11 +646,12 @@ PERF_TEST(Perf_HashTSDF, integrate_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -672,11 +679,12 @@ PERF_TEST(Perf_HashTSDF, integrate_frame) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -705,12 +713,13 @@ PERF_TEST(Perf_HashTSDF, raycast_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -722,7 +731,7 @@ PERF_TEST(Perf_HashTSDF, raycast_mat) volume.integrate(depth, pose); startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, points, normals); + volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals); stopTimer(); if (display) @@ -743,11 +752,12 @@ PERF_TEST(Perf_HashTSDF_CPU, integrate_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); @@ -776,11 +786,12 @@ PERF_TEST(Perf_HashTSDF_CPU, integrate_frame) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -809,12 +820,13 @@ PERF_TEST(Perf_HashTSDF_CPU, raycast_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -826,7 +838,7 @@ PERF_TEST(Perf_HashTSDF_CPU, raycast_mat) volume.integrate(depth, pose); startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, points, normals); + volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals); stopTimer(); if (display) @@ -851,11 +863,12 @@ PERF_TEST(Perf_ColorTSDF, integrate_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -884,11 +897,12 @@ PERF_TEST(Perf_ColorTSDF, integrate_frame) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -918,12 +932,13 @@ PERF_TEST(Perf_ColorTSDF, raycast_mat) Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); for (size_t i = 0; i < poses.size(); i++) @@ -937,7 +952,7 @@ PERF_TEST(Perf_ColorTSDF, raycast_mat) volume.integrate(depth, rgb, pose); startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, points, normals, colors); + volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals, colors); stopTimer(); if (display) diff --git a/modules/3d/src/rgbd/color_tsdf_functions.cpp b/modules/3d/src/rgbd/color_tsdf_functions.cpp index d6827d75c4..44111d1880 100644 --- a/modules/3d/src/rgbd/color_tsdf_functions.cpp +++ b/modules/3d/src/rgbd/color_tsdf_functions.cpp @@ -13,7 +13,7 @@ namespace cv { void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, - InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume) + InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume) { Matx44f volumePose; settings.getVolumePose(volumePose); @@ -21,11 +21,10 @@ void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& } -void integrateColorTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, - InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume) +void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, + InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume) { - //std::cout << "integrateColorTsdfVolumeUnit()" << std::endl; + CV_TRACE_FUNCTION(); Depth depth = _depth.getMat(); Colors color = _rgb.getMat(); @@ -35,7 +34,7 @@ void integrateColorTsdfVolumeUnit( RGBTsdfVoxel* volDataStart = volume.ptr(); Vec4i volStrides; - settings.getVolumeDimensions(volStrides); + settings.getVolumeStrides(volStrides); Vec3i resolution; settings.getVolumeResolution(resolution); @@ -49,10 +48,6 @@ void integrateColorTsdfVolumeUnit( settings.getCameraIntegrateIntrinsics(intr); const Intr::Projector projDepth = Intr(intr).makeProjector(); - Matx33f rgb_intr; - settings.getCameraIntegrateIntrinsics(rgb_intr); - const Intr::Projector projColor = Intr(rgb_intr); - const float dfac(1.f / settings.getDepthFactor()); const float truncDist = settings.getTsdfTruncateDistance(); const float truncDistInv = 1.f / truncDist; @@ -70,7 +65,6 @@ void integrateColorTsdfVolumeUnit( v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); v_float32x4 vfxy(projDepth.fx, projDepth.fy, 0.f, 0.f), vcxy(projDepth.cx, projDepth.cy, 0.f, 0.f); - v_float32x4 rgb_vfxy(projColor.fx, projColor.fy, 0.f, 0.f), rgb_vcxy(projColor.cx, projColor.cy, 0.f, 0.f); const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0)); for (int x = range.start; x < range.end; x++) @@ -128,7 +122,7 @@ void integrateColorTsdfVolumeUnit( v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy); // leave only first 2 lanes projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & - v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); + v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); depthType v; // bilinearly interpolate depth at projected @@ -136,7 +130,7 @@ void integrateColorTsdfVolumeUnit( const v_float32x4& pt = projected; // check coords >= 0 and < imgSize v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) | - v_reinterpret_as_u32(pt >= upLimits); + v_reinterpret_as_u32(pt >= upLimits); limits = limits | v_rotate_right<1>(limits); if (limits.get0()) continue; @@ -177,23 +171,15 @@ void integrateColorTsdfVolumeUnit( continue; } - v_float32x4 projectedRGB = v_muladd(camPixVec, rgb_vfxy, rgb_vcxy); - // leave only first 2 lanes - projectedRGB = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & - v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); - // norm(camPixVec) produces double which is too slow int _u = (int)projected.get0(); int _v = (int)v_rotate_right<1>(projected).get0(); - int rgb_u = (int)projectedRGB.get0(); - int rgb_v = (int)v_rotate_right<1>(projectedRGB).get0(); - if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows && - rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols)) + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) continue; float pixNorm = pixNorms.at(_v, _u); // TODO: Add support of 3point and 4 point representation - Vec3f colorRGB = color.at(rgb_v, rgb_u); + Vec3f colorRGB = color.at(_v, _u); //float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); // difference between distances of point and of surface to camera float sdf = pixNorm * (v * dfac - zCamSpace); @@ -283,7 +269,6 @@ void integrateColorTsdfVolumeUnit( Point3f camPixVec; Point2f projected = projDepth(camSpacePt, camPixVec); - Point2f projectedRGB = projColor(camSpacePt, camPixVec); depthType v = bilinearDepth(depth, projected); if (v == 0) { @@ -292,17 +277,12 @@ void integrateColorTsdfVolumeUnit( int _u = projected.x; int _v = projected.y; - int rgb_u = (int)projectedRGB.x; - int rgb_v = (int)projectedRGB.y; - - if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows - && rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols - )) + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) continue; float pixNorm = pixNorms.at(_v, _u); // TODO: Add support of 3point and 4 point representation - Vec3f colorRGB = color.at(rgb_v, rgb_u); + Vec3f colorRGB = color.at(_v, _u); // difference between distances of point and of surface to camera float sdf = pixNorm * (v * dfac - camSpacePt.z); @@ -337,10 +317,6 @@ void integrateColorTsdfVolumeUnit( }; #endif parallel_for_(integrateRange, IntegrateInvoker); - //IntegrateInvoker(integrateRange); - - //std::cout << "integrateColorTsdfVolumeUnit() end" << std::endl; - } @@ -349,8 +325,8 @@ void integrateColorTsdfVolumeUnit( // all coordinate checks should be done in inclosing cycle inline float interpolateColorVoxel(const Mat& volume, - const Vec4i& volDims, const Vec8i& neighbourCoords, - const v_float32x4& p) + const Vec4i& volDims, const Vec8i& neighbourCoords, + const v_float32x4& p) { // tx, ty, tz = floor(p) v_int32x4 ip = v_floor(p); @@ -392,8 +368,8 @@ inline float interpolateColorVoxel(const Mat& volume, } inline float interpolateColorVoxel(const Mat& volume, - const Vec4i& volDims, const Vec8i& neighbourCoords, - const Point3f& _p) + const Vec4i& volDims, const Vec8i& neighbourCoords, + const Point3f& _p) { v_float32x4 p(_p.x, _p.y, _p.z, 0); return interpolateColorVoxel(volume, volDims, neighbourCoords, p); @@ -402,8 +378,8 @@ inline float interpolateColorVoxel(const Mat& volume, #else inline float interpolateColorVoxel(const Mat& volume, - const Vec4i& volDims, const Vec8i& neighbourCoords, - const Point3f& p) + const Vec4i& volDims, const Vec8i& neighbourCoords, + const Point3f& p) { int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; @@ -440,8 +416,8 @@ inline float interpolateColorVoxel(const Mat& volume, //gradientDeltaFactor is fixed at 1.0 of voxel size inline v_float32x4 getNormalColorVoxel(const Mat& volume, - const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, - const v_float32x4& p) + const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, + const v_float32x4& p) { if (v_check_any(p < v_float32x4(1.f, 1.f, 1.f, 0.f)) || v_check_any(p >= v_float32x4((float)(volResolution.x - 2), @@ -501,8 +477,8 @@ inline v_float32x4 getNormalColorVoxel(const Mat& volume, } inline Point3f getNormalColorVoxel(const Mat& volume, - const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, - const Point3f& _p) + const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, + const Point3f& _p) { v_float32x4 p(_p.x, _p.y, _p.z, 0.f); v_float32x4 result = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, p); @@ -512,8 +488,8 @@ inline Point3f getNormalColorVoxel(const Mat& volume, } #else inline Point3f getNormalColorVoxel(const Mat& volume, - const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, - const Point3f& p) + const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, + const Point3f& p) { int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const RGBTsdfVoxel* volData = volume.ptr(); @@ -712,16 +688,17 @@ inline Point3f getColorVoxel(const Mat& volume, #endif - - -void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, - InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors) +void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose, + int height, int width, InputArray intr, + InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors) { - //std::cout << "raycastColorTsdfVolumeUnit()" << std::endl; + CV_TRACE_FUNCTION(); Size frameSize(width, height); CV_Assert(frameSize.area() > 0); + Matx33f mintr(intr.getMat()); + _points.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); _colors.create(frameSize, COLOR_TYPE); @@ -731,7 +708,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& c Colors colors = _colors.getMat(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -748,9 +725,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& c const Point3i volResolution = Point3i(resolution); const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize(); - Matx33f intr; - settings.getCameraRaycastIntrinsics(intr); - const Intr::Reprojector reprojDepth = Intr(intr).makeReprojector(); + const Intr::Reprojector reprojDepth = Intr(mintr).makeReprojector(); Matx44f _pose; settings.getVolumePose(_pose); @@ -1027,17 +1002,12 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& c #endif parallel_for_(raycastRange, RaycastInvoker); - //RaycastInvoker(raycastRange); - - //std::cout << "raycastColorTsdfVolumeUnit() end" << std::endl; } void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, - InputArray _points, OutputArray _normals) + InputArray _points, OutputArray _normals) { - //std::cout << "fetchNormalsFromColorTsdfVolumeUnit" << std::endl; - CV_TRACE_FUNCTION(); CV_Assert(!_points.empty()); if (!_normals.needed()) @@ -1058,7 +1028,7 @@ void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputAr float voxelSizeInv = 1.f / settings.getVoxelSize(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -1171,7 +1141,7 @@ void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings& setti float voxelSizeInv = 1.f / settings.getVoxelSize(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), diff --git a/modules/3d/src/rgbd/color_tsdf_functions.hpp b/modules/3d/src/rgbd/color_tsdf_functions.hpp index 5d99784278..217033e8f9 100644 --- a/modules/3d/src/rgbd/color_tsdf_functions.hpp +++ b/modules/3d/src/rgbd/color_tsdf_functions.hpp @@ -17,26 +17,19 @@ namespace cv { - -void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, - InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume); - -void integrateColorTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, - InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume); - -void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, - InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors); - -void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, - InputArray _points, OutputArray _normals); - -void fetchPointsNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, - OutputArray _points, OutputArray _normals); - -void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, - OutputArray _points, OutputArray _normals, OutputArray _colors); - +void integrateColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose, + InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume); +void integrateColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &volumePose, const Matx44f &cameraPose, + InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume); +void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose, + int height, int width, InputArray intr, + InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors); +void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume, + InputArray _points, OutputArray _normals); +void fetchPointsNormalsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume, + OutputArray _points, OutputArray _normals); +void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume, + OutputArray _points, OutputArray _normals, OutputArray _colors); } // namespace cv diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.cpp b/modules/3d/src/rgbd/hash_tsdf_functions.cpp index c2a689233f..7b2fefe788 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.cpp +++ b/modules/3d/src/rgbd/hash_tsdf_functions.cpp @@ -497,7 +497,7 @@ void ocl_integrateHashTsdfVolumeUnit( const Intr intrinsics(intr); Vec4i volStrides; - settings.getVolumeDimensions(volStrides); + settings.getVolumeStrides(volStrides); Vec3i resolution; settings.getVolumeResolution(resolution); @@ -988,15 +988,15 @@ Point3f ocl_getNormalVoxel( #endif void raycastHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals) { - //std::cout << "raycastHashTsdfVolumeUnit()" << std::endl; - CV_TRACE_FUNCTION(); Size frameSize(width, height); CV_Assert(frameSize.area() > 0); + Matx33f mintr(intr.getMat()); + Mat volUnitsData = _volUnitsData.getMat(); _points.create(frameSize, POINT_TYPE); @@ -1016,7 +1016,7 @@ void raycastHashTsdfVolumeUnit( const float voxelSizeInv = 1.f / voxelSize; const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); Vec3i resolution; settings.getVolumeResolution(resolution); const Point3i volResolution = Point3i(resolution); @@ -1028,9 +1028,7 @@ void raycastHashTsdfVolumeUnit( const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); - Matx33f intr; - settings.getCameraRaycastIntrinsics(intr); - const Intr intrinsics(intr); + const Intr intrinsics(mintr); const Intr::Reprojector reproj(intrinsics.makeReprojector()); const int nstripes = -1; @@ -1094,7 +1092,6 @@ void raycastHashTsdfVolumeUnit( stepSize = tstep; } - //std::cout << prevTsdf << " " << currTsdf << " " << currWeight << std::endl; //! Surface crossing if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { @@ -1124,20 +1121,20 @@ void raycastHashTsdfVolumeUnit( }; parallel_for_(Range(0, points.rows), _HashRaycastInvoker, nstripes); - - //std::cout << "raycastHashTsdfVolumeUnit() end" << std::endl; } #ifdef HAVE_OPENCL void ocl_raycastHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals) { CV_TRACE_FUNCTION(); Size frameSize(width, height); CV_Assert(frameSize.area() > 0); + Matx33f mintr(intr.getMat()); + UMat volUnitsData = _volUnitsData.getUMat(); String errorStr; @@ -1156,9 +1153,7 @@ void ocl_raycastHashTsdfVolumeUnit( UMat points = _points.getUMat(); UMat normals = _normals.getUMat(); - Matx33f intr; - settings.getCameraRaycastIntrinsics(intr); - Intr intrinsics(intr); + Intr intrinsics(mintr); Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); @@ -1170,7 +1165,7 @@ void ocl_raycastHashTsdfVolumeUnit( const float voxelSizeInv = 1.f / voxelSize; const Vec4i volStrides; - settings.getVolumeDimensions(volStrides); + settings.getVolumeStrides(volStrides); Vec3i resolution; settings.getVolumeResolution(resolution); const Point3i volResolution = Point3i(resolution); @@ -1223,6 +1218,7 @@ void ocl_raycastHashTsdfVolumeUnit( } #endif + void fetchNormalsFromHashTsdfVolumeUnit( const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, const int volumeUnitDegree, InputArray _points, OutputArray _normals) @@ -1243,7 +1239,7 @@ void fetchNormalsFromHashTsdfVolumeUnit( const float voxelSizeInv = 1.f / voxelSize; const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); Matx44f _pose; settings.getVolumePose(_pose); @@ -1288,7 +1284,7 @@ void olc_fetchNormalsFromHashTsdfVolumeUnit( const float voxelSizeInv = 1.f / voxelSize; const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); Matx44f _pose; settings.getVolumePose(_pose); @@ -1334,7 +1330,7 @@ void fetchPointsNormalsFromHashTsdfVolumeUnit( const float volumeUnitSize = voxelSize * resolution[0]; const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); std::vector totalVolUnits; for (const auto& keyvalue : volumeUnits) @@ -1461,7 +1457,7 @@ void ocl_fetchPointsNormalsFromHashTsdfVolumeUnit( const float volumeUnitSize = voxelSize * resolution[0]; const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); Range _fetchRange(0, hashTable.last); diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.hpp b/modules/3d/src/rgbd/hash_tsdf_functions.hpp index 9da904e6d0..70826dbb94 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.hpp +++ b/modules/3d/src/rgbd/hash_tsdf_functions.hpp @@ -290,7 +290,7 @@ void integrateHashTsdfVolumeUnit( InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits); void raycastHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals); void fetchNormalsFromHashTsdfVolumeUnit( @@ -307,7 +307,7 @@ void ocl_integrateHashTsdfVolumeUnit( InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags); void ocl_raycastHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals); void olc_fetchNormalsFromHashTsdfVolumeUnit( diff --git a/modules/3d/src/rgbd/tsdf_functions.cpp b/modules/3d/src/rgbd/tsdf_functions.cpp index e935dcae12..2fdbc78e99 100644 --- a/modules/3d/src/rgbd/tsdf_functions.cpp +++ b/modules/3d/src/rgbd/tsdf_functions.cpp @@ -13,7 +13,7 @@ namespace cv { void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm) { - //std::cout << "preCalculationPixNorm" << std::endl; + CV_TRACE_FUNCTION(); Point2f fl(intrinsics.fx, intrinsics.fy); Point2f pp(intrinsics.cx, intrinsics.cy); @@ -67,7 +67,7 @@ void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volu TsdfVoxel* volDataStart = volume.ptr(); Vec4i volStrides; - settings.getVolumeDimensions(volStrides); + settings.getVolumeStrides(volStrides); Vec3i resolution; settings.getVolumeResolution(resolution); @@ -330,12 +330,9 @@ void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volu #ifdef HAVE_OPENCL void ocl_integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, - InputArray _depth, InputArray _pixNorms, InputArray _volume) + InputArray _depth, InputArray _pixNorms, InputArray _volume) { - //std::cout << "ocl_integrateTsdfVolumeUnit" << std::endl; - CV_TRACE_FUNCTION(); - //CV_UNUSED(frameId); CV_Assert(!_depth.empty()); UMat depth = _depth.getUMat(); @@ -369,7 +366,7 @@ void ocl_integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& Intr intrinsics(intr); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const float voxelSize = settings.getVoxelSize(); const float truncatedDistance = settings.getTsdfTruncateDistance(); @@ -617,13 +614,17 @@ inline Point3f getNormalVoxel( const Mat& volume, } #endif -void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, - InputArray _volume, OutputArray _points, OutputArray _normals) +void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, + int height, int width, InputArray intr, + InputArray _volume, OutputArray _points, OutputArray _normals) { - //std::cout << "raycastVolumeUnit" << std::endl; + CV_TRACE_FUNCTION(); const Size frameSize(width, height); - //CV_Assert(frameSize.area() > 0); + CV_Assert(frameSize.area() > 0); + + Matx33f mintr(intr.getMat()); + _points.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); @@ -631,7 +632,7 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera Normals normals = _normals.getMat(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -648,9 +649,6 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera const Point3i volResolution = Point3i(resolution); const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize(); - Matx33f intr; - settings.getCameraRaycastIntrinsics(intr); - Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose = Affine3f(_pose); @@ -663,7 +661,7 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera const Mat volume = _volume.getMat(); float voxelSize = settings.getVoxelSize(); float voxelSizeInv = 1.0f / voxelSize; - const Intr::Reprojector reproj = Intr(intr).makeReprojector(); + const Intr::Reprojector reproj = Intr(mintr).makeReprojector(); float tstep = settings.getTsdfTruncateDistance() * settings.getRaycastStepFactor(); Range raycastRange = Range(0, points.rows); @@ -917,21 +915,21 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera #endif parallel_for_(raycastRange, RaycastInvoker); - //RaycastInvoker(raycastRange); } #ifdef HAVE_OPENCL -void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, - InputArray _volume, OutputArray _points, OutputArray _normals) +void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, + int height, int width, InputArray intr, + InputArray _volume, OutputArray _points, OutputArray _normals) { - //std::cout << "ocl_raycastVolumeUnit" << std::endl; - CV_TRACE_FUNCTION(); const Size frameSize(width, height); CV_Assert(frameSize.area() > 0); + Matx33f mintr(intr.getMat()); + String errorStr; String name = "raycast"; ocl::ProgramSource source = ocl::_3d::tsdf_oclsrc; @@ -949,7 +947,7 @@ void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& ca UMat normals = _normals.getUMat(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -975,9 +973,8 @@ void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& ca Affine3f cam2vol = pose.inv() * Affine3f(cameraPose); Mat(cam2vol.matrix).copyTo(cam2volGpu); Mat(vol2cam.matrix).copyTo(vol2camGpu); - Matx33f intr; - settings.getCameraRaycastIntrinsics(intr); - Intr intrinsics(intr); + + Intr intrinsics(mintr); Intr::Reprojector r = intrinsics.makeReprojector(); const UMat volume = _volume.getUMat(); @@ -1023,8 +1020,6 @@ void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& ca void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, InputArray _points, OutputArray _normals) { - //std::cout << "fetchNormalsFromTsdfVolumeUnit" << std::endl; - CV_TRACE_FUNCTION(); CV_Assert(!_points.empty()); if (!_normals.needed()) @@ -1046,7 +1041,7 @@ void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _ float voxelSizeInv = 1.f / settings.getVoxelSize(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -1099,7 +1094,7 @@ void ocl_fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArr float voxelSizeInv = 1.f / settings.getVoxelSize(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -1222,7 +1217,7 @@ void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputA float voxelSizeInv = 1.f / settings.getVoxelSize(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), @@ -1320,7 +1315,7 @@ void ocl_fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, In float voxelSizeInv = 1.f / settings.getVoxelSize(); const Vec4i volDims; - settings.getVolumeDimensions(volDims); + settings.getVolumeStrides(volDims); const Vec8i neighbourCoords = Vec8i( volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), diff --git a/modules/3d/src/rgbd/tsdf_functions.hpp b/modules/3d/src/rgbd/tsdf_functions.hpp index 9294dc1eb5..5cc6b68dd6 100644 --- a/modules/3d/src/rgbd/tsdf_functions.hpp +++ b/modules/3d/src/rgbd/tsdf_functions.hpp @@ -175,7 +175,7 @@ void integrateTsdfVolumeUnit( InputArray _depth, InputArray _pixNorms, InputArray _volume); -void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, +void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, InputArray _volume, OutputArray _points, OutputArray _normals); void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, @@ -191,7 +191,7 @@ void ocl_integrateTsdfVolumeUnit( InputArray _depth, InputArray _pixNorms, InputArray _volume); void ocl_raycastTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, + const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, InputArray _volume, OutputArray _points, OutputArray _normals); void ocl_fetchNormalsFromTsdfVolumeUnit( diff --git a/modules/3d/src/rgbd/volume_impl.cpp b/modules/3d/src/rgbd/volume_impl.cpp index 43d84cdeca..5c62abb6f7 100644 --- a/modules/3d/src/rgbd/volume_impl.cpp +++ b/modules/3d/src/rgbd/volume_impl.cpp @@ -99,11 +99,13 @@ void TsdfVolume::integrate(InputArray, InputArray, InputArray) void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const { - raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); + Matx33f intr; + settings.getCameraRaycastIntrinsics(intr); + raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors); } -void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const +void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const { if (_colors.needed()) CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors"); @@ -113,12 +115,12 @@ void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputAr const Matx44f cameraPose = _cameraPose.getMat(); #ifndef HAVE_OPENCL - raycastTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals); + raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, volume, _points, _normals); #else if (useGPU) - ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, gpu_volume, _points, _normals); + ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, gpu_volume, _points, _normals); else - raycastTsdfVolumeUnit(settings, cameraPose, height, width, cpu_volume, _points, _normals); + raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, cpu_volume, _points, _normals); #endif } @@ -293,11 +295,13 @@ void HashTsdfVolume::integrate(InputArray, InputArray, InputArray) void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const { - raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); + Matx33f intr; + settings.getCameraRaycastIntrinsics(intr); + raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors); } -void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const +void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const { if (_colors.needed()) CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors"); @@ -305,12 +309,12 @@ void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, Outp const Matx44f cameraPose = _cameraPose.getMat(); #ifndef HAVE_OPENCL - raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals); + raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals); #else if (useGPU) - ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals); + ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals); else - raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals); + raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals); #endif } @@ -525,13 +529,15 @@ void ColorTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const { - raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); + Matx33f intr; + settings.getCameraRaycastIntrinsics(intr); + raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors); } -void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const +void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const { const Matx44f cameraPose = _cameraPose.getMat(); - raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals, _colors); + raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, intr, volume, _points, _normals, _colors); } void ColorTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const diff --git a/modules/3d/src/rgbd/volume_impl.hpp b/modules/3d/src/rgbd/volume_impl.hpp index 01267b5fde..fffe0a31be 100644 --- a/modules/3d/src/rgbd/volume_impl.hpp +++ b/modules/3d/src/rgbd/volume_impl.hpp @@ -27,7 +27,7 @@ public: virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0; - virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const = 0; + virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const = 0; virtual void fetchNormals(InputArray points, OutputArray normals) const = 0; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0; @@ -42,7 +42,7 @@ public: virtual bool getEnableGrowth() const = 0; public: - const VolumeSettings& settings; + VolumeSettings settings; #ifdef HAVE_OPENCL const bool useGPU; #endif @@ -59,7 +59,7 @@ public: virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; + virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -113,7 +113,7 @@ public: virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; + virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -177,7 +177,7 @@ public: virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; + virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -216,7 +216,7 @@ Volume::Volume() VolumeSettings settings; this->impl = makePtr(settings); } -Volume::Volume(VolumeType vtype, const VolumeSettings& settings) +Volume::Volume(VolumeType vtype, VolumeSettings settings) { switch (vtype) { @@ -240,7 +240,7 @@ void Volume::integrate(const OdometryFrame& frame, InputArray pose) { this->impl void Volume::integrate(InputArray depth, InputArray pose) { this->impl->integrate(depth, pose); } void Volume::integrate(InputArray depth, InputArray image, InputArray pose) { this->impl->integrate(depth, image, pose); } void Volume::raycast(InputArray cameraPose, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, _points, _normals, _colors); } -void Volume::raycast(InputArray cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _points, _normals, _colors); } +void Volume::raycast(InputArray cameraPose, int height, int width, InputArray _intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _intr, _points, _normals, _colors); } void Volume::fetchNormals(InputArray points, OutputArray normals) const { this->impl->fetchNormals(points, normals); } void Volume::fetchPointsNormals(OutputArray points, OutputArray normals) const { this->impl->fetchPointsNormals(points, normals); } void Volume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const { this->impl->fetchPointsNormalsColors(points, normals, colors); }; diff --git a/modules/3d/src/rgbd/volume_settings_impl.cpp b/modules/3d/src/rgbd/volume_settings_impl.cpp index fa60360d50..eea64716df 100644 --- a/modules/3d/src/rgbd/volume_settings_impl.cpp +++ b/modules/3d/src/rgbd/volume_settings_impl.cpp @@ -8,7 +8,27 @@ namespace cv { -Vec4i calcVolumeDimensions(Point3i volumeResolution, bool ZFirstMemOrder); +static Vec4i calcVolumeStrides(Point3i volumeResolution, bool ZFirstMemOrder) +{ + // (xRes*yRes*zRes) array + // Depending on zFirstMemOrder arg: + // &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z; + // &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes; + int xdim, ydim, zdim; + if (ZFirstMemOrder) + { + xdim = volumeResolution.z * volumeResolution.y; + ydim = volumeResolution.z; + zdim = 1; + } + else + { + xdim = 1; + ydim = volumeResolution.x; + zdim = volumeResolution.x * volumeResolution.y; + } + return Vec4i(xdim, ydim, zdim); +} class VolumeSettings::Impl { @@ -41,7 +61,7 @@ public: virtual void getVolumePose(OutputArray val) const = 0; virtual void setVolumeResolution(InputArray val) = 0; virtual void getVolumeResolution(OutputArray val) const = 0; - virtual void getVolumeDimensions(OutputArray val) const = 0; + virtual void getVolumeStrides(OutputArray val) const = 0; virtual void setCameraIntegrateIntrinsics(InputArray val) = 0; virtual void getCameraIntegrateIntrinsics(OutputArray val) const = 0; virtual void setCameraRaycastIntrinsics(InputArray val) = 0; @@ -80,7 +100,7 @@ public: virtual void getVolumePose(OutputArray val) const override; virtual void setVolumeResolution(InputArray val) override; virtual void getVolumeResolution(OutputArray val) const override; - virtual void getVolumeDimensions(OutputArray val) const override; + virtual void getVolumeStrides(OutputArray val) const override; virtual void setCameraIntegrateIntrinsics(InputArray val) override; virtual void getCameraIntegrateIntrinsics(OutputArray val) const override; virtual void setCameraRaycastIntrinsics(InputArray val) override; @@ -103,7 +123,7 @@ private: Matx44f volumePose; Point3i volumeResolution; - Vec4i volumeDimensions; + Vec4i volumeStrides; Matx33f cameraIntegrateIntrinsics; Matx33f cameraRaycastIntrinsics; @@ -265,7 +285,7 @@ void VolumeSettings::setVolumePose(InputArray val) { this->impl->setVolumePose(v void VolumeSettings::getVolumePose(OutputArray val) const { this->impl->getVolumePose(val); }; void VolumeSettings::setVolumeResolution(InputArray val) { this->impl->setVolumeResolution(val); }; void VolumeSettings::getVolumeResolution(OutputArray val) const { this->impl->getVolumeResolution(val); }; -void VolumeSettings::getVolumeDimensions(OutputArray val) const { this->impl->getVolumeDimensions(val); }; +void VolumeSettings::getVolumeStrides(OutputArray val) const { this->impl->getVolumeStrides(val); }; void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl->setCameraIntegrateIntrinsics(val); }; void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); }; void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); }; @@ -298,7 +318,7 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType) this->volumePose = ds.volumePoseMatrix; this->volumeResolution = ds.volumeResolution; - this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder); + this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; } @@ -320,7 +340,7 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType) this->volumePose = ds.volumePoseMatrix; this->volumeResolution = ds.volumeResolution; - this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder); + this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; } @@ -342,7 +362,7 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType) this->volumePose = ds.volumePoseMatrix; this->volumeResolution = ds.volumeResolution; - this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder); + this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; } @@ -470,7 +490,7 @@ void VolumeSettingsImpl::setVolumeResolution(InputArray val) if (!val.empty()) { this->volumeResolution = Point3i(val.getMat()); - this->volumeDimensions = calcVolumeDimensions(this->volumeResolution, this->zFirstMemOrder); + this->volumeStrides = calcVolumeStrides(this->volumeResolution, this->zFirstMemOrder); } } @@ -479,9 +499,9 @@ void VolumeSettingsImpl::getVolumeResolution(OutputArray val) const Mat(this->volumeResolution).copyTo(val); } -void VolumeSettingsImpl::getVolumeDimensions(OutputArray val) const +void VolumeSettingsImpl::getVolumeStrides(OutputArray val) const { - Mat(this->volumeDimensions).copyTo(val); + Mat(this->volumeStrides).copyTo(val); } void VolumeSettingsImpl::setCameraIntegrateIntrinsics(InputArray val) @@ -511,27 +531,4 @@ void VolumeSettingsImpl::getCameraRaycastIntrinsics(OutputArray val) const Mat(this->cameraRaycastIntrinsics).copyTo(val); } - -Vec4i calcVolumeDimensions(Point3i volumeResolution, bool ZFirstMemOrder) -{ - // (xRes*yRes*zRes) array - // Depending on zFirstMemOrder arg: - // &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z; - // &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes; - int xdim, ydim, zdim; - if (ZFirstMemOrder) - { - xdim = volumeResolution.z * volumeResolution.y; - ydim = volumeResolution.z; - zdim = 1; - } - else - { - xdim = 1; - ydim = volumeResolution.x; - zdim = volumeResolution.x * volumeResolution.y; - } - return Vec4i(xdim, ydim, zdim); -} - } diff --git a/modules/3d/test/ocl/test_tsdf.cpp b/modules/3d/test/ocl/test_tsdf.cpp index f372e1049a..1e32af5b77 100644 --- a/modules/3d/test/ocl/test_tsdf.cpp +++ b/modules/3d/test/ocl/test_tsdf.cpp @@ -343,12 +343,13 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = true; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -367,14 +368,14 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test if (testFunction == VolumeTestFunction::RAYCAST) { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals); } else if (testFunction == VolumeTestFunction::FETCH_NORMALS) { if (testSrcType == VolumeTestSrcType::MAT) { // takes only point from raycast for checking fetched normals on the display - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, utmpnormals); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, utmpnormals); // volume.fetchPointsNormals(upoints, utmpnormals); volume.fetchNormals(upoints, unormals); } @@ -402,12 +403,13 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = true; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -461,12 +463,13 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = true; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -485,7 +488,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType else volume.integrate(odf, poses[0].matrix); - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals); normals = unormals.getMat(af); points = upoints.getMat(af); @@ -495,7 +498,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType if (cvtest::debugLevel > 0) displayImage(depth, points, normals, depthFactor, lightPose); - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, upoints1, unormals1); + volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, upoints1, unormals1); normals = unormals1.getMat(af); points = upoints1.getMat(af); @@ -519,12 +522,13 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = true; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); diff --git a/modules/3d/test/test_tsdf.cpp b/modules/3d/test/test_tsdf.cpp index b08421c693..e6cd091b86 100644 --- a/modules/3d/test/test_tsdf.cpp +++ b/modules/3d/test/test_tsdf.cpp @@ -477,26 +477,31 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); - bool onlySemisphere = false; + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); + bool onlySemisphere = false; //TODO: check both float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); Mat rgb = scene->rgb(poses[0]); - Mat points, normals, tmpnormals, colors; + //TODO: check this utmpnormals, it looks redundant + UMat upoints, unormals, utmpnormals, ucolors; + UMat udepth, urgb; + depth.copyTo(udepth); + rgb.copyTo(urgb); - OdometryFrame odf(rgb, depth); + OdometryFrame odf(urgb, udepth); if (testSrcType == VolumeTestSrcType::MAT) { if (volumeType == VolumeType::ColorTSDF) - volume.integrate(depth, rgb, poses[0].matrix); + volume.integrate(udepth, urgb, poses[0].matrix); else - volume.integrate(depth, poses[0].matrix); + volume.integrate(udepth, poses[0].matrix); } else { @@ -506,26 +511,31 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test if (testFunction == VolumeTestFunction::RAYCAST) { if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors); else - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals); } else if (testFunction == VolumeTestFunction::FETCH_NORMALS) { if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, tmpnormals, colors); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, utmpnormals, ucolors); else - // hash_tsdf cpu don't works with raycast normals - //volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, tmpnormals); - volume.fetchPointsNormals(points, tmpnormals); + // hash_tsdf cpu doesn't work with raycast normals + //volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, utmpnormals); + volume.fetchPointsNormals(upoints, utmpnormals); - volume.fetchNormals(points, normals); + volume.fetchNormals(upoints, unormals); } else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS) { - volume.fetchPointsNormals(points, normals); + volume.fetchPointsNormals(upoints, unormals); } + Mat points, normals, colors; + points = upoints.getMat(ACCESS_READ); + normals = unormals.getMat(ACCESS_READ); + colors = ucolors.getMat(ACCESS_READ); + if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0) { if (volumeType == VolumeType::ColorTSDF) @@ -544,12 +554,13 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -611,12 +622,13 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = true; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -639,9 +651,9 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType } if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals, colors); else - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals); patchNaNs(points); anfas = counterOfValid(points); @@ -658,9 +670,9 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType normals.release(); if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals, colors); + volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals, colors); else - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals); + volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals); patchNaNs(points); profile = counterOfValid(points); @@ -688,12 +700,13 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = true; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -803,11 +816,12 @@ void boundingBoxGrowthTest(bool enableGrowth) Volume volume(VolumeType::HashTSDF, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -972,12 +986,13 @@ void regressionVolPoseRot() Volume volumeRot(VolumeType::HashTSDF, vsRot); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); bool onlySemisphere = false; float depthFactor = vs.getDepthFactor(); Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + Ptr scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere); std::vector poses = scene->getPoses(); Mat depth = scene->depth(poses[0]); @@ -1203,250 +1218,330 @@ TEST_P(StaticVolumeBoundingBox, boundingBox) INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); -#else -TEST(TSDF_CPU, raycast_custom_framesize_normals_mat) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} +#endif -TEST(TSDF_CPU, raycast_custom_framesize_normals_frame) +enum PlatformType { - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, raycast_common_framesize_normals_mat) -{ - cv::ocl::setUseOpenCL(false); - normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, raycast_common_framesize_normals_frame) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, valid_points_custom_framesize_mat) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, valid_points_custom_framesize_frame) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, valid_points_common_framesize_mat) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, valid_points_common_framesize_frame) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, raycast_custom_framesize_normals_mat) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, raycast_custom_framesize_normals_frame) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, raycast_common_framesize_normals_mat) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, raycast_common_framesize_normals_frame) -{ - cv::ocl::setUseOpenCL(false); - normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, valid_points_custom_framesize_mat) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, valid_points_custom_framesize_frame) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, valid_points_common_framesize_mat) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, valid_points_common_framesize_frame) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, reproduce_volPoseRot) -{ - cv::ocl::setUseOpenCL(false); - regressionVolPoseRot(); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_mat) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_frame) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, raycast_common_framesize_normals_mat) -{ - cv::ocl::setUseOpenCL(false); - normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, raycast_common_framesize_normals_frame) -{ - cv::ocl::setUseOpenCL(false); - normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, valid_points_custom_framesize_mat) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, valid_points_custom_framesize_fetch) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, valid_points_common_framesize_mat) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT); - cv::ocl::setUseOpenCL(true); -} - -TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME); - cv::ocl::setUseOpenCL(true); -} - + CPU = 0, GPU = 1 +}; +CV_ENUM(PlatformTypeEnum, PlatformType::CPU, PlatformType::GPU); // used to store current OpenCL status (on/off) and revert it after test is done // works even after exceptions thrown in test body struct OpenCLStatusRevert { - OpenCLStatusRevert(bool oclStatus) : originalOpenCLStatus(oclStatus) { } +#ifdef HAVE_OPENCL + OpenCLStatusRevert() + { + originalOpenCLStatus = cv::ocl::useOpenCL(); + } ~OpenCLStatusRevert() { cv::ocl::setUseOpenCL(originalOpenCLStatus); } + void off() + { + cv::ocl::setUseOpenCL(false); + } bool originalOpenCLStatus; +#else + void off() { } +#endif }; -class StaticVolumeBoundingBox : public ::testing::TestWithParam -{ }; -TEST_P(StaticVolumeBoundingBox, boundingBoxCPU) +// CV_ENUM does not support enum class types, so let's implement the class explicitly +namespace { - OpenCLStatusRevert revert(cv::ocl::useOpenCL()); + struct VolumeTypeEnum + { + static const std::array vals; + static const std::array svals; - cv::ocl::setUseOpenCL(false); + VolumeTypeEnum(VolumeType v = VolumeType::TSDF) : val(v) {} + operator VolumeType() const { return val; } + void PrintTo(std::ostream *os) const + { + int v = int(val); + if (v >= 0 && v < 3) + { + *os << svals[v]; + } + else + { + *os << "UNKNOWN"; + } + } + static ::testing::internal::ParamGenerator all() + { + return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2])); + } - staticBoundingBoxTest(GetParam()); + private: + VolumeType val; + }; + const std::array VolumeTypeEnum::vals{VolumeType::TSDF, VolumeType::HashTSDF, VolumeType::ColorTSDF}; + const std::array VolumeTypeEnum::svals{std::string("TSDF"), std::string("HashTSDF"), std::string("ColorTSDF")}; + + static inline void PrintTo(const VolumeTypeEnum &t, std::ostream *os) { t.PrintTo(os); } } -INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); +typedef std::tuple PlatformVolumeType; +struct VolumeTestFixture : public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + auto p = GetParam(); + gpu = std::get<0>(p); + volumeType = std::get<1>(p); + + if (!gpu) + oclStatus.off(); + } + + bool gpu; + VolumeType volumeType; + OpenCLStatusRevert oclStatus; +}; + + +TEST_P(VolumeTestFixture, raycast_custom_framesize_normals_mat) +{ + normal_test_custom_framesize(volumeType, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); +} + +//TODO: uncomment it when ColorTSDF gets GPU version +INSTANTIATE_TEST_CASE_P(VolumeCPU, VolumeTestFixture, /*::testing::Combine(PlatformTypeEnum::all(), VolumeTypeEnum::all())*/ + ::testing::Values(PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF}, + PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF}, + PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF})); + + +#ifdef HAVE_OPENCL +TEST(TSDF_CPU, raycast_custom_framesize_normals_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(TSDF_CPU, raycast_common_framesize_normals_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); +} + +TEST(TSDF_CPU, raycast_common_framesize_normals_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(TSDF_CPU, fetch_points_normals) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT); +} + +TEST(TSDF_CPU, fetch_normals) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT); +} + +TEST(TSDF_CPU, valid_points_custom_framesize_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT); +} + +TEST(TSDF_CPU, valid_points_custom_framesize_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(TSDF_CPU, valid_points_common_framesize_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT); +} + +TEST(TSDF_CPU, valid_points_common_framesize_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME); +} + + +TEST(HashTSDF_CPU, raycast_custom_framesize_normals_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(HashTSDF_CPU, raycast_common_framesize_normals_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); +} + +TEST(HashTSDF_CPU, raycast_common_framesize_normals_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(HashTSDF_CPU, fetch_points_normals) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT); +} + +TEST(HashTSDF_CPU, fetch_normals) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT); +} + +TEST(HashTSDF_CPU, valid_points_custom_framesize_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT); +} + +TEST(HashTSDF_CPU, valid_points_custom_framesize_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(HashTSDF_CPU, valid_points_common_framesize_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT); +} + +TEST(HashTSDF_CPU, valid_points_common_framesize_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(HashTSDF_CPU, reproduce_volPoseRot) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + regressionVolPoseRot(); +} + + + +TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(ColorTSDF_CPU, raycast_common_framesize_normals_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); +} + +TEST(ColorTSDF_CPU, raycast_common_framesize_normals_frame) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(ColorTSDF_CPU, fetch_normals) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT); +} + +TEST(ColorTSDF_CPU, fetch_points_normals) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT); +} + +TEST(ColorTSDF_CPU, valid_points_custom_framesize_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT); +} + +TEST(ColorTSDF_CPU, valid_points_custom_framesize_fetch) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME); +} + +TEST(ColorTSDF_CPU, valid_points_common_framesize_mat) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT); +} + +TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch) +{ + OpenCLStatusRevert oclStatus; + oclStatus.off(); + valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME); +} + + +class StaticVolumeBoundingBox : public ::testing::TestWithParam +{ }; + +TEST_P(StaticVolumeBoundingBox, staticBoundingBox) +{ + auto p = GetParam(); + bool gpu = bool(std::get<0>(p)); + VolumeType volumeType = std::get<1>(p); + + OpenCLStatusRevert oclStatus; + if (!gpu) + oclStatus.off(); + + staticBoundingBoxTest(volumeType); +} + +//TODO: edit this list when ColorTSDF gets GPU support +INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values( + PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF}, + PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF})); // OpenCL tests @@ -1455,44 +1550,31 @@ TEST(HashTSDF_GPU, reproduce_volPoseRot) regressionVolPoseRot(); } -//TODO: use this when ColorTSDF gets GPU support -/* -class StaticVolumeBoundingBox : public ::testing::TestWithParam -{ }; -TEST_P(StaticVolumeBoundingBox, GPU) +enum Growth { - staticBoundingBoxTest(GetParam()); -} + OFF = 0, ON = 1 +}; +CV_ENUM(GrowthEnum, Growth::OFF, Growth::ON); -INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); -*/ - -// until that, we don't need parametrized test -TEST(TSDF, boundingBoxGPU) -{ - staticBoundingBoxTest(VolumeType::TSDF); -} - - -class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam> +class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam> { }; TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) { auto p = GetParam(); - bool gpu = std::get<0>(p); - bool enableGrowth = std::get<1>(p); + bool gpu = bool(std::get<0>(p)); + bool enableGrowth = bool(std::get<1>(p)); - OpenCLStatusRevert revert(cv::ocl::useOpenCL()); + OpenCLStatusRevert oclStatus; if (!gpu) - cv::ocl::setUseOpenCL(false); + oclStatus.off(); boundingBoxGrowthTest(enableGrowth); } -INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Combine(::testing::Bool(), ::testing::Bool())); +INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all())); #endif }