From 8b7e586faa4cd57959ecefee19e1b1446e1fc381 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 24 Oct 2022 15:34:01 +0200 Subject: [PATCH] Merge pull request #22598 from savuor:icp_oframe_readonly Complement PR: #3366@contrib Changes OdometryFrame losts its getters: a user can provide data at construction stage only, pyramids and other generated data is read-only now OdometryFrame is based on UMats: no TMat templates inside, CPU operations are done with UMat::getMat() method, chaining issues are solved ad-hoc No more Odometry::createOdometryFrame() method, frames are compatible with all odometry algorithms Normals computer is cached inside Odometry and exposed to API as well as its settings Volume::raycast() won't return the result in OdometryFrame anymore Added test for Odometry::prepareFrame*() & other test fixes Minor code improvements TODOs: fix TODOs in code lower acceptable accuracy errors --- modules/3d/include/opencv2/3d/depth.hpp | 7 +- .../3d/include/opencv2/3d/detail/submap.hpp | 18 +- modules/3d/include/opencv2/3d/odometry.hpp | 20 +- .../3d/include/opencv2/3d/odometry_frame.hpp | 55 +- .../include/opencv2/3d/odometry_settings.hpp | 5 + modules/3d/include/opencv2/3d/volume.hpp | 20 - modules/3d/misc/python/test/test_odometry.py | 92 +- modules/3d/perf/perf_tsdf.cpp | 253 +--- modules/3d/samples/odometry_evaluation.cpp | 8 +- modules/3d/src/rgbd/depth_to_3d.cpp | 35 + modules/3d/src/rgbd/depth_to_3d.hpp | 28 + modules/3d/src/rgbd/odometry.cpp | 201 ++-- modules/3d/src/rgbd/odometry_frame_impl.cpp | 245 +--- modules/3d/src/rgbd/odometry_functions.cpp | 1059 +++++++---------- modules/3d/src/rgbd/odometry_functions.hpp | 76 +- .../3d/src/rgbd/odometry_settings_impl.cpp | 45 +- modules/3d/src/rgbd/utils.cpp | 74 -- modules/3d/src/rgbd/utils.hpp | 69 +- modules/3d/src/rgbd/volume_impl.cpp | 85 -- modules/3d/src/rgbd/volume_impl.hpp | 11 - modules/3d/test/ocl/test_tsdf.cpp | 84 +- modules/3d/test/test_normal.cpp | 2 +- modules/3d/test/test_odometry.cpp | 229 +++- modules/3d/test/test_tsdf.cpp | 130 +- 24 files changed, 994 insertions(+), 1857 deletions(-) delete mode 100644 modules/3d/src/rgbd/utils.cpp diff --git a/modules/3d/include/opencv2/3d/depth.hpp b/modules/3d/include/opencv2/3d/depth.hpp index 54b5853232..c05d014e72 100644 --- a/modules/3d/include/opencv2/3d/depth.hpp +++ b/modules/3d/include/opencv2/3d/depth.hpp @@ -104,13 +104,14 @@ CV_EXPORTS_W void registerDepth(InputArray unregisteredCameraMatrix, InputArray */ CV_EXPORTS_W void depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d); -/** Converts a depth image to an organized set of 3d points. +/** Converts a depth image to 3d points. If the mask is empty then the resulting array has the same dimensions as `depth`, + * otherwise it is 1d vector containing mask-enabled values only. * The coordinate system is x pointing left, y down and z away from the camera * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters * (as done with the Microsoft Kinect), otherwise, if given as CV_32F or CV_64F, it is assumed in meters) * @param K The calibration matrix - * @param points3d the resulting 3d points (point is represented by 4 chanels value [x, y, z, 0]). They are of depth the same as `depth` if it is CV_32F or CV_64F, and the - * depth of `K` if `depth` is of depth CV_U + * @param points3d the resulting 3d points (point is represented by 4 channels value [x, y, z, 0]). They are of the same depth as `depth` if it is CV_32F or CV_64F, and the + * depth of `K` if `depth` is of depth CV_16U or CV_16S * @param mask the mask of the points to consider (can be empty) */ CV_EXPORTS_W void depthTo3d(InputArray depth, InputArray K, OutputArray points3d, InputArray mask = noArray()); diff --git a/modules/3d/include/opencv2/3d/detail/submap.hpp b/modules/3d/include/opencv2/3d/detail/submap.hpp index d42b9360bf..2a0dd37868 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 Odometry& icp, const cv::Affine3f& cameraPose, cv::Size frameSize, + virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, OutputArray points = noArray(), OutputArray normals = noArray()); virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); }; @@ -112,25 +112,21 @@ void Submap::integrate(InputArray _depth, const int currFrameId) } template -void Submap::raycast(const Odometry& icp, const cv::Affine3f& _cameraPose, cv::Size frameSize, +void Submap::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, OutputArray points, OutputArray normals) { if (!points.needed() && !normals.needed()) { MatType pts, nrm; - - frame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0); - frame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0); + //TODO: get depth instead of pts from raycast volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, pts, nrm); - frame.setPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0); - frame.setPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0); + + std::vector pch(3); + split(pts, pch); renderFrame = frame; - Mat depth; - frame.getScaledDepth(depth); - frame = icp.createOdometryFrame(); - frame.setDepth(depth); + frame = OdometryFrame(noArray(), pch[2]); } else { diff --git a/modules/3d/include/opencv2/3d/odometry.hpp b/modules/3d/include/opencv2/3d/odometry.hpp index e1e38f53a9..ec8c0ec3b4 100644 --- a/modules/3d/include/opencv2/3d/odometry.hpp +++ b/modules/3d/include/opencv2/3d/odometry.hpp @@ -43,24 +43,16 @@ public: Odometry(OdometryType otype, const OdometrySettings settings, OdometryAlgoType algtype); ~Odometry(); - /** Create new odometry frame - * The Type (Mat or UMat) depends on odometry type - */ - OdometryFrame createOdometryFrame() const; - - // Deprecated - OdometryFrame createOdometryFrame(OdometryFrameStoreType matType) const; - /** Prepare frame for odometry calculation * @param frame odometry prepare this frame as src frame and dst frame simultaneously */ - void prepareFrame(OdometryFrame& frame); + void prepareFrame(OdometryFrame& frame) const; /** Prepare frame for odometry calculation * @param srcFrame frame will be prepared as src frame ("original" image) * @param dstFrame frame will be prepared as dsr frame ("rotated" image) */ - void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame); + void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const; /** Compute Rigid Transformation between two frames so that Rt * src = dst * @param srcFrame src frame ("original" image) @@ -71,11 +63,15 @@ public: * R_31 R_32 R_33 t_3 * 0 0 0 1 } */ - bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt); + bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const; - CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const; + CV_WRAP bool compute(InputArray srcDepthFrame, InputArray dstDepthFrame, OutputArray Rt) const; CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const; + //TODO: document it + //requires frame size, initialized at prepareFrame stage() + Ptr getNormalsComputer() const; + class Impl; private: Ptr impl; diff --git a/modules/3d/include/opencv2/3d/odometry_frame.hpp b/modules/3d/include/opencv2/3d/odometry_frame.hpp index f5454dea83..c71f1dda83 100644 --- a/modules/3d/include/opencv2/3d/odometry_frame.hpp +++ b/modules/3d/include/opencv2/3d/odometry_frame.hpp @@ -9,62 +9,45 @@ namespace cv { -/** Indicates what pyramid is to access using get/setPyramid... methods: -* @param PYR_IMAGE The pyramid of RGB images -* @param PYR_DEPTH The pyramid of depth images -* @param PYR_MASK The pyramid of masks -* @param PYR_CLOUD The pyramid of point clouds, produced from the pyramid of depths -* @param PYR_DIX The pyramid of dI/dx derivative images -* @param PYR_DIY The pyramid of dI/dy derivative images -* @param PYR_TEXMASK The pyramid of textured masks -* @param PYR_NORM The pyramid of normals -* @param PYR_NORMMASK The pyramid of normals masks +/** Indicates what pyramid is to access using getPyramidAt() method: */ enum OdometryFramePyramidType { - PYR_IMAGE = 0, - PYR_DEPTH = 1, - PYR_MASK = 2, - PYR_CLOUD = 3, - PYR_DIX = 4, - PYR_DIY = 5, - PYR_TEXMASK = 6, - PYR_NORM = 7, - PYR_NORMMASK = 8, + PYR_IMAGE = 0, //!< The pyramid of grayscale images + PYR_DEPTH = 1, //!< The pyramid of depth images + PYR_MASK = 2, //!< The pyramid of masks + PYR_CLOUD = 3, //!< The pyramid of point clouds, produced from the pyramid of depths + PYR_DIX = 4, //!< The pyramid of dI/dx derivative images + PYR_DIY = 5, //!< The pyramid of dI/dy derivative images + PYR_TEXMASK = 6, //!< The pyramid of "textured" masks (i.e. additional masks for normals or grayscale images) + PYR_NORM = 7, //!< The pyramid of normals + PYR_NORMMASK = 8, //!< The pyramid of normals masks N_PYRAMIDS }; -enum class OdometryFrameStoreType -{ - MAT = 0, - UMAT = 1 -}; - class CV_EXPORTS_W OdometryFrame { public: - OdometryFrame(); - OdometryFrame(OdometryFrameStoreType matType); + //TODO: add to docs: check image channels, if 3 or 4 then do cvtColor(BGR(A)2GRAY) + OdometryFrame(InputArray image = noArray(), InputArray depth = noArray(), InputArray mask = noArray(), InputArray normals = noArray()); ~OdometryFrame() {}; - void setImage(InputArray image); + void getImage(OutputArray image) const; void getGrayImage(OutputArray image) const; - void setDepth(InputArray depth); void getDepth(OutputArray depth) const; void getScaledDepth(OutputArray depth) const; - void setMask(InputArray mask); void getMask(OutputArray mask) const; - void setNormals(InputArray normals); void getNormals(OutputArray normals) const; - void setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype); - void setPyramidLevels(size_t _nLevels); - size_t getPyramidLevels(OdometryFramePyramidType oftype) const; - void setPyramidAt(InputArray img, OdometryFramePyramidType pyrType, size_t level); + + //TODO: add docs + // returns amt of levels in pyramids (all of them should have the same amt of levels) or 0 if no pyramids were prepared yet + size_t getPyramidLevels() const; + //TODO: add docs + // returns empty img if no data in the pyramid or in the pyramid's level void getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const; class Impl; -private: Ptr impl; }; } diff --git a/modules/3d/include/opencv2/3d/odometry_settings.hpp b/modules/3d/include/opencv2/3d/odometry_settings.hpp index 9b38d7b9f5..04cefc51a1 100644 --- a/modules/3d/include/opencv2/3d/odometry_settings.hpp +++ b/modules/3d/include/opencv2/3d/odometry_settings.hpp @@ -33,8 +33,13 @@ public: int getSobelSize() const; void setSobelScale(double val); double getSobelScale() const; + void setNormalWinSize(int val); int getNormalWinSize() const; + void setNormalDiffThreshold(float val); + float getNormalDiffThreshold() const; + void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm); + RgbdNormals::RgbdNormalsMethod getNormalMethod() const; void setAngleThreshold(float val); float getAngleThreshold() const; diff --git a/modules/3d/include/opencv2/3d/volume.hpp b/modules/3d/include/opencv2/3d/volume.hpp index 18f96b6856..fcfc2de8b0 100644 --- a/modules/3d/include/opencv2/3d/volume.hpp +++ b/modules/3d/include/opencv2/3d/volume.hpp @@ -61,15 +61,6 @@ public: Rendered image size and camera intrinsics are taken from volume settings structure. - * @param cameraPose the pose of camera in global coordinates. - * @param outFrame the object where to store rendered points and normals. - */ - void raycast(InputArray cameraPose, OdometryFrame& outFrame) const; - - /** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system. - - Rendered image size and camera intrinsics are taken from volume settings structure. - * @param cameraPose the pose of camera in global coordinates. * @param points image to store rendered points. * @param normals image to store rendered normals corresponding to points. @@ -81,17 +72,6 @@ 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 outFrame the object where to store rendered points and normals. - */ - void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const; - - /** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system. - - 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. diff --git a/modules/3d/misc/python/test/test_odometry.py b/modules/3d/misc/python/test/test_odometry.py index cc16725d20..48b839a43d 100644 --- a/modules/3d/misc/python/test/test_odometry.py +++ b/modules/3d/misc/python/test/test_odometry.py @@ -6,8 +6,10 @@ import cv2 as cv from tests_common import NewOpenCVTests class odometry_test(NewOpenCVTests): - def test_OdometryDefault(self): + def commonOdometryTest(self, needRgb, otype): depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) + if needRgb: + rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) radian = np.radians(1) Rt_warp = np.array( [[np.cos(radian), -np.sin(radian), 0], @@ -22,91 +24,33 @@ class odometry_test(NewOpenCVTests): ) Rt_res = np.zeros((4, 4)) - odometry = cv.Odometry() + if otype is not None: + odometry = cv.Odometry(otype) + else: + odometry = cv.Odometry() warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) - isCorrect = odometry.compute(depth, warped_depth, Rt_res) + if needRgb: + warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) + isCorrect = odometry.compute(depth, rgb, warped_depth, warped_rgb, Rt_res) + else: + isCorrect = odometry.compute(depth, warped_depth, Rt_res) res = np.absolute(Rt_curr - Rt_res).sum() eps = 0.15 self.assertLessEqual(res, eps) self.assertTrue(isCorrect) + def test_OdometryDefault(self): + self.commonOdometryTest(False, None) + def test_OdometryDepth(self): - depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) - radian = np.radians(1) - Rt_warp = np.array( - [[np.cos(radian), -np.sin(radian), 0], - [np.sin(radian), np.cos(radian), 0], - [0, 0, 1]], dtype=np.float32 - ) - Rt_curr = np.array( - [[np.cos(radian), -np.sin(radian), 0, 0], - [np.sin(radian), np.cos(radian), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]], dtype=np.float32 - ) - Rt_res = np.zeros((4, 4)) - - odometry = cv.Odometry(cv.DEPTH) - warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) - - isCorrect = odometry.compute(depth, warped_depth, Rt_res) - res = np.absolute(Rt_curr - Rt_res).sum() - eps = 0.15 - self.assertLessEqual(res, eps) - self.assertTrue(isCorrect) + self.commonOdometryTest(False, cv.DEPTH) def test_OdometryRGB(self): - rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) - radian = np.radians(1) - Rt_warp = np.array( - [[np.cos(radian), -np.sin(radian), 0], - [np.sin(radian), np.cos(radian), 0], - [0, 0, 1]], dtype=np.float32 - ) - Rt_curr = np.array( - [[np.cos(radian), -np.sin(radian), 0, 0], - [np.sin(radian), np.cos(radian), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]], dtype=np.float32 - ) - Rt_res = np.zeros((4, 4)) - - odometry = cv.Odometry(cv.RGB) - warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) - isCorrect = odometry.compute(rgb, warped_rgb, Rt_res) - - res = np.absolute(Rt_curr - Rt_res).sum() - eps = 0.15 - self.assertLessEqual(res, eps) - self.assertTrue(isCorrect) + self.commonOdometryTest(True, cv.RGB) def test_OdometryRGB_Depth(self): - depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) - rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) - radian = np.radians(1) - Rt_warp = np.array( - [[np.cos(radian), -np.sin(radian), 0], - [np.sin(radian), np.cos(radian), 0], - [0, 0, 1]], dtype=np.float32 - ) - Rt_curr = np.array( - [[np.cos(radian), -np.sin(radian), 0, 0], - [np.sin(radian), np.cos(radian), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]], dtype=np.float32 - ) - Rt_res = np.zeros((4, 4)) - - odometry = cv.Odometry(cv.RGB_DEPTH) - warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) - warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) - isCorrect = odometry.compute(depth, rgb, warped_depth, warped_rgb, Rt_res) - - res = np.absolute(Rt_curr - Rt_res).sum() - eps = 0.15 - self.assertLessEqual(res, eps) - self.assertTrue(isCorrect) + self.commonOdometryTest(True, cv.RGB_DEPTH) if __name__ == '__main__': NewOpenCVTests.bootstrap() diff --git a/modules/3d/perf/perf_tsdf.cpp b/modules/3d/perf/perf_tsdf.cpp index f387bba043..86cfda991b 100644 --- a/modules/3d/perf/perf_tsdf.cpp +++ b/modules/3d/perf/perf_tsdf.cpp @@ -471,13 +471,11 @@ PERF_TEST(Perf_TSDF, integrate_frame) { Matx44f pose = poses[i].matrix; Mat depth = scene->depth(pose); - OdometryFrame odf; - odf.setDepth(depth); + OdometryFrame odf(noArray(), depth); startTimer(); volume.integrate(odf, pose); stopTimer(); - } SANITY_CHECK_NOTHING(); } @@ -521,55 +519,6 @@ PERF_TEST(Perf_TSDF, raycast_mat) SANITY_CHECK_NOTHING(); } -// Perf_TSDF_GPU.raycast_frame -#ifdef HAVE_OPENCL -PERF_TEST(Perf_TSDF_GPU, raycast_frame) -#else -PERF_TEST(Perf_TSDF, raycast_frame) -#endif -{ - VolumeType volumeType = VolumeType::TSDF; - VolumeSettings vs(volumeType); - Volume volume(volumeType, vs); - - Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); - bool onlySemisphere = false; - float depthFactor = vs.getDepthFactor(); - Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); - std::vector poses = scene->getPoses(); - - for (size_t i = 0; i < poses.size(); i++) - { - Matx44f pose = poses[i].matrix; - Mat depth = scene->depth(pose); - -#ifdef HAVE_OPENCL - OdometryFrame odf(OdometryFrameStoreType::UMAT); -#else - OdometryFrame odf; -#endif - - odf.setDepth(depth); - volume.integrate(odf, pose); - - startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, odf); - stopTimer(); - - if (display) - { - Mat points, normals; - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - displayImage(depth, points, normals, depthFactor, lightPose); - } - } - SANITY_CHECK_NOTHING(); -} - #ifdef HAVE_OPENCL // Perf_TSDF_CPU.integrate_mat @@ -626,8 +575,7 @@ PERF_TEST(Perf_TSDF_CPU, integrate_frame) { Matx44f pose = poses[i].matrix; Mat depth = scene->depth(pose); - OdometryFrame odf; - odf.setDepth(depth); + OdometryFrame odf(noArray(), depth); startTimer(); volume.integrate(odf, pose); @@ -678,53 +626,8 @@ PERF_TEST(Perf_TSDF_CPU, raycast_mat) cv::ocl::setUseOpenCL(true); } -// Perf_TSDF_CPU.raycast_frame -PERF_TEST(Perf_TSDF_CPU, raycast_frame) -{ - cv::ocl::setUseOpenCL(false); - - VolumeType volumeType = VolumeType::TSDF; - VolumeSettings vs(volumeType); - Volume volume(volumeType, vs); - - Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); - bool onlySemisphere = false; - float depthFactor = vs.getDepthFactor(); - Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); - std::vector poses = scene->getPoses(); - - for (size_t i = 0; i < poses.size(); i++) - { - Matx44f pose = poses[i].matrix; - Mat depth = scene->depth(pose); - OdometryFrame odf; - odf.setDepth(depth); - volume.integrate(odf, pose); - - startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, odf); - stopTimer(); - - if (display) - { - Mat points, normals; - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - displayImage(depth, points, normals, depthFactor, lightPose); - } - } - SANITY_CHECK_NOTHING(); - - cv::ocl::setUseOpenCL(true); -} #endif - - - // Perf_HashTSDF_GPU.integrate_mat #ifdef HAVE_OPENCL PERF_TEST(Perf_HashTSDF_GPU, integrate_mat) @@ -780,8 +683,7 @@ PERF_TEST(Perf_HashTSDF, integrate_frame) { Matx44f pose = poses[i].matrix; Mat depth = scene->depth(pose); - OdometryFrame odf; - odf.setDepth(depth); + OdometryFrame odf(noArray(), depth); startTimer(); volume.integrate(odf, pose); @@ -830,56 +732,6 @@ PERF_TEST(Perf_HashTSDF, raycast_mat) SANITY_CHECK_NOTHING(); } -// Perf_HashTSDF_GPU.raycast_frame -#ifdef HAVE_OPENCL -PERF_TEST(Perf_HashTSDF_GPU, raycast_frame) -#else -PERF_TEST(Perf_HashTSDF, raycast_frame) -#endif -{ - VolumeType volumeType = VolumeType::HashTSDF; - VolumeSettings vs(volumeType); - Volume volume(volumeType, vs); - - Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); - bool onlySemisphere = false; - float depthFactor = vs.getDepthFactor(); - Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); - std::vector poses = scene->getPoses(); - - for (size_t i = 0; i < poses.size(); i++) - { - Matx44f pose = poses[i].matrix; - Mat depth = scene->depth(pose); - -#ifdef HAVE_OPENCL - OdometryFrame odf(OdometryFrameStoreType::UMAT); -#else - OdometryFrame odf; -#endif - - odf.setDepth(depth); - volume.integrate(odf, pose); - - startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, odf); - stopTimer(); - - if (display) - { - Mat points, normals; - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - displayImage(depth, points, normals, depthFactor, lightPose); - } - } - SANITY_CHECK_NOTHING(); -} - - #ifdef HAVE_OPENCL // Perf_HashTSDF_CPU.integrate_mat PERF_TEST(Perf_HashTSDF_CPU, integrate_mat) @@ -935,8 +787,7 @@ PERF_TEST(Perf_HashTSDF_CPU, integrate_frame) { Matx44f pose = poses[i].matrix; Mat depth = scene->depth(pose); - OdometryFrame odf; - odf.setDepth(depth); + OdometryFrame odf(noArray(), depth); startTimer(); volume.integrate(odf, pose); @@ -985,53 +836,9 @@ PERF_TEST(Perf_HashTSDF_CPU, raycast_mat) cv::ocl::setUseOpenCL(true); } - -// Perf_HashTSDF_CPU.raycast_frame -PERF_TEST(Perf_HashTSDF_CPU, raycast_frame) -{ - cv::ocl::setUseOpenCL(false); - - VolumeType volumeType = VolumeType::HashTSDF; - VolumeSettings vs(volumeType); - Volume volume(volumeType, vs); - - Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); - bool onlySemisphere = false; - float depthFactor = vs.getDepthFactor(); - Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); - std::vector poses = scene->getPoses(); - - for (size_t i = 0; i < poses.size(); i++) - { - Matx44f pose = poses[i].matrix; - Mat depth = scene->depth(pose); - OdometryFrame odf; - odf.setDepth(depth); - volume.integrate(odf, pose); - - startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, odf); - stopTimer(); - - if (display) - { - Mat points, normals; - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - displayImage(depth, points, normals, depthFactor, lightPose); - } - } - SANITY_CHECK_NOTHING(); - - cv::ocl::setUseOpenCL(true); -} #endif - // Perf_ColorTSDF_CPU.integrate_mat #ifdef HAVE_OPENCL PERF_TEST(Perf_ColorTSDF_CPU, integrate_mat) @@ -1089,9 +896,7 @@ PERF_TEST(Perf_ColorTSDF, integrate_frame) Matx44f pose = poses[i].matrix; Mat depth = scene->depth(pose); Mat rgb = scene->rgb(pose); - OdometryFrame odf; - odf.setDepth(depth); - odf.setImage(rgb); + OdometryFrame odf(rgb, depth); startTimer(); volume.integrate(odf, pose); @@ -1141,52 +946,4 @@ PERF_TEST(Perf_ColorTSDF, raycast_mat) SANITY_CHECK_NOTHING(); } -// Perf_ColorTSDF_CPU.raycast_frame -#ifdef HAVE_OPENCL -PERF_TEST(Perf_ColorTSDF_CPU, raycast_frame) -#else -PERF_TEST(Perf_ColorTSDF, raycast_frame) -#endif -{ - VolumeType volumeType = VolumeType::ColorTSDF; - VolumeSettings vs(volumeType); - Volume volume(volumeType, vs); - - Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); - Matx33f intr; - vs.getCameraIntegrateIntrinsics(intr); - bool onlySemisphere = false; - float depthFactor = vs.getDepthFactor(); - Vec3f lightPose = Vec3f::all(0.f); - Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); - std::vector poses = scene->getPoses(); - - for (size_t i = 0; i < poses.size(); i++) - { - Matx44f pose = poses[i].matrix; - Mat depth = scene->depth(pose); - Mat rgb = scene->rgb(pose); - - OdometryFrame odf; - odf.setDepth(depth); - odf.setImage(rgb); - volume.integrate(odf, pose); - - startTimer(); - volume.raycast(pose, frameSize.height, frameSize.width, odf); - stopTimer(); - - if (display) - { - Mat points, normals, colors; - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - displayColorImage(depth, rgb, points, normals, colors, depthFactor, lightPose); - } - } - SANITY_CHECK_NOTHING(); -} - - }} // namespace diff --git a/modules/3d/samples/odometry_evaluation.cpp b/modules/3d/samples/odometry_evaluation.cpp index 7b1776cde6..b6b2c30ae7 100644 --- a/modules/3d/samples/odometry_evaluation.cpp +++ b/modules/3d/samples/odometry_evaluation.cpp @@ -128,8 +128,7 @@ int main(int argc, char** argv) return -1; } - OdometryFrame frame_prev = odometry.createOdometryFrame(), - frame_curr = odometry.createOdometryFrame(); + OdometryFrame frame_prev, frame_curr; TickMeter gtm; int count = 0; @@ -182,8 +181,7 @@ int main(int argc, char** argv) { Mat gray; cvtColor(image, gray, COLOR_BGR2GRAY); - frame_curr.setImage(gray); - frame_curr.setDepth(depth); + frame_curr = OdometryFrame(gray, depth); Mat Rt; if(!Rts.empty()) @@ -216,7 +214,7 @@ int main(int argc, char** argv) //if (!frame_prev.empty()) // frame_prev.release(); frame_prev = frame_curr; - frame_curr = odometry.createOdometryFrame(); + frame_curr = OdometryFrame(); //std::swap(frame_prev, frame_curr); } } diff --git a/modules/3d/src/rgbd/depth_to_3d.cpp b/modules/3d/src/rgbd/depth_to_3d.cpp index 48c48f322e..acfa20f659 100644 --- a/modules/3d/src/rgbd/depth_to_3d.cpp +++ b/modules/3d/src/rgbd/depth_to_3d.cpp @@ -8,6 +8,41 @@ namespace cv { +/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided + * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits::quiet_NaN() + * Otherwise, the image is simply converted to floats + * @param in_in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters + * (as done with the Microsoft Kinect), it is assumed in meters) + * @param depth the desired output depth (floats or double) + * @param out_out The rescaled float depth image + */ +void rescaleDepth(InputArray in_in, int type, OutputArray out_out, double depth_factor) +{ + cv::Mat in = in_in.getMat(); + CV_Assert(in.type() == CV_64FC1 || in.type() == CV_32FC1 || in.type() == CV_16UC1 || in.type() == CV_16SC1); + CV_Assert(type == CV_64FC1 || type == CV_32FC1); + + int in_depth = in.depth(); + + out_out.create(in.size(), type); + cv::Mat out = out_out.getMat(); + if (in_depth == CV_16U) + { + in.convertTo(out, type, 1 / depth_factor); //convert to float so that it is in meters + cv::Mat valid_mask = in == std::numeric_limits::min(); // Should we do std::numeric_limits::max() too ? + out.setTo(std::numeric_limits::quiet_NaN(), valid_mask); //set a$ + } + if (in_depth == CV_16S) + { + in.convertTo(out, type, 1 / depth_factor); //convert to float so tha$ + cv::Mat valid_mask = (in == std::numeric_limits::min()) | (in == std::numeric_limits::max()); // Should we do std::numeric_limits::max() too ? + out.setTo(std::numeric_limits::quiet_NaN(), valid_mask); //set a$ + } + if ((in_depth == CV_32F) || (in_depth == CV_64F)) + in.convertTo(out, type); +} + + /** * @param K * @param depth the depth image diff --git a/modules/3d/src/rgbd/depth_to_3d.hpp b/modules/3d/src/rgbd/depth_to_3d.hpp index 14be3cc373..463646eceb 100644 --- a/modules/3d/src/rgbd/depth_to_3d.hpp +++ b/modules/3d/src/rgbd/depth_to_3d.hpp @@ -11,6 +11,34 @@ namespace cv { +/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided + * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits::quiet_NaN() + * Otherwise, the image is simply converted to floats + * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters + * (as done with the Microsoft Kinect), it is assumed in meters) + * @param the desired output depth (floats or double) + * @param out The rescaled float depth image + */ +/* void rescaleDepth(InputArray in_in, int depth, OutputArray out_out); */ + +template +void +rescaleDepthTemplated(const Mat& in, Mat& out); + +template<> +inline void +rescaleDepthTemplated(const Mat& in, Mat& out) +{ + rescaleDepth(in, CV_32F, out); +} + +template<> +inline void +rescaleDepthTemplated(const Mat& in, Mat& out) +{ + rescaleDepth(in, CV_64F, out); +} + /** * @param depth the depth image, containing depth with the value T * @param the mask, containing CV_8UC1 diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index 91a1361185..ebb30ae0bf 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -12,18 +12,16 @@ namespace cv class Odometry::Impl { -private: - public: Impl() {}; virtual ~Impl() {}; - virtual OdometryFrame createOdometryFrame() const = 0; - virtual void prepareFrame(OdometryFrame& frame) = 0; - virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) = 0; + virtual void prepareFrame(OdometryFrame& frame) const = 0; + virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const = 0; virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const = 0; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const = 0; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0; + virtual Ptr getNormalsComputer() const = 0; }; @@ -32,47 +30,36 @@ class OdometryICP : public Odometry::Impl private: OdometrySettings settings; OdometryAlgoType algtype; + mutable Ptr normalsComputer; public: - OdometryICP(OdometrySettings settings, OdometryAlgoType algtype); - ~OdometryICP(); + OdometryICP(OdometrySettings _settings, OdometryAlgoType _algtype) : + settings(_settings), algtype(_algtype), normalsComputer() + { } + ~OdometryICP() { } - virtual OdometryFrame createOdometryFrame() const override; - virtual void prepareFrame(OdometryFrame& frame) override; - virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override; + virtual void prepareFrame(OdometryFrame& frame) const override; + virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const override; virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; + virtual Ptr getNormalsComputer() const override; }; -OdometryICP::OdometryICP(OdometrySettings _settings, OdometryAlgoType _algtype) +Ptr OdometryICP::getNormalsComputer() const { - this->settings = _settings; - this->algtype = _algtype; + return this->normalsComputer; } -OdometryICP::~OdometryICP() +void OdometryICP::prepareFrame(OdometryFrame& frame) const { + prepareICPFrame(frame, frame, this->normalsComputer, this->settings, this->algtype); } -OdometryFrame OdometryICP::createOdometryFrame() const +void OdometryICP::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const { -#ifdef HAVE_OPENCL - return OdometryFrame(OdometryFrameStoreType::UMAT); -#else - return OdometryFrame(OdometryFrameStoreType::MAT); -#endif -} - -void OdometryICP::prepareFrame(OdometryFrame& frame) -{ - prepareICPFrame(frame, frame, this->settings, this->algtype); -} - -void OdometryICP::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) -{ - prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype); + prepareICPFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype); } bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const @@ -92,22 +79,26 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds return isCorrect; } -bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const +bool OdometryICP::compute(InputArray _srcDepth, InputArray _dstDepth, OutputArray Rt) const { - OdometryFrame srcFrame = this->createOdometryFrame(); - OdometryFrame dstFrame = this->createOdometryFrame(); - srcFrame.setDepth(_srcFrame); - dstFrame.setDepth(_dstFrame); + OdometryFrame srcFrame(noArray(), _srcDepth); + OdometryFrame dstFrame(noArray(), _dstDepth); - prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype); + prepareICPFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype); bool isCorrect = compute(srcFrame, dstFrame, Rt); return isCorrect; } -bool OdometryICP::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const +bool OdometryICP::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, + InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const { - CV_Error(cv::Error::StsBadFunc, "This odometry does not work with depth and rgb data simultaneously"); + CV_UNUSED(srcDepthFrame); + CV_UNUSED(srcRGBFrame); + CV_UNUSED(dstDepthFrame); + CV_UNUSED(dstRGBFrame); + CV_UNUSED(Rt); + CV_Error(cv::Error::StsBadFunc, "This odometry does not work with rgb data"); } class OdometryRGB : public Odometry::Impl @@ -117,41 +108,27 @@ private: OdometryAlgoType algtype; public: - OdometryRGB(OdometrySettings settings, OdometryAlgoType algtype); - ~OdometryRGB(); + OdometryRGB(OdometrySettings _settings, OdometryAlgoType _algtype) : settings(_settings), algtype(_algtype) { } + ~OdometryRGB() { } - virtual OdometryFrame createOdometryFrame() const override; - virtual void prepareFrame(OdometryFrame& frame) override; - virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override; + virtual void prepareFrame(OdometryFrame& frame) const override; + virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const override; virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; + virtual Ptr getNormalsComputer() const override { return Ptr(); } }; -OdometryRGB::OdometryRGB(OdometrySettings _settings, OdometryAlgoType _algtype) + +void OdometryRGB::prepareFrame(OdometryFrame& frame) const { - this->settings = _settings; - this->algtype = _algtype; + prepareRGBFrame(frame, frame, this->settings); } -OdometryRGB::~OdometryRGB() +void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const { -} - -OdometryFrame OdometryRGB::createOdometryFrame() const -{ - return OdometryFrame(OdometryFrameStoreType::MAT); -} - -void OdometryRGB::prepareFrame(OdometryFrame& frame) -{ - prepareRGBFrame(frame, frame, this->settings, false); -} - -void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) -{ - prepareRGBFrame(srcFrame, dstFrame, this->settings, false); + prepareRGBFrame(srcFrame, dstFrame, this->settings); } bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const @@ -172,22 +149,22 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds return isCorrect; } -bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const +bool OdometryRGB::compute(InputArray _srcImage, InputArray _dstImage, OutputArray Rt) const { - OdometryFrame srcFrame = this->createOdometryFrame(); - OdometryFrame dstFrame = this->createOdometryFrame(); - srcFrame.setImage(_srcFrame); - dstFrame.setImage(_dstFrame); - - prepareRGBFrame(srcFrame, dstFrame, this->settings, false); - - bool isCorrect = compute(srcFrame, dstFrame, Rt); - return isCorrect; + CV_UNUSED(_srcImage); + CV_UNUSED(_dstImage); + CV_UNUSED(Rt); + CV_Error(cv::Error::StsBadFunc, "This odometry algorithm requires depth and rgb data simultaneously"); } -bool OdometryRGB::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const +bool OdometryRGB::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const { - CV_Error(cv::Error::StsBadFunc, "This odometry does not work with depth and rgb data simultaneously"); + OdometryFrame srcFrame(srcRGBFrame, srcDepthFrame); + OdometryFrame dstFrame(dstRGBFrame, dstDepthFrame); + + prepareRGBFrame(srcFrame, dstFrame, this->settings); + + return compute(srcFrame, dstFrame, Rt); } class OdometryRGBD : public Odometry::Impl @@ -195,43 +172,34 @@ class OdometryRGBD : public Odometry::Impl private: OdometrySettings settings; OdometryAlgoType algtype; + mutable Ptr normalsComputer; public: - OdometryRGBD(OdometrySettings settings, OdometryAlgoType algtype); - ~OdometryRGBD(); + OdometryRGBD(OdometrySettings _settings, OdometryAlgoType _algtype) : settings(_settings), algtype(_algtype), normalsComputer() { } + ~OdometryRGBD() { } - virtual OdometryFrame createOdometryFrame() const override; - virtual void prepareFrame(OdometryFrame& frame) override; - virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override; + virtual void prepareFrame(OdometryFrame& frame) const override; + virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const override; virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; + virtual Ptr getNormalsComputer() const override; }; -OdometryRGBD::OdometryRGBD(OdometrySettings _settings, OdometryAlgoType _algtype) +Ptr OdometryRGBD::getNormalsComputer() const { - this->settings = _settings; - this->algtype = _algtype; + return normalsComputer; } -OdometryRGBD::~OdometryRGBD() +void OdometryRGBD::prepareFrame(OdometryFrame& frame) const { + prepareRGBDFrame(frame, frame, this->normalsComputer, this->settings, this->algtype); } -OdometryFrame OdometryRGBD::createOdometryFrame() const +void OdometryRGBD::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const { - return OdometryFrame(OdometryFrameStoreType::MAT); -} - -void OdometryRGBD::prepareFrame(OdometryFrame& frame) -{ - prepareRGBDFrame(frame, frame, this->settings, this->algtype); -} - -void OdometryRGBD::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) -{ - prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); + prepareRGBDFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype); } bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const @@ -251,26 +219,26 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d return isCorrect; } -bool OdometryRGBD::compute(InputArray, InputArray, OutputArray) const +bool OdometryRGBD::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const { - CV_Error(cv::Error::StsBadFunc, "This volume needs depth and rgb data simultaneously"); + CV_UNUSED(srcFrame); + CV_UNUSED(dstFrame); + CV_UNUSED(Rt); + CV_Error(cv::Error::StsBadFunc, "This odometry algorithm needs depth and rgb data simultaneously"); } bool OdometryRGBD::compute(InputArray _srcDepthFrame, InputArray _srcRGBFrame, InputArray _dstDepthFrame, InputArray _dstRGBFrame, OutputArray Rt) const { - OdometryFrame srcFrame = this->createOdometryFrame(); - OdometryFrame dstFrame = this->createOdometryFrame(); - srcFrame.setDepth(_srcDepthFrame); - srcFrame.setImage(_srcRGBFrame); - dstFrame.setDepth(_dstDepthFrame); - dstFrame.setImage(_dstRGBFrame); + OdometryFrame srcFrame(_srcRGBFrame, _srcDepthFrame); + OdometryFrame dstFrame(_dstRGBFrame, _dstDepthFrame); - prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); + prepareRGBDFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype); bool isCorrect = compute(srcFrame, dstFrame, Rt); return isCorrect; } + Odometry::Odometry() { OdometrySettings settings; @@ -313,54 +281,45 @@ Odometry::Odometry(OdometryType otype, OdometrySettings settings, OdometryAlgoTy break; default: CV_Error(Error::StsInternal, - "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }"); + "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }"); break; } - } Odometry::~Odometry() { } -OdometryFrame Odometry::createOdometryFrame() const -{ - return this->impl->createOdometryFrame(); -} - -OdometryFrame Odometry::createOdometryFrame(OdometryFrameStoreType matType) const -{ - return OdometryFrame(matType); -} - -void Odometry::prepareFrame(OdometryFrame& frame) +void Odometry::prepareFrame(OdometryFrame& frame) const { this->impl->prepareFrame(frame); } -void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) +void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const { this->impl->prepareFrames(srcFrame, dstFrame); } -bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) +bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const { return this->impl->compute(srcFrame, dstFrame, Rt); } - bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const { return this->impl->compute(srcFrame, dstFrame, Rt); } - bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const { return this->impl->compute(srcDepthFrame, srcRGBFrame, dstDepthFrame, dstRGBFrame, Rt); } +Ptr Odometry::getNormalsComputer() const +{ + return this->impl->getNormalsComputer(); +} void warpFrame(InputArray depth, InputArray image, InputArray mask, diff --git a/modules/3d/src/rgbd/odometry_frame_impl.cpp b/modules/3d/src/rgbd/odometry_frame_impl.cpp index cf3963c845..3bde589443 100644 --- a/modules/3d/src/rgbd/odometry_frame_impl.cpp +++ b/modules/3d/src/rgbd/odometry_frame_impl.cpp @@ -11,257 +11,90 @@ namespace cv { -class OdometryFrame::Impl +OdometryFrame::OdometryFrame(InputArray image, InputArray depth, InputArray mask, InputArray normals) { -public: - Impl() {} - virtual ~Impl() {} - virtual void setImage(InputArray image) = 0; - virtual void getImage(OutputArray image) const = 0; - virtual void getGrayImage(OutputArray image) const = 0; - virtual void setDepth(InputArray depth) = 0; - virtual void getDepth(OutputArray depth) const = 0; - virtual void getScaledDepth(OutputArray depth) const = 0; - virtual void setMask(InputArray mask) = 0; - virtual void getMask(OutputArray mask) const = 0; - virtual void setNormals(InputArray normals) = 0; - virtual void getNormals(OutputArray normals) const = 0; - virtual void setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype) = 0; - virtual void setPyramidLevels(size_t _nLevels) = 0; - virtual size_t getPyramidLevels(OdometryFramePyramidType oftype) const = 0; - virtual void setPyramidAt(InputArray img, - OdometryFramePyramidType pyrType, size_t level) = 0; - virtual void getPyramidAt(OutputArray img, - OdometryFramePyramidType pyrType, size_t level) const = 0; -}; + this->impl = makePtr(); + if (!image.empty()) + { + image.copyTo(this->impl->image); + } + if (!depth.empty()) + { + depth.copyTo(this->impl->depth); + } + if (!mask.empty()) + { + mask.copyTo(this->impl->mask); + } + if (!normals.empty()) + { + normals.copyTo(this->impl->normals); + } +} -template -class OdometryFrameImplTMat : public OdometryFrame::Impl -{ -public: - OdometryFrameImplTMat(); - ~OdometryFrameImplTMat() {}; - - virtual void setImage(InputArray image) override; - virtual void getImage(OutputArray image) const override; - virtual void getGrayImage(OutputArray image) const override; - virtual void setDepth(InputArray depth) override; - virtual void getDepth(OutputArray depth) const override; - virtual void getScaledDepth(OutputArray depth) const override; - virtual void setMask(InputArray mask) override; - virtual void getMask(OutputArray mask) const override; - virtual void setNormals(InputArray normals) override; - virtual void getNormals(OutputArray normals) const override; - virtual void setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype) override; - virtual void setPyramidLevels(size_t _nLevels) override; - virtual size_t getPyramidLevels(OdometryFramePyramidType oftype) const override; - virtual void setPyramidAt(InputArray img, - OdometryFramePyramidType pyrType, size_t level) override; - virtual void getPyramidAt(OutputArray img, - OdometryFramePyramidType pyrType, size_t level) const override; - -private: - void findMask(InputArray image); - - TMat image; - TMat imageGray; - TMat depth; - TMat scaledDepth; - TMat mask; - TMat normals; - std::vector< std::vector > pyramids; -}; - -OdometryFrame::OdometryFrame() -{ - this->impl = makePtr>(); -}; - -OdometryFrame::OdometryFrame(OdometryFrameStoreType matType) -{ - if (matType == OdometryFrameStoreType::UMAT) - this->impl = makePtr>(); - else - this->impl = makePtr>(); -}; - -void OdometryFrame::setImage(InputArray image) { this->impl->setImage(image); } void OdometryFrame::getImage(OutputArray image) const { this->impl->getImage(image); } void OdometryFrame::getGrayImage(OutputArray image) const { this->impl->getGrayImage(image); } -void OdometryFrame::setDepth(InputArray depth) { this->impl->setDepth(depth); } void OdometryFrame::getDepth(OutputArray depth) const { this->impl->getDepth(depth); } void OdometryFrame::getScaledDepth(OutputArray depth) const { this->impl->getScaledDepth(depth); } -void OdometryFrame::setMask(InputArray mask) { this->impl->setMask(mask); } void OdometryFrame::getMask(OutputArray mask) const { this->impl->getMask(mask); } -void OdometryFrame::setNormals(InputArray normals) { this->impl->setNormals(normals); } void OdometryFrame::getNormals(OutputArray normals) const { this->impl->getNormals(normals); } -void OdometryFrame::setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype) -{ - this->impl->setPyramidLevel(_nLevels, oftype); -} -void OdometryFrame::setPyramidLevels(size_t _nLevels) { this->impl->setPyramidLevels(_nLevels); } -size_t OdometryFrame::getPyramidLevels(OdometryFramePyramidType oftype) const { return this->impl->getPyramidLevels(oftype); } -void OdometryFrame::setPyramidAt(InputArray img, OdometryFramePyramidType pyrType, size_t level) -{ - this->impl->setPyramidAt(img, pyrType, level); -} + +size_t OdometryFrame::getPyramidLevels() const { return this->impl->getPyramidLevels(); } + void OdometryFrame::getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const { this->impl->getPyramidAt(img, pyrType, level); } -template -OdometryFrameImplTMat::OdometryFrameImplTMat() - : pyramids(OdometryFramePyramidType::N_PYRAMIDS) -{ -}; - -template -void OdometryFrameImplTMat::setImage(InputArray _image) -{ - this->image = getTMat(_image); - Mat gray; - if (_image.channels() != 1) - cvtColor(_image, gray, COLOR_BGR2GRAY, 1); - else - gray = getTMat(_image); - gray.convertTo(gray, CV_8UC1); - this->imageGray = getTMat(gray); -} - -template -void OdometryFrameImplTMat::getImage(OutputArray _image) const +void OdometryFrame::Impl::getImage(OutputArray _image) const { _image.assign(this->image); } -template -void OdometryFrameImplTMat::getGrayImage(OutputArray _image) const +void OdometryFrame::Impl::getGrayImage(OutputArray _image) const { _image.assign(this->imageGray); } -template -void OdometryFrameImplTMat::setDepth(InputArray _depth) -{ - TMat depth_tmp; - Mat depth_flt; - - depth_tmp = getTMat(_depth); - // Odometry works well with depth values in range [0, 10) - // If it's bigger, let's scale it down by 5000, a typical depth factor - double max; - cv::minMaxLoc(depth_tmp, nullptr, &max); - if (max > 10) - { - depth_tmp.convertTo(depth_flt, CV_32FC1, 1.f / 5000.f); - // getTMat(depth_flt) < FLT_EPSILON dont work with UMat - // depth_flt.setTo(std::numeric_limits::quiet_NaN(), getTMat(depth_flt) < FLT_EPSILON); - depth_flt.setTo(std::numeric_limits::quiet_NaN(), depth_flt < FLT_EPSILON); - depth_tmp = getTMat(depth_flt); - - } - this->depth = getTMat(_depth); - this->scaledDepth = depth_tmp; - this->findMask(_depth); -} - -template -void OdometryFrameImplTMat::getDepth(OutputArray _depth) const +void OdometryFrame::Impl::getDepth(OutputArray _depth) const { _depth.assign(this->depth); } -template -void OdometryFrameImplTMat::getScaledDepth(OutputArray _depth) const +void OdometryFrame::Impl::getScaledDepth(OutputArray _depth) const { _depth.assign(this->scaledDepth); } -template -void OdometryFrameImplTMat::setMask(InputArray _mask) -{ - this->mask = getTMat(_mask); -} - -template -void OdometryFrameImplTMat::getMask(OutputArray _mask) const +void OdometryFrame::Impl::getMask(OutputArray _mask) const { _mask.assign(this->mask); } -template -void OdometryFrameImplTMat::setNormals(InputArray _normals) -{ - this->normals = getTMat(_normals); -} - -template -void OdometryFrameImplTMat::getNormals(OutputArray _normals) const +void OdometryFrame::Impl::getNormals(OutputArray _normals) const { _normals.assign(this->normals); } -template -void OdometryFrameImplTMat::setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype) +size_t OdometryFrame::Impl::getPyramidLevels() const { - if (oftype < OdometryFramePyramidType::N_PYRAMIDS) - pyramids[oftype].resize(_nLevels, TMat()); - else - CV_Error(Error::StsBadArg, "Incorrect type."); - -} - -template -void OdometryFrameImplTMat::setPyramidLevels(size_t _nLevels) -{ - for (auto& p : pyramids) + // all pyramids should have the same size + for (const auto& p : this->pyramids) { - p.resize(_nLevels, TMat()); + if (!p.empty()) + return p.size(); } + return 0; } -template -size_t OdometryFrameImplTMat::getPyramidLevels(OdometryFramePyramidType oftype) const -{ - if (oftype < OdometryFramePyramidType::N_PYRAMIDS) - return pyramids[oftype].size(); - else - return 0; -} -template -void OdometryFrameImplTMat::setPyramidAt(InputArray _img, OdometryFramePyramidType pyrType, size_t level) -{ - CV_Assert(pyrType >= 0); - CV_Assert((size_t)pyrType < pyramids.size()); - CV_Assert(level < pyramids[pyrType].size()); - TMat img = getTMat(_img); - pyramids[pyrType][level] = img; -} - -template -void OdometryFrameImplTMat::getPyramidAt(OutputArray _img, OdometryFramePyramidType pyrType, size_t level) const +void OdometryFrame::Impl::getPyramidAt(OutputArray _img, OdometryFramePyramidType pyrType, size_t level) const { CV_Assert(pyrType < OdometryFramePyramidType::N_PYRAMIDS); - CV_Assert(level < pyramids[pyrType].size()); - TMat img = pyramids[pyrType][level]; - _img.assign(img); -} - -template -void OdometryFrameImplTMat::findMask(InputArray _depth) -{ - Mat depth_value = _depth.getMat(); - CV_Assert(depth_value.type() == DEPTH_TYPE); - Mat m(depth_value.size(), CV_8UC1, Scalar(255)); - for (int y = 0; y < depth_value.rows; y++) - for (int x = 0; x < depth_value.cols; x++) - { - if (cvIsNaN(depth_value.at(y, x)) || depth_value.at(y, x) <= FLT_EPSILON) - m.at(y, x) = 0; - } - this->setMask(m); + if (level < pyramids[pyrType].size()) + _img.assign(pyramids[pyrType][level]); + else + _img.clear(); } } diff --git a/modules/3d/src/rgbd/odometry_functions.cpp b/modules/3d/src/rgbd/odometry_functions.cpp index 42ccbf0689..c395dee2e0 100644 --- a/modules/3d/src/rgbd/odometry_functions.cpp +++ b/modules/3d/src/rgbd/odometry_functions.cpp @@ -14,537 +14,7 @@ namespace cv { -static const int normalWinSize = 5; -static const RgbdNormals::RgbdNormalsMethod normalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS; - -enum -{ - UTSIZE = 27 -}; - -void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, OdometryAlgoType algtype) -{ - prepareRGBFrame(srcFrame, dstFrame, settings, true); - prepareICPFrame(srcFrame, dstFrame, settings, algtype); -} - -void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, bool useDepth) -{ - prepareRGBFrameBase(srcFrame, settings, useDepth); - prepareRGBFrameBase(dstFrame, settings, useDepth); - - prepareRGBFrameSrc(srcFrame, settings); - prepareRGBFrameDst(dstFrame, settings); -} - -void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, OdometryAlgoType algtype) -{ - prepareICPFrameBase(srcFrame, settings); - prepareICPFrameBase(dstFrame, settings); - - prepareICPFrameSrc(srcFrame, settings); - if (algtype == OdometryAlgoType::FAST) - prepareICPFrameDst(srcFrame, settings); - prepareICPFrameDst(dstFrame, settings); -} - -void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool useDepth) -{ - // Can be transformed into template argument in the future - // when this algorithm supports OCL UMats too - - typedef Mat TMat; - - TMat image; - frame.getGrayImage(image); - if (image.empty()) - { - if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_IMAGE) > 0) - { - TMat pyr0; - frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_IMAGE, 0); - frame.setImage(pyr0); - frame.getGrayImage(image); - } - else - CV_Error(Error::StsBadSize, "Image or pyramidImage have to be set."); - } - checkImage(image); - - TMat depth; - if (useDepth) - { - frame.getScaledDepth(depth); - if (depth.empty()) - { - if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0) - { - TMat pyr0; - frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0); - frame.setDepth(pyr0); - } - else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0) - { - TMat cloud; - frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0); - std::vector xyz; - split(cloud, xyz); - frame.setDepth(xyz[2]); - frame.getScaledDepth(depth); - } - else - CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set."); - } - checkDepth(depth, image.size()); - } - else - depth = TMat(image.size(), CV_32F, 1); - - TMat mask; - frame.getMask(mask); - if (mask.empty() && frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK) > 0) - { - TMat pyr0; - frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_MASK, 0); - frame.setMask(pyr0); - frame.getMask(mask); - } - checkMask(mask, image.size()); - - std::vector iterCounts; - Mat miterCounts; - settings.getIterCounts(miterCounts); - for (int i = 0; i < miterCounts.size().height; i++) - iterCounts.push_back(miterCounts.at(i)); - - std::vector ipyramids; - preparePyramidImage(image, ipyramids, iterCounts.size()); - setPyramids(frame, OdometryFramePyramidType::PYR_IMAGE, ipyramids); - - std::vector dpyramids; - preparePyramidImage(depth, dpyramids, iterCounts.size()); - setPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids); - - std::vector mpyramids; - std::vector npyramids; - preparePyramidMask(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), npyramids, mpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); -} - -void prepareRGBFrameSrc(OdometryFrame& frame, OdometrySettings settings) -{ - typedef Mat TMat; - - std::vector dpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH)); - getPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids); - std::vector mpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK)); - getPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); - - std::vector cpyramids; - Matx33f cameraMatrix; - settings.getCameraMatrix(cameraMatrix); - - preparePyramidCloud(dpyramids, cameraMatrix, cpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_CLOUD, cpyramids); -} - -void prepareRGBFrameDst(OdometryFrame& frame, OdometrySettings settings) -{ - typedef Mat TMat; - - std::vector ipyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_IMAGE)); - getPyramids(frame, OdometryFramePyramidType::PYR_IMAGE, ipyramids); - std::vector mpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK)); - getPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); - - std::vector dxpyramids, dypyramids, tmpyramids; - - Mat _minGradientMagnitudes; - std::vector minGradientMagnitudes; - settings.getMinGradientMagnitudes(_minGradientMagnitudes); - for (int i = 0; i < _minGradientMagnitudes.size().height; i++) - minGradientMagnitudes.push_back(_minGradientMagnitudes.at(i)); - - preparePyramidSobel(ipyramids, 1, 0, dxpyramids, settings.getSobelSize()); - preparePyramidSobel(ipyramids, 0, 1, dypyramids, settings.getSobelSize()); - preparePyramidTexturedMask(dxpyramids, dypyramids, minGradientMagnitudes, - mpyramids, settings.getMaxPointsPart(), tmpyramids, settings.getSobelScale()); - - setPyramids(frame, OdometryFramePyramidType::PYR_DIX, dxpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_DIY, dypyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_TEXMASK, tmpyramids); -} - -void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings) -{ - typedef Mat TMat; - - TMat depth; - frame.getScaledDepth(depth); - if (depth.empty()) - { - if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0) - { - TMat pyr0; - frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0); - frame.setDepth(pyr0); - frame.getScaledDepth(depth); - } - else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0) - { - TMat cloud; - frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0); - std::vector xyz; - split(cloud, xyz); - frame.setDepth(xyz[2]); - frame.getScaledDepth(depth); - } - else - CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set."); - } - - checkDepth(depth, depth.size()); - - TMat mask; - frame.getMask(mask); - if (mask.empty() && frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK) > 0) - { - TMat pyr0; - frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_MASK, 0); - frame.setMask(pyr0); - frame.getMask(mask); - } - checkMask(mask, depth.size()); - - std::vector iterCounts; - Mat miterCounts; - settings.getIterCounts(miterCounts); - for (int i = 0; i < miterCounts.size().height; i++) - iterCounts.push_back(miterCounts.at(i)); - - std::vector dpyramids; - preparePyramidImage(depth, dpyramids, iterCounts.size()); - setPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids); - - std::vector mpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK)); - getPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); - - std::vector cpyramids; - Matx33f cameraMatrix; - settings.getCameraMatrix(cameraMatrix); - - preparePyramidCloud(dpyramids, cameraMatrix, cpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_CLOUD, cpyramids); -} - -void prepareICPFrameSrc(OdometryFrame& frame, OdometrySettings settings) -{ - typedef Mat TMat; - - TMat mask; - frame.getMask(mask); - - std::vector dpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH)); - getPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids); - - std::vector mpyramids; - std::vector npyramids; - preparePyramidMask(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), - npyramids, mpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); -} - -void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings) -{ - typedef Mat TMat; - - Ptr normalsComputer; - Matx33f cameraMatrix; - settings.getCameraMatrix(cameraMatrix); - - TMat depth, mask, normals; - frame.getScaledDepth(depth); - frame.getMask(mask); - frame.getNormals(normals); - - if (normals.empty()) - { - if ( frame.getPyramidLevels(OdometryFramePyramidType::PYR_NORM)) - { - TMat n0; - frame.getPyramidAt(n0, OdometryFramePyramidType::PYR_NORM, 0); - frame.setNormals(n0); - frame.getNormals(normals); - } - else - { - Matx33f K; - if (!normalsComputer.empty()) - normalsComputer->getK(K); - if (normalsComputer.empty() || - normalsComputer->getRows() != depth.rows || - normalsComputer->getCols() != depth.cols || - norm(K, cameraMatrix) > FLT_EPSILON) - normalsComputer = RgbdNormals::create(depth.rows, - depth.cols, - depth.depth(), - cameraMatrix, - normalWinSize, - 50.f, - normalMethod); - TMat c0; - frame.getPyramidAt(c0, OdometryFramePyramidType::PYR_CLOUD, 0); - normalsComputer->apply(c0, normals); - frame.setNormals(normals); - frame.getNormals(normals); - } - } - - std::vector npyramids; - std::vector dpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH)); - getPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids); - preparePyramidNormals(normals, dpyramids, npyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_NORM, npyramids); - - std::vector mpyramids; - preparePyramidMask(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), - npyramids, mpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); - - std::vector nmpyramids; - preparePyramidNormalsMask(npyramids, mpyramids, settings.getMaxPointsPart(), nmpyramids); - setPyramids(frame, OdometryFramePyramidType::PYR_NORMMASK, nmpyramids); -} - -void setPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, InputArrayOfArrays pyramidImage) -{ - size_t nLevels = pyramidImage.size(-1).width; - std::vector pyramids; - pyramidImage.getMatVector(pyramids); - odf.setPyramidLevel(nLevels, oftype); - for (size_t l = 0; l < nLevels; l++) - { - odf.setPyramidAt(pyramids[l], oftype, l); - } -} - -void getPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, OutputArrayOfArrays _pyramid) -{ - typedef Mat TMat; - - size_t nLevels = odf.getPyramidLevels(oftype); - for (size_t l = 0; l < nLevels; l++) - { - TMat img; - odf.getPyramidAt(img, oftype, l); - TMat& p = _pyramid.getMatRef(int(l)); - img.copyTo(p); - } -} - -void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage, size_t levelCount) -{ - if (!pyramidImage.empty()) - { - size_t nLevels = pyramidImage.size(-1).width; - if (nLevels < levelCount) - CV_Error(Error::StsBadSize, "Levels count of pyramidImage has to be equal or less than size of iterCounts."); - - CV_Assert(pyramidImage.size(0) == image.size()); - for (size_t i = 0; i < nLevels; i++) - CV_Assert(pyramidImage.type((int)i) == image.type()); - } - else - buildPyramid(image, pyramidImage, (int)levelCount - 1); -} - -template -void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth, - InputArrayOfArrays pyramidNormal, - InputOutputArrayOfArrays pyramidMask) -{ - minDepth = std::max(0.f, minDepth); - - int nLevels = pyramidDepth.size(-1).width; - if (!pyramidMask.empty()) - { - if (pyramidMask.size(-1).width != nLevels) - CV_Error(Error::StsBadSize, "Levels count of pyramidMask has to be equal to size of pyramidDepth."); - - for (int i = 0; i < pyramidMask.size(-1).width; i++) - { - CV_Assert(pyramidMask.size(i) == pyramidDepth.size(i)); - CV_Assert(pyramidMask.type(i) == CV_8UC1); - } - } - else - { - TMat validMask; - if (mask.empty()) - validMask = TMat(pyramidDepth.size(0), CV_8UC1, Scalar(255)); - else - validMask = getTMat(mask, -1).clone(); - - buildPyramid(validMask, pyramidMask, nLevels - 1); - - for (int i = 0; i < pyramidMask.size(-1).width; i++) - { - TMat levelDepth = getTMat(pyramidDepth, i).clone(); - patchNaNs(levelDepth, 0); - - TMat& levelMask = getTMatRef(pyramidMask, i); - TMat gtmin, ltmax, tmpMask; - cv::compare(levelDepth, Scalar(minDepth), gtmin, CMP_GT); - cv::compare(levelDepth, Scalar(maxDepth), ltmax, CMP_LT); - cv::bitwise_and(gtmin, ltmax, tmpMask); - cv::bitwise_and(levelMask, tmpMask, levelMask); - - if (!pyramidNormal.empty()) - { - CV_Assert(pyramidNormal.type(i) == CV_32FC4); - CV_Assert(pyramidNormal.size(i) == pyramidDepth.size(i)); - TMat levelNormal = getTMat(pyramidNormal, i).clone(); - - TMat validNormalMask; - // NaN check - cv::compare(levelNormal, levelNormal, validNormalMask, CMP_EQ); - CV_Assert(validNormalMask.type() == CV_8UC4); - - std::vector channelMasks; - split(validNormalMask, channelMasks); - TMat tmpChMask; - cv::bitwise_and(channelMasks[0], channelMasks[1], tmpChMask); - cv::bitwise_and(channelMasks[2], tmpChMask, validNormalMask); - cv::bitwise_and(levelMask, validNormalMask, levelMask); - } - } - } -} - -template -void preparePyramidCloud(InputArrayOfArrays pyramidDepth, const Matx33f& cameraMatrix, InputOutputArrayOfArrays pyramidCloud) -{ - size_t depthSize = pyramidDepth.size(-1).width; - size_t cloudSize = pyramidCloud.size(-1).width; - if (!pyramidCloud.empty()) - { - if (cloudSize != depthSize) - CV_Error(Error::StsBadSize, "Incorrect size of pyramidCloud."); - - for (size_t i = 0; i < depthSize; i++) - { - CV_Assert(pyramidCloud.size((int)i) == pyramidDepth.size((int)i)); - CV_Assert(pyramidCloud.type((int)i) == CV_32FC4); - } - } - else - { - std::vector pyramidCameraMatrix; - buildPyramidCameraMatrix(cameraMatrix, (int)depthSize, pyramidCameraMatrix); - - pyramidCloud.create((int)depthSize, 1, CV_32FC4, -1); - for (size_t i = 0; i < depthSize; i++) - { - TMat cloud; - depthTo3d(getTMat(pyramidDepth, (int)i), pyramidCameraMatrix[i], cloud, Mat()); - getTMatRef(pyramidCloud, (int)i) = cloud; - - } - } -} - -void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector& pyramidCameraMatrix) -{ - pyramidCameraMatrix.resize(levels); - - for (int i = 0; i < levels; i++) - { - Matx33f levelCameraMatrix = (i == 0) ? cameraMatrix : 0.5f * pyramidCameraMatrix[i - 1]; - levelCameraMatrix(2, 2) = 1.0; - pyramidCameraMatrix[i] = levelCameraMatrix; - } -} - - -template -void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel, int sobelSize) -{ - size_t imgLevels = pyramidImage.size(-1).width; - size_t sobelLvls = pyramidSobel.size(-1).width; - if (!pyramidSobel.empty()) - { - if (sobelLvls != imgLevels) - CV_Error(Error::StsBadSize, "Incorrect size of pyramidSobel."); - - for (size_t i = 0; i < sobelLvls; i++) - { - CV_Assert(pyramidSobel.size((int)i) == pyramidImage.size((int)i)); - CV_Assert(pyramidSobel.type((int)i) == CV_16SC1); - } - } - else - { - pyramidSobel.create((int)imgLevels, 1, CV_16SC1, -1); - for (size_t i = 0; i < imgLevels; i++) - { - Sobel(getTMat(pyramidImage, (int)i), getTMatRef(pyramidSobel, (int)i), CV_16S, dx, dy, sobelSize); - } - } -} - -void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy, - InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale) -{ - size_t didxLevels = pyramid_dI_dx.size(-1).width; - size_t texLevels = pyramidTexturedMask.size(-1).width; - if (!pyramidTexturedMask.empty()) - { - if (texLevels != didxLevels) - CV_Error(Error::StsBadSize, "Incorrect size of pyramidTexturedMask."); - - for (size_t i = 0; i < texLevels; i++) - { - CV_Assert(pyramidTexturedMask.size((int)i) == pyramid_dI_dx.size((int)i)); - CV_Assert(pyramidTexturedMask.type((int)i) == CV_8UC1); - } - } - else - { - CV_Assert(minGradMagnitudes.type() == CV_32F); - Mat_ mgMags = minGradMagnitudes.getMat(); - - const float sobelScale2_inv = (float) (1. / (sobelScale * sobelScale)); - pyramidTexturedMask.create((int)didxLevels, 1, CV_8UC1, -1); - for (size_t i = 0; i < didxLevels; i++) - { - const float minScaledGradMagnitude2 = mgMags((int)i) * mgMags((int)i) * sobelScale2_inv; - const Mat& dIdx = pyramid_dI_dx.getMat((int)i); - const Mat& dIdy = pyramid_dI_dy.getMat((int)i); - - Mat texturedMask(dIdx.size(), CV_8UC1, Scalar(0)); - - for (int y = 0; y < dIdx.rows; y++) - { - const short* dIdx_row = dIdx.ptr(y); - const short* dIdy_row = dIdy.ptr(y); - uchar* texturedMask_row = texturedMask.ptr(y); - for (int x = 0; x < dIdx.cols; x++) - { - float magnitude2 = static_cast(dIdx_row[x] * dIdx_row[x] + dIdy_row[x] * dIdy_row[x]); - if (magnitude2 >= minScaledGradMagnitude2) - texturedMask_row[x] = 255; - } - } - Mat texMask = texturedMask & pyramidMask.getMat((int)i); - - randomSubsetOfMask(texMask, (float)maxPointsPart); - pyramidTexturedMask.getMatRef((int)i) = texMask; - } - } -} - -void randomSubsetOfMask(InputOutputArray _mask, float part) +static void randomSubsetOfMask(InputOutputArray _mask, float part) { const int minPointsCount = 1000; // minimum point count (we can process them fast) const int nonzeros = countNonZero(_mask); @@ -571,82 +41,450 @@ void randomSubsetOfMask(InputOutputArray _mask, float part) } } -void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals) + +static UMat prepareScaledDepth(OdometryFrame& frame) +{ + UMat depth; + frame.getDepth(depth); + CV_Assert(!depth.empty()); + + // Odometry works well with depth values in range [0, 10) + // If it's bigger, let's scale it down by 5000, a typical depth factor + double maxv; + cv::minMaxLoc(depth, nullptr, &maxv); + UMat depthFlt; + depth.convertTo(depthFlt, CV_32FC1, maxv > 10 ? (1.f / 5000.f) : 1.f); + patchNaNs(depthFlt, 0); + frame.impl->scaledDepth = depthFlt; + + return depthFlt; +} + + +//TODO: this with bilateral, maybe masks +static void preparePyramidDepth(UMat depth, std::vector& dpyramids, int maxLevel) +{ + //TODO: make resize down instead + buildPyramid(depth, dpyramids, maxLevel); + //resize(depth, resized, dsize, fx, fy, INTER_NEAREST); +} + + +static void preparePyramidMask(UMat mask, const std::vector pyramidDepth, int nLevels, float minDepth, float maxDepth, + std::vector& pyramidMask) +{ + minDepth = std::max(0.f, minDepth); + + buildPyramid(mask, pyramidMask, nLevels - 1); + + for (int i = 0; i < nLevels; i++) + { + UMat maski = pyramidMask[i]; + threshold(maski, maski, 254, 255, THRESH_TOZERO); + const UMat depthi = pyramidDepth[i]; + + UMat gtmin, ltmax, tmpMask; + cv::compare(depthi, Scalar(minDepth), gtmin, CMP_GT); + cv::compare(depthi, Scalar(maxDepth), ltmax, CMP_LT); + cv::bitwise_and(gtmin, ltmax, tmpMask); + cv::bitwise_and(maski, tmpMask, maski); + } +} + +static void extendPyrMaskByPyrNormals(const std::vector& pyramidNormals, std::vector& pyramidMask) { - size_t depthLevels = pyramidDepth.size(-1).width; - size_t normalsLevels = pyramidNormals.size(-1).width; if (!pyramidNormals.empty()) { - if (normalsLevels != depthLevels) - CV_Error(Error::StsBadSize, "Incorrect size of pyramidNormals."); + int nLevels = pyramidNormals.size(); - for (size_t i = 0; i < normalsLevels; i++) + for (int i = 0; i < nLevels; i++) { - CV_Assert(pyramidNormals.size((int)i) == pyramidDepth.size((int)i)); - CV_Assert(pyramidNormals.type((int)i) == CV_32FC3); + UMat maski = pyramidMask[i]; + UMat normali = pyramidNormals[i]; + UMat validNormalMask; + // NaN check + cv::compare(normali, normali, validNormalMask, CMP_EQ); + CV_Assert(validNormalMask.type() == CV_8UC4); + + std::vector channelMasks; + split(validNormalMask, channelMasks); + UMat tmpChMask; + cv::bitwise_and(channelMasks[0], channelMasks[1], tmpChMask); + cv::bitwise_and(channelMasks[2], tmpChMask, tmpChMask); + cv::bitwise_and(maski, tmpChMask, maski); } } - else +} + +static void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector& pyramidCameraMatrix) +{ + pyramidCameraMatrix.resize(levels); + + for (int i = 0; i < levels; i++) { - buildPyramid(normals, pyramidNormals, (int)depthLevels - 1); - // renormalize normals - for (size_t i = 1; i < depthLevels; i++) + Matx33f levelCameraMatrix = (i == 0) ? cameraMatrix : 0.5f * pyramidCameraMatrix[i - 1]; + levelCameraMatrix(2, 2) = 1.0; + pyramidCameraMatrix[i] = levelCameraMatrix; + } +} + +static void preparePyramidCloud(const std::vector& pyramidDepth, const Matx33f& cameraMatrix, std::vector& pyramidCloud) +{ + int nLevels = pyramidDepth.size(); + + std::vector pyramidCameraMatrix; + buildPyramidCameraMatrix(cameraMatrix, nLevels, pyramidCameraMatrix); + + pyramidCloud.resize(nLevels, UMat()); + + for (int i = 0; i < nLevels; i++) + { + UMat cloud; + depthTo3d(pyramidDepth[i], pyramidCameraMatrix[i], cloud, noArray()); + pyramidCloud[i] = cloud; + } +} + + +static void preparePyramidTexturedMask(const std::vector& pyramid_dI_dx, const std::vector& pyramid_dI_dy, + std::vector minGradMagnitudes, const std::vector& pyramidMask, double maxPointsPart, + std::vector& pyramidTexturedMask, double sobelScale) +{ + int nLevels = pyramid_dI_dx.size(); + + const float sobelScale2_inv = (float)(1. / (sobelScale * sobelScale)); + pyramidTexturedMask.resize(nLevels, UMat()); + for (int i = 0; i < nLevels; i++) + { + const float minScaledGradMagnitude2 = minGradMagnitudes[i] * minGradMagnitudes[i] * sobelScale2_inv; + const Mat dIdx = pyramid_dI_dx[i].getMat(ACCESS_READ); + const Mat dIdy = pyramid_dI_dy[i].getMat(ACCESS_READ); + + Mat texturedMask(dIdx.size(), CV_8UC1, Scalar(0)); + + for (int y = 0; y < dIdx.rows; y++) { - Mat& currNormals = pyramidNormals.getMatRef((int)i); - for (int y = 0; y < currNormals.rows; y++) + const short *dIdx_row = dIdx.ptr(y); + const short *dIdy_row = dIdy.ptr(y); + uchar *texturedMask_row = texturedMask.ptr(y); + for (int x = 0; x < dIdx.cols; x++) { - Point3f* normals_row = currNormals.ptr(y); - for (int x = 0; x < currNormals.cols; x++) - { - double nrm = norm(normals_row[x]); - normals_row[x] *= 1. / nrm; - } + float magnitude2 = static_cast(dIdx_row[x] * dIdx_row[x] + dIdy_row[x] * dIdy_row[x]); + if (magnitude2 >= minScaledGradMagnitude2) + texturedMask_row[x] = 255; + } + } + Mat texMask = texturedMask & pyramidMask[i].getMat(ACCESS_READ); + randomSubsetOfMask(texMask, (float)maxPointsPart); + texMask.copyTo(pyramidTexturedMask[i]); + } +} + + +static void preparePyramidNormals(const UMat &normals, const std::vector &pyramidDepth, std::vector &pyramidNormals) +{ + int nLevels = pyramidDepth.size(); + + buildPyramid(normals, pyramidNormals, nLevels - 1); + // renormalize normals + for (int i = 1; i < nLevels; i++) + { + Mat currNormals = pyramidNormals[i].getMat(ACCESS_RW); + CV_Assert(currNormals.type() == CV_32FC4); + for (int y = 0; y < currNormals.rows; y++) + { + Vec4f *normals_row = currNormals.ptr(y); + for (int x = 0; x < currNormals.cols; x++) + { + Vec4f n4 = normals_row[x]; + Point3f n(n4[0], n4[1], n4[2]); + double nrm = norm(n); + n *= 1.f / nrm; + normals_row[x] = Vec4f(n.x, n.y, n.z, 0); } } } } -void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask) -{ - size_t maskLevels = pyramidMask.size(-1).width; - size_t norMaskLevels = pyramidNormalsMask.size(-1).width; - if (!pyramidNormalsMask.empty()) - { - if (norMaskLevels != maskLevels) - CV_Error(Error::StsBadSize, "Incorrect size of pyramidNormalsMask."); - for (size_t i = 0; i < norMaskLevels; i++) +static void preparePyramidNormalsMask(const std::vector &pyramidNormals, const std::vector &pyramidMask, double maxPointsPart, + std::vector &pyramidNormalsMask) +{ + int nLevels = pyramidNormals.size(); + pyramidNormalsMask.resize(nLevels, UMat()); + for (int i = 0; i < nLevels; i++) + { + Mat pyrMask = pyramidMask[i].getMat(ACCESS_READ); + + const Mat normals = pyramidNormals[i].getMat(ACCESS_READ); + Mat_ normalsMask(pyrMask.size(), (uchar)255); + for (int y = 0; y < normalsMask.rows; y++) { - CV_Assert(pyramidNormalsMask.size((int)i) == pyramidMask.size((int)i)); - CV_Assert(pyramidNormalsMask.type((int)i) == pyramidMask.type((int)i)); + const Vec4f *normals_row = normals.ptr(y); + uchar *normalsMask_row = normalsMask.ptr(y); + for (int x = 0; x < normalsMask.cols; x++) + { + Vec4f n = normals_row[x]; + if (!(std::isfinite(n[0]) && std::isfinite(n[1]) && std::isfinite(n[2]))) + { + normalsMask_row[x] = 0; + } + } } + cv::bitwise_and(pyrMask, normalsMask, normalsMask); + + randomSubsetOfMask(normalsMask, (float)maxPointsPart); + normalsMask.copyTo(pyramidNormalsMask[i]); + } +} + + +static void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings) +{ + UMat grayImage; + frame.getGrayImage(grayImage); + if (grayImage.empty()) + { + UMat image; + frame.getImage(image); + CV_Assert(!image.empty() && image.depth() == CV_8U); + + int ch = image.channels(); + if (ch == 3 || ch == 4) + cvtColor(image, grayImage, ch == 3 ? COLOR_BGR2GRAY : COLOR_BGRA2GRAY, 1); + else if (ch == 1) + grayImage = image; + else + CV_Error(Error::StsBadArg, "Image should have 3 or 4 channels (RGB) or 1 channel (grayscale)"); + grayImage.convertTo(grayImage, CV_8U); + frame.impl->imageGray = grayImage; + } + + //TODO: don't use scaled when scale bug is fixed + UMat scaledDepth; + frame.getScaledDepth(scaledDepth); + if (scaledDepth.empty()) + { + scaledDepth = prepareScaledDepth(frame); + CV_Assert(scaledDepth.size() == grayImage.size()); + } + + UMat depthMask; + // ignore small, negative, Inf, NaN values + cv::compare(scaledDepth, Scalar(FLT_EPSILON), depthMask, CMP_GT); + + UMat mask; + frame.getMask(mask); + if (mask.empty()) + { + frame.impl->mask = depthMask; } else { - pyramidNormalsMask.create((int)maskLevels, 1, CV_8U, -1); - for (size_t i = 0; i < maskLevels; i++) - { - Mat& normalsMask = pyramidNormalsMask.getMatRef((int)i); - normalsMask = pyramidMask.getMat((int)i).clone(); - - const Mat normals = pyramidNormals.getMat((int)i); - for (int y = 0; y < normalsMask.rows; y++) - { - const Vec4f* normals_row = normals.ptr(y); - uchar* normalsMask_row = normalsMask.ptr(y); - for (int x = 0; x < normalsMask.cols; x++) - { - Vec4f n = normals_row[x]; - if (cvIsNaN(n[0])) - { - normalsMask_row[x] = 0; - } - } - } - randomSubsetOfMask(normalsMask, (float)maxPointsPart); - } + CV_Assert(mask.type() == CV_8UC1); + CV_Assert(mask.size() == grayImage.size()); + cv::bitwise_and(mask, depthMask, frame.impl->mask); } + frame.getMask(mask); + + std::vector iterCounts; + settings.getIterCounts(iterCounts); + + int maxLevel = (int)iterCounts.size() - 1; + std::vector& ipyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_IMAGE]; + if (ipyramids.empty()) + buildPyramid(grayImage, ipyramids, maxLevel); + + std::vector& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH]; + if (dpyramids.empty()) + preparePyramidDepth(scaledDepth, dpyramids, maxLevel); + + std::vector& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK]; + if (mpyramids.empty()) + preparePyramidMask(mask, dpyramids, maxLevel + 1, settings.getMinDepth(), settings.getMaxDepth(), mpyramids); +} + + +static void prepareRGBFrameSrc(OdometryFrame& frame, OdometrySettings settings) +{ + Matx33f cameraMatrix; + settings.getCameraMatrix(cameraMatrix); + + std::vector& cpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_CLOUD]; + const std::vector& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH]; + + preparePyramidCloud(dpyramids, cameraMatrix, cpyramids); +} + + +static void prepareRGBFrameDst(OdometryFrame& frame, OdometrySettings settings) +{ + const std::vector& ipyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_IMAGE]; + const std::vector& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK]; + + std::vector& dxpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DIX]; + std::vector& dypyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DIY]; + + std::vector minGradientMagnitudes; + settings.getMinGradientMagnitudes(minGradientMagnitudes); + + int nLevels = ipyramids.size(); + dxpyramids.resize(nLevels, UMat()); + dypyramids.resize(nLevels, UMat()); + int sobelSize = settings.getSobelSize(); + for (int i = 0; i < nLevels; i++) + { + Sobel(ipyramids[i], dxpyramids[i], CV_16S, 1, 0, sobelSize); + Sobel(ipyramids[i], dypyramids[i], CV_16S, 0, 1, sobelSize); + } + + std::vector& tmpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_TEXMASK]; + preparePyramidTexturedMask(dxpyramids, dypyramids, minGradientMagnitudes, + mpyramids, settings.getMaxPointsPart(), tmpyramids, settings.getSobelScale()); +} + + +static void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings) +{ + //TODO: don't use scaled when scale bug is fixed + UMat scaledDepth; + frame.getScaledDepth(scaledDepth); + if (scaledDepth.empty()) + { + scaledDepth = prepareScaledDepth(frame); + } + + UMat depthMask; + // ignore small, negative, Inf, NaN values + cv::compare(scaledDepth, Scalar(FLT_EPSILON), depthMask, CMP_GT); + + UMat mask; + frame.getMask(mask); + if (mask.empty()) + { + frame.impl->mask = depthMask; + } + else + { + CV_Assert(mask.type() == CV_8UC1); + CV_Assert(mask.size() == scaledDepth.size()); + cv::bitwise_and(mask, depthMask, frame.impl->mask); + } + frame.getMask(mask); + + std::vector iterCounts; + settings.getIterCounts(iterCounts); + + int maxLevel = iterCounts.size() - 1; + std::vector& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH]; + if (dpyramids.empty()) + preparePyramidDepth(scaledDepth, dpyramids, maxLevel); + + Matx33f cameraMatrix; + settings.getCameraMatrix(cameraMatrix); + + std::vector& cpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_CLOUD]; + if (cpyramids.empty()) + preparePyramidCloud(dpyramids, cameraMatrix, cpyramids); +} + + +static void prepareICPFrameSrc(OdometryFrame& frame, OdometrySettings settings) +{ + UMat mask; + frame.getMask(mask); + + const std::vector& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH]; + + std::vector iterCounts; + settings.getIterCounts(iterCounts); + std::vector& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK]; + if (mpyramids.empty()) + preparePyramidMask(mask, dpyramids, iterCounts.size(), settings.getMinDepth(), settings.getMaxDepth(), mpyramids); +} + + +static void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings, Ptr& normalsComputer) +{ + Matx33f cameraMatrix; + settings.getCameraMatrix(cameraMatrix); + + UMat scaledDepth, mask, normals; + frame.getScaledDepth(scaledDepth); + frame.getMask(mask); + frame.getNormals(normals); + + if (normals.empty()) + { + Matx33f K; + if (!normalsComputer.empty()) + normalsComputer->getK(K); + if (normalsComputer.empty() || + normalsComputer->getRows() != scaledDepth.rows || + normalsComputer->getCols() != scaledDepth.cols || + norm(K, cameraMatrix) > FLT_EPSILON) + { + int normalWinSize = settings.getNormalWinSize(); + float diffThreshold = settings.getNormalDiffThreshold(); + RgbdNormals::RgbdNormalsMethod normalMethod = settings.getNormalMethod(); + normalsComputer = RgbdNormals::create(scaledDepth.rows, + scaledDepth.cols, + scaledDepth.depth(), + cameraMatrix, + normalWinSize, + diffThreshold, + normalMethod); + } + const UMat& c0 = frame.impl->pyramids[OdometryFramePyramidType::PYR_CLOUD][0]; + normalsComputer->apply(c0, normals); + frame.impl->normals = normals; + } + + const std::vector& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH]; + + std::vector& npyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_NORM]; + if (npyramids.empty()) + preparePyramidNormals(normals, dpyramids, npyramids); + + std::vector& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK]; + if (mpyramids.empty()) + { + std::vector iterCounts; + settings.getIterCounts(iterCounts); + preparePyramidMask(mask, dpyramids, iterCounts.size(), settings.getMinDepth(), settings.getMaxDepth(), mpyramids); + extendPyrMaskByPyrNormals(npyramids, mpyramids); + } + + std::vector& nmpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_NORMMASK]; + if (nmpyramids.empty()) + preparePyramidNormalsMask(npyramids, mpyramids, settings.getMaxPointsPart(), nmpyramids); +} + + +void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings) +{ + prepareRGBFrameBase(srcFrame, settings); + prepareRGBFrameBase(dstFrame, settings); + + prepareRGBFrameSrc(srcFrame, settings); + prepareRGBFrameDst(dstFrame, settings); +} + +void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr& normalsComputer, OdometrySettings settings, OdometryAlgoType algtype) +{ + prepareICPFrameBase(srcFrame, settings); + prepareICPFrameBase(dstFrame, settings); + + prepareICPFrameSrc(srcFrame, settings); + if (algtype == OdometryAlgoType::FAST) + prepareICPFrameDst(srcFrame, settings, normalsComputer); + prepareICPFrameDst(dstFrame, settings, normalsComputer); +} + +void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr& normalsComputer, OdometrySettings settings, OdometryAlgoType algtype) +{ + prepareRGBFrame(srcFrame, dstFrame, settings); + prepareICPFrame(srcFrame, dstFrame, normalsComputer, settings, algtype); } bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, @@ -678,7 +516,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, srcFrame.getPyramidAt(srcLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level); dstFrame.getPyramidAt(dstLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level); - if (method != OdometryType::DEPTH) + if (method != OdometryType::DEPTH) // RGB(D) { srcFrame.getPyramidAt(srcLevelImage, OdometryFramePyramidType::PYR_IMAGE, level); dstFrame.getPyramidAt(dstLevelImage, OdometryFramePyramidType::PYR_IMAGE, level); @@ -701,7 +539,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, const Mat pyramidMask; srcFrame.getPyramidAt(pyramidMask, OdometryFramePyramidType::PYR_MASK, level); - if(method != OdometryType::DEPTH) // RGB + if(method != OdometryType::DEPTH) // RGB(D) { const Mat pyramidTexturedMask; dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level); @@ -711,7 +549,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB); } - if(method != OdometryType::RGB) // ICP + if(method != OdometryType::RGB) // ICP, RGBD { if (algtype == OdometryAlgoType::COMMON) { @@ -727,7 +565,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount && algtype != OdometryAlgoType::FAST) break; - const Mat srcPyrCloud; + const UMat srcPyrCloud; srcFrame.getPyramidAt(srcPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level); @@ -739,7 +577,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, dstFrame.getPyramidAt(dstPyrImage, OdometryFramePyramidType::PYR_IMAGE, level); dstFrame.getPyramidAt(dstPyrIdx, OdometryFramePyramidType::PYR_DIX, level); dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level); - calcRgbdLsmMatrices(srcPyrCloud, resultRt, dstPyrIdx, dstPyrIdy, + calcRgbdLsmMatrices(srcPyrCloud.getMat(ACCESS_READ), resultRt, dstPyrIdx, dstPyrIdy, corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale, AtA_rgbd, AtB_rgbd, transformType); AtA += AtA_rgbd; @@ -747,17 +585,19 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, } if(corresps_icp.rows >= minCorrespsCount || algtype == OdometryAlgoType::FAST) { - const Mat dstPyrCloud, dstPyrNormals, srcPyrNormals; - dstFrame.getPyramidAt(dstPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level); - dstFrame.getPyramidAt(dstPyrNormals, OdometryFramePyramidType::PYR_NORM, level); - if (algtype == OdometryAlgoType::COMMON) { - calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals, + const Mat dstPyrCloud, dstPyrNormals; + dstFrame.getPyramidAt(dstPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level); + dstFrame.getPyramidAt(dstPyrNormals, OdometryFramePyramidType::PYR_NORM, level); + calcICPLsmMatrices(srcPyrCloud.getMat(ACCESS_READ), resultRt, dstPyrCloud, dstPyrNormals, corresps_icp, AtA_icp, AtB_icp, transformType); } else { + const UMat dstPyrCloud, dstPyrNormals, srcPyrNormals; + dstFrame.getPyramidAt(dstPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level); + dstFrame.getPyramidAt(dstPyrNormals, OdometryFramePyramidType::PYR_NORM, level); srcFrame.getPyramidAt(srcPyrNormals, OdometryFramePyramidType::PYR_NORM, level); cv::Matx66f A; cv::Vec6f b; @@ -1133,6 +973,9 @@ bool testDeltaTransformation(const Mat& deltaRt, double maxTranslation, double m return translation <= maxTranslation && rotation <= maxRotation; } +// Upper triangle buffer size (for symmetric matrix build) +const size_t UTSIZE = 27; + #if USE_INTRINSICS static inline bool fastCheck(const v_float32x4& p0, const v_float32x4& p1) { @@ -1356,9 +1199,11 @@ struct GetAbInvoker : ParallelLoopBody } } + CV_UNUSED(UTSIZE); + #else float upperTriangle[UTSIZE]; - for (int i = 0; i < UTSIZE; i++) + for (size_t i = 0; i < UTSIZE; i++) upperTriangle[i] = 0; for (int y = range.start; y < range.end; y++) @@ -1482,25 +1327,23 @@ struct GetAbInvoker : ParallelLoopBody float minCos; }; -void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, - cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b) +void calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, + cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b) { CV_Assert(oldPts.size() == oldNrm.size()); CV_Assert(newPts.size() == newNrm.size()); CV_OCL_RUN(ocl::isOpenCLActivated(), ocl_calcICPLsmMatricesFast(cameraMatrix, - oldPts.getUMat(AccessFlag::ACCESS_READ), oldNrm.getUMat(AccessFlag::ACCESS_READ), - newPts.getUMat(AccessFlag::ACCESS_READ), newNrm.getUMat(AccessFlag::ACCESS_READ), + oldPts, oldNrm, newPts, newNrm, pose, level, maxDepthDiff, angleThreshold, A, b) ); ABtype sumAB = ABtype::zeros(); Mutex mutex; - const Points op(oldPts), np(newPts); - const Normals on(oldNrm), nn(newNrm); - + const Points op(oldPts.getMat(AccessFlag::ACCESS_READ)), np(newPts.getMat(AccessFlag::ACCESS_READ)); + const Normals on(oldNrm.getMat(AccessFlag::ACCESS_READ)), nn(newNrm.getMat(AccessFlag::ACCESS_READ)); Intr intrinsics(cameraMatrix); GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose, @@ -1526,7 +1369,7 @@ void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& #ifdef HAVE_OPENCL bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, - cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b) + cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b) { CV_TRACE_FUNCTION(); @@ -1564,7 +1407,7 @@ bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const lrows = roundDownPow2(lrows); size_t localSize[2] = { lcols, lrows }; Size ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]), - (int)divUp(globalSize[1], (unsigned int)localSize[1])); + (int)divUp(globalSize[1], (unsigned int)localSize[1])); // size of local buffer for group-wide reduce size_t lsz = localSize[0] * localSize[1] * ltsz; @@ -1599,7 +1442,7 @@ bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const throw std::runtime_error("Failed to run kernel"); float upperTriangle[UTSIZE]; - for (int i = 0; i < UTSIZE; i++) + for (size_t i = 0; i < UTSIZE; i++) upperTriangle[i] = 0; Mat groupedSumCpu = groupedSumGpu.getMat(ACCESS_READ); @@ -1607,10 +1450,10 @@ bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const for (int y = 0; y < ngroups.height; y++) { const float* rowr = groupedSumCpu.ptr(y); - for (int x = 0; x < ngroups.width; x++) + for (size_t x = 0; x < size_t(ngroups.width); x++) { const float* p = rowr + x * UTSIZE; - for (int j = 0; j < UTSIZE; j++) + for (size_t j = 0; j < UTSIZE; j++) { upperTriangle[j] += p[j]; } diff --git a/modules/3d/src/rgbd/odometry_functions.hpp b/modules/3d/src/rgbd/odometry_functions.hpp index aa1530ed9c..c1fd2ca584 100644 --- a/modules/3d/src/rgbd/odometry_functions.hpp +++ b/modules/3d/src/rgbd/odometry_functions.hpp @@ -30,37 +30,6 @@ static inline int getTransformDim(OdometryTransformType transformType) } } -static inline -void checkImage(InputArray image) -{ - if (image.empty()) - CV_Error(Error::StsBadSize, "Image is empty."); - if (image.type() != CV_8UC1) - CV_Error(Error::StsBadSize, "Image type has to be CV_8UC1."); -} - -static inline -void checkDepth(InputArray depth, const Size& imageSize) -{ - if (depth.empty()) - CV_Error(Error::StsBadSize, "Depth is empty."); - if (depth.size() != imageSize) - CV_Error(Error::StsBadSize, "Depth has to have the size equal to the image size."); - if (depth.type() != CV_32FC1) - CV_Error(Error::StsBadSize, "Depth type has to be CV_32FC1."); -} - -static inline -void checkMask(InputArray mask, const Size& imageSize) -{ - if (!mask.empty()) - { - if (mask.size() != imageSize) - CV_Error(Error::StsBadSize, "Mask has to have the size equal to the image size."); - if (mask.type() != CV_8UC1) - CV_Error(Error::StsBadSize, "Mask type has to be CV_8UC1."); - } -} static inline void checkNormals(InputArray normals, const Size& depthSize) @@ -195,46 +164,9 @@ void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f C[i] = ret[i]; } -void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype); -void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth); -void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype); - -void prepareRGBFrameBase(OdometryFrame& frame, const OdometrySettings settings, bool useDepth); -void prepareRGBFrameSrc (OdometryFrame& frame, const OdometrySettings settings); -void prepareRGBFrameDst (OdometryFrame& frame, const OdometrySettings settings); - -void prepareICPFrameBase(OdometryFrame& frame, const OdometrySettings settings); -void prepareICPFrameSrc (OdometryFrame& frame, const OdometrySettings settings); -void prepareICPFrameDst (OdometryFrame& frame, const OdometrySettings settings); - - -void setPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, InputArrayOfArrays pyramidImage); -void getPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, OutputArrayOfArrays _pyramid); - -void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage, size_t levelCount); - -template -void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth, InputArrayOfArrays pyramidNormal, InputOutputArrayOfArrays pyramidMask); - -template -void preparePyramidCloud(InputArrayOfArrays pyramidDepth, const Matx33f& cameraMatrix, InputOutputArrayOfArrays pyramidCloud); - -void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector& pyramidCameraMatrix); - -template -void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel, int sobelSize); - -void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy, - InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale); - -void randomSubsetOfMask(InputOutputArray _mask, float part); - -void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals); - -void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask); - +void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr& normalsComputer, const OdometrySettings settings, OdometryAlgoType algtype); +void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings); +void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr& normalsComputer, const OdometrySettings settings, OdometryAlgoType algtype); bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, const OdometryFrame srcFrame, @@ -260,7 +192,7 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, const Mat& corresps, Mat& AtA, Mat& AtB, OdometryTransformType transformType); -void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, +void calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b); #ifdef HAVE_OPENCL diff --git a/modules/3d/src/rgbd/odometry_settings_impl.cpp b/modules/3d/src/rgbd/odometry_settings_impl.cpp index ea6f2b0403..c5d306ff87 100644 --- a/modules/3d/src/rgbd/odometry_settings_impl.cpp +++ b/modules/3d/src/rgbd/odometry_settings_impl.cpp @@ -31,8 +31,13 @@ public: virtual int getSobelSize() const = 0; virtual void setSobelScale(double val) = 0; virtual double getSobelScale() const = 0; + virtual void setNormalWinSize(int val) = 0; virtual int getNormalWinSize() const = 0; + virtual void setNormalDiffThreshold(float val) = 0; + virtual float getNormalDiffThreshold() const = 0; + virtual void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) = 0; + virtual RgbdNormals::RgbdNormalsMethod getNormalMethod() const = 0; virtual void setAngleThreshold(float val) = 0; virtual float getAngleThreshold() const = 0; @@ -70,8 +75,13 @@ public: virtual int getSobelSize() const override; virtual void setSobelScale(double val) override; virtual double getSobelScale() const override; + virtual void setNormalWinSize(int val) override; virtual int getNormalWinSize() const override; + virtual void setNormalDiffThreshold(float val) override; + virtual float getNormalDiffThreshold() const override; + virtual void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) override; + virtual RgbdNormals::RgbdNormalsMethod getNormalMethod() const override; virtual void setAngleThreshold(float val) override; virtual float getAngleThreshold() const override; @@ -96,7 +106,10 @@ private: int sobelSize; double sobelScale; + int normalWinSize; + float normalDiffThreshold; + RgbdNormals::RgbdNormalsMethod normalMethod; float angleThreshold; float maxTranslation; @@ -124,7 +137,10 @@ public: static const int defaultSobelSize = 3; static constexpr double defaultSobelScale = 1. / 8.; + static const int defaultNormalWinSize = 5; + static const RgbdNormals::RgbdNormalsMethod defaultNormalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS; + static constexpr float defaultNormalDiffThreshold = 50.f; static constexpr float defaultAngleThreshold = (float)(30. * CV_PI / 180.); static constexpr float defaultMaxTranslation = 0.15f; @@ -159,8 +175,13 @@ void OdometrySettings::setSobelSize(int val) { this->impl->setSobelSize(val); } int OdometrySettings::getSobelSize() const { return this->impl->getSobelSize(); } void OdometrySettings::setSobelScale(double val) { this->impl->setSobelScale(val); } double OdometrySettings::getSobelScale() const { return this->impl->getSobelScale(); } -void OdometrySettings::setNormalWinSize(int val) { this->impl->setNormalWinSize(val); } -int OdometrySettings::getNormalWinSize() const { return this->impl->getNormalWinSize(); } + +void OdometrySettings::setNormalWinSize(int val) { this->impl->setNormalWinSize(val); } +int OdometrySettings::getNormalWinSize() const { return this->impl->getNormalWinSize(); } +void OdometrySettings::setNormalDiffThreshold(float val) { this->impl->setNormalDiffThreshold(val); } +float OdometrySettings::getNormalDiffThreshold() const { return this->impl->getNormalDiffThreshold(); } +void OdometrySettings::setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) { this->impl->setNormalMethod(nm); } +RgbdNormals::RgbdNormalsMethod OdometrySettings::getNormalMethod() const { return this->impl->getNormalMethod(); } void OdometrySettings::setAngleThreshold(float val) { this->impl->setAngleThreshold(val); } float OdometrySettings::getAngleThreshold() const { return this->impl->getAngleThreshold(); } @@ -188,7 +209,10 @@ OdometrySettingsImplCommon::OdometrySettingsImplCommon() this->sobelSize = ds.defaultSobelSize; this->sobelScale = ds.defaultSobelScale; + this->normalWinSize = ds.defaultNormalWinSize; + this->normalDiffThreshold = ds.defaultNormalDiffThreshold; + this->normalMethod = ds.defaultNormalMethod; this->angleThreshold = ds.defaultAngleThreshold; this->maxTranslation = ds.defaultMaxTranslation; @@ -298,7 +322,22 @@ int OdometrySettingsImplCommon::getNormalWinSize() const { return this->normalWinSize; } - +void OdometrySettingsImplCommon::setNormalDiffThreshold(float val) +{ + this->normalDiffThreshold = val; +} +float OdometrySettingsImplCommon::getNormalDiffThreshold() const +{ + return this->normalDiffThreshold; +} +void OdometrySettingsImplCommon::setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) +{ + this->normalMethod = nm; +} +RgbdNormals::RgbdNormalsMethod OdometrySettingsImplCommon::getNormalMethod() const +{ + return this->normalMethod; +} void OdometrySettingsImplCommon::setAngleThreshold(float val) { this->angleThreshold = val; diff --git a/modules/3d/src/rgbd/utils.cpp b/modules/3d/src/rgbd/utils.cpp deleted file mode 100644 index 6227895dbc..0000000000 --- a/modules/3d/src/rgbd/utils.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html - -#include "../precomp.hpp" -#include "utils.hpp" - -namespace cv -{ - -template -inline TMat getTMat(InputArray, int) -{ - return TMat(); -} - -template<> -Mat getTMat(InputArray a, int i) -{ - return a.getMat(i); -} - -template<> -UMat getTMat(InputArray a, int i) -{ - return a.getUMat(i); -} - -template -inline TMat& getTMatRef(InputOutputArray, int) -{ - return TMat(); -} - -template<> -Mat& getTMatRef(InputOutputArray a, int i) -{ - return a.getMatRef(i); -} - -/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided - * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits::quiet_NaN() - * Otherwise, the image is simply converted to floats - * @param in_in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters - * (as done with the Microsoft Kinect), it is assumed in meters) - * @param depth the desired output depth (floats or double) - * @param out_out The rescaled float depth image - */ -void rescaleDepth(InputArray in_in, int type, OutputArray out_out, double depth_factor) -{ - cv::Mat in = in_in.getMat(); - CV_Assert(in.type() == CV_64FC1 || in.type() == CV_32FC1 || in.type() == CV_16UC1 || in.type() == CV_16SC1); - CV_Assert(type == CV_64FC1 || type == CV_32FC1); - - int in_depth = in.depth(); - - out_out.create(in.size(), type); - cv::Mat out = out_out.getMat(); - if (in_depth == CV_16U) - { - in.convertTo(out, type, 1 / depth_factor); //convert to float so that it is in meters - cv::Mat valid_mask = in == std::numeric_limits::min(); // Should we do std::numeric_limits::max() too ? - out.setTo(std::numeric_limits::quiet_NaN(), valid_mask); //set a$ - } - if (in_depth == CV_16S) - { - in.convertTo(out, type, 1 / depth_factor); //convert to float so tha$ - cv::Mat valid_mask = (in == std::numeric_limits::min()) | (in == std::numeric_limits::max()); // Should we do std::numeric_limits::max() too ? - out.setTo(std::numeric_limits::quiet_NaN(), valid_mask); //set a$ - } - if ((in_depth == CV_32F) || (in_depth == CV_64F)) - in.convertTo(out, type); -} -} // namespace cv diff --git a/modules/3d/src/rgbd/utils.hpp b/modules/3d/src/rgbd/utils.hpp index d0fcf1c748..cca2e9464f 100644 --- a/modules/3d/src/rgbd/utils.hpp +++ b/modules/3d/src/rgbd/utils.hpp @@ -13,7 +13,6 @@ namespace cv { - /** Checks if the value is a valid depth. For CV_16U or CV_16S, the convention is to be invalid if it is * a limit. For a float/double, we just check if it is a NaN * @param depth the depth to check for validity @@ -46,41 +45,12 @@ inline bool isValidDepth(const int& depth) (depth != std::numeric_limits::max()); } - inline bool isValidDepth(const unsigned int& depth) { return (depth != std::numeric_limits::min()) && (depth != std::numeric_limits::max()); } -/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided - * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits::quiet_NaN() - * Otherwise, the image is simply converted to floats - * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters - * (as done with the Microsoft Kinect), it is assumed in meters) - * @param the desired output depth (floats or double) - * @param out The rescaled float depth image - */ -/* void rescaleDepth(InputArray in_in, int depth, OutputArray out_out); */ - -template -void -rescaleDepthTemplated(const Mat& in, Mat& out); - -template<> -inline void -rescaleDepthTemplated(const Mat& in, Mat& out) -{ - rescaleDepth(in, CV_32F, out); -} - -template<> -inline void -rescaleDepthTemplated(const Mat& in, Mat& out) -{ - rescaleDepth(in, CV_64F, out); -} - // One place to turn intrinsics on and off #define USE_INTRINSICS CV_SIMD128 @@ -105,18 +75,6 @@ static inline bool isNaN(const cv::v_float32x4& p) } #endif -template -inline TMat getTMat(InputArray, int = -1); -template<> -Mat getTMat(InputArray a, int i); -template<> -UMat getTMat(InputArray a, int i); - -template -inline TMat& getTMatRef(InputOutputArray, int = -1); -template<> -Mat& getTMatRef(InputOutputArray a, int i); - inline size_t roundDownPow2(size_t x) { size_t shift = 0; @@ -280,6 +238,33 @@ struct Intr float fx, fy, cx, cy; }; +class OdometryFrame::Impl +{ +public: + Impl() : pyramids(OdometryFramePyramidType::N_PYRAMIDS) { } + virtual ~Impl() {} + + virtual void getImage(OutputArray image) const ; + virtual void getGrayImage(OutputArray image) const ; + virtual void getDepth(OutputArray depth) const ; + virtual void getScaledDepth(OutputArray depth) const ; + virtual void getMask(OutputArray mask) const ; + virtual void getNormals(OutputArray normals) const ; + + virtual size_t getPyramidLevels() const ; + + virtual void getPyramidAt(OutputArray img, + OdometryFramePyramidType pyrType, size_t level) const ; + + UMat imageGray; + UMat image; + UMat depth; + UMat scaledDepth; + UMat mask; + UMat normals; + std::vector< std::vector > pyramids; +}; + } // namespace cv diff --git a/modules/3d/src/rgbd/volume_impl.cpp b/modules/3d/src/rgbd/volume_impl.cpp index a2edc063e4..6ebf3a1dcb 100644 --- a/modules/3d/src/rgbd/volume_impl.cpp +++ b/modules/3d/src/rgbd/volume_impl.cpp @@ -96,47 +96,12 @@ void TsdfVolume::integrate(InputArray, InputArray, InputArray) CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors"); } -void TsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const -{ - raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame); -} void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const { raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); } -void TsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const -{ -#ifndef HAVE_OPENCL - Mat points, normals; - raycast(cameraPose, height, width, points, normals, noArray()); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); -#else - if (useGPU) - { - UMat points, normals; - raycast(cameraPose, height, width, points, normals, noArray()); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - } - else - { - Mat points, normals; - raycast(cameraPose, height, width, points, normals, noArray()); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - } -#endif -} - void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const { @@ -301,47 +266,13 @@ void HashTsdfVolume::integrate(InputArray, InputArray, InputArray) CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors"); } -void HashTsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const -{ - raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame); -} void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const { raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); } -void HashTsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const -{ -#ifndef HAVE_OPENCL - Mat points, normals; - raycast(cameraPose, height, width, points, normals, noArray()); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); -#else - if (useGPU) - { - UMat points, normals; - raycast(cameraPose, height, width, points, normals, noArray()); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - } - else - { - Mat points, normals; - raycast(cameraPose, height, width, points, normals, noArray()); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - } -#endif -} void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const { if (_colors.needed()) @@ -481,27 +412,11 @@ void ColorTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray integrateColorTsdfVolumeUnit(settings, cameraPose, depth, image, pixNorms, volume); } -void ColorTsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const -{ - raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame); -} void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const { raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); } -void ColorTsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const -{ - Mat points, normals, colors; - raycast(cameraPose, height, width, points, normals, colors); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM); - outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_IMAGE); - outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - outFrame.setPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); -} - void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const { const Matx44f cameraPose = _cameraPose.getMat(); diff --git a/modules/3d/src/rgbd/volume_impl.hpp b/modules/3d/src/rgbd/volume_impl.hpp index 9ad51f0f8c..b0af2b5a54 100644 --- a/modules/3d/src/rgbd/volume_impl.hpp +++ b/modules/3d/src/rgbd/volume_impl.hpp @@ -26,10 +26,7 @@ public: virtual void integrate(InputArray depth, InputArray pose) = 0; virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0; - virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const = 0; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0; - - virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const = 0; virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const = 0; virtual void fetchNormals(InputArray points, OutputArray normals) const = 0; @@ -57,9 +54,7 @@ public: virtual void integrate(const OdometryFrame& frame, InputArray pose) override; virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; - virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override; virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; @@ -100,9 +95,7 @@ public: virtual void integrate(const OdometryFrame& frame, InputArray pose) override; virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; - virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override; virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; @@ -151,9 +144,7 @@ public: virtual void integrate(const OdometryFrame& frame, InputArray pose) override; virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; - virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override; virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; @@ -202,9 +193,7 @@ Volume::~Volume() {} void Volume::integrate(const OdometryFrame& frame, InputArray pose) { this->impl->integrate(frame, pose); } 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, OdometryFrame& outFrame) const { this->impl->raycast(cameraPose, outFrame); } 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, OdometryFrame& outFrame) const { this->impl->raycast(cameraPose, height, width, outFrame); } 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::fetchNormals(InputArray points, OutputArray normals) const { this->impl->fetchNormals(points, normals); } void Volume::fetchPointsNormals(OutputArray points, OutputArray normals) const { this->impl->fetchPointsNormals(points, normals); } diff --git a/modules/3d/test/ocl/test_tsdf.cpp b/modules/3d/test/ocl/test_tsdf.cpp index 63f7b8c415..792da2f2f9 100644 --- a/modules/3d/test/ocl/test_tsdf.cpp +++ b/modules/3d/test/ocl/test_tsdf.cpp @@ -358,8 +358,7 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test Mat points, normals; AccessFlag af = ACCESS_READ; - OdometryFrame odf(OdometryFrameStoreType::UMAT); - odf.setDepth(udepth); + OdometryFrame odf(noArray(), udepth); if (testSrcType == VolumeTestSrcType::MAT) volume.integrate(depth, poses[0].matrix); @@ -368,24 +367,15 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test if (testFunction == VolumeTestFunction::RAYCAST) { - if (testSrcType == VolumeTestSrcType::MAT) - { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf); - odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0); - } + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, 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.fetchPointsNormals(upoints, utmpnormals); + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, utmpnormals); + // volume.fetchPointsNormals(upoints, utmpnormals); volume.fetchNormals(upoints, unormals); } } @@ -427,8 +417,7 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test Mat points, normals; AccessFlag af = ACCESS_READ; - OdometryFrame odf(OdometryFrameStoreType::UMAT); - odf.setDepth(udepth); + OdometryFrame odf(noArray(), udepth); if (testSrcType == VolumeTestSrcType::MAT) volume.integrate(depth, poses[0].matrix); @@ -437,16 +426,7 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test if (testFunction == VolumeTestFunction::RAYCAST) { - if (testSrcType == VolumeTestSrcType::MAT) - { volume.raycast(poses[0].matrix, upoints, unormals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, odf); - odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0); - } } else if (testFunction == VolumeTestFunction::FETCH_NORMALS) { @@ -498,25 +478,14 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType AccessFlag af = ACCESS_READ; int anfas, profile; - OdometryFrame odf(OdometryFrameStoreType::UMAT); - odf.setDepth(udepth); + OdometryFrame odf(noArray(), udepth); if (testSrcType == VolumeTestSrcType::MAT) volume.integrate(depth, poses[0].matrix); else volume.integrate(odf, poses[0].matrix); - - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf); - odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0); - } + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); normals = unormals.getMat(af); points = upoints.getMat(af); @@ -526,16 +495,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType if (cvtest::debugLevel > 0) displayImage(depth, points, normals, depthFactor, lightPose); - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, upoints1, unormals1); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, odf); - odf.getPyramidAt(upoints1, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(unormals1, OdometryFramePyramidType::PYR_NORM, 0); - } + volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, upoints1, unormals1); normals = unormals1.getMat(af); points = upoints1.getMat(af); @@ -576,25 +536,14 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType AccessFlag af = ACCESS_READ; int anfas, profile; - OdometryFrame odf(OdometryFrameStoreType::UMAT); - odf.setDepth(udepth); + OdometryFrame odf(noArray(), udepth); if (testSrcType == VolumeTestSrcType::MAT) volume.integrate(depth, poses[0].matrix); else volume.integrate(odf, poses[0].matrix); - - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - volume.raycast(poses[0].matrix, upoints, unormals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, odf); - odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0); - } + volume.raycast(poses[0].matrix, upoints, unormals); normals = unormals.getMat(af); points = upoints.getMat(af); @@ -604,19 +553,10 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType if (cvtest::debugLevel > 0) displayImage(depth, points, normals, depthFactor, lightPose); - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - volume.raycast(poses[17].matrix, upoints1, unormals1); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[17].matrix, odf); - odf.getPyramidAt(upoints1, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(unormals1, OdometryFramePyramidType::PYR_NORM, 0); - } + volume.raycast(poses[17].matrix, upoints1, unormals1); normals = unormals1.getMat(af); - points = upoints1.getMat(af); + points = upoints1.getMat(af); patchNaNs(points); profile = counterOfValid(points); diff --git a/modules/3d/test/test_normal.cpp b/modules/3d/test/test_normal.cpp index 0ef419f26a..4ed574090c 100644 --- a/modules/3d/test/test_normal.cpp +++ b/modules/3d/test/test_normal.cpp @@ -625,7 +625,7 @@ TEST_P(RenderedNormals, check) INSTANTIATE_TEST_CASE_P(RGBD_Normals, RenderedNormals, ::testing::Combine(::testing::Values(CV_32F, CV_64F), ::testing::Values( - NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_FALS, { 81.8210, 0}}, + NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_FALS, { 81.8213, 0}}, NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, { 107.2710, 29168}}, NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_SRI, { 73.2027, 17693}}, NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, { 57.9832, 2531}}), diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index d1193dbdd8..c67aeeaae8 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -136,18 +136,17 @@ void OdometryTest::checkUMats() Mat image, depth; readData(image, depth); - OdometrySettings ods; - ods.setCameraMatrix(K); - Odometry odometry = Odometry(otype, ods, algtype); - OdometryFrame odf = odometry.createOdometryFrame(OdometryFrameStoreType::UMAT); - - Mat calcRt; - UMat uimage, udepth; image.copyTo(uimage); depth.copyTo(udepth); - odf.setImage(uimage); - odf.setDepth(udepth); + + OdometrySettings ods; + ods.setCameraMatrix(K); + Odometry odometry = Odometry(otype, ods, algtype); + OdometryFrame odf(uimage, udepth); + + Mat calcRt; + uimage.release(); udepth.release(); @@ -155,7 +154,7 @@ void OdometryTest::checkUMats() bool isComputed = odometry.compute(odf, odf, calcRt); ASSERT_TRUE(isComputed); double diff = cv::norm(calcRt, Mat::eye(4, 4, CV_64FC1)); - ASSERT_FALSE(diff > idError) << "Incorrect transformation between the same frame (not the identity matrix), diff = " << diff << std::endl; + ASSERT_LE(diff, idError) << "Incorrect transformation between the same frame (not the identity matrix)" << std::endl; } void OdometryTest::run() @@ -167,9 +166,7 @@ void OdometryTest::run() OdometrySettings ods; ods.setCameraMatrix(K); Odometry odometry = Odometry(otype, ods, algtype); - OdometryFrame odf = odometry.createOdometryFrame(); - odf.setImage(image); - odf.setDepth(depth); + OdometryFrame odf(image, depth); Mat calcRt; // 1. Try to find Rt between the same frame (try masks also). @@ -180,7 +177,7 @@ void OdometryTest::run() ASSERT_TRUE(isComputed) << "Can not find Rt between the same frame" << std::endl; double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1)); - ASSERT_FALSE(ndiff > idError) << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl; + ASSERT_LE(ndiff, idError) << "Incorrect transformation between the same frame (not the identity matrix)" << std::endl; // 2. Generate random rigid body motion in some ranges several times (iterCount). // On each iteration an input frame is warped using generated transformation. @@ -202,13 +199,8 @@ void OdometryTest::run() warpFrame(depth, image, noArray(), rt.matrix, K, warpedDepth, warpedImage); dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping - OdometryFrame odfSrc = odometry.createOdometryFrame(); - OdometryFrame odfDst = odometry.createOdometryFrame(); - - odfSrc.setImage(image); - odfSrc.setDepth(depth); - odfDst.setImage(warpedImage); - odfDst.setDepth(warpedDepth); + OdometryFrame odfSrc(image, depth); + OdometryFrame odfDst(warpedImage, warpedDepth); odometry.prepareFrames(odfSrc, odfDst); isComputed = odometry.compute(odfSrc, odfDst, calcRt); @@ -235,7 +227,7 @@ void OdometryTest::run() } // compare rotation - double possibleError = algtype == OdometryAlgoType::COMMON ? 0.11f : 0.015f; + double possibleError = algtype == OdometryAlgoType::COMMON ? 0.015f : 0.01f; Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec)); Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec)); @@ -275,44 +267,178 @@ void OdometryTest::prepareFrameCheck() { Mat K = getCameraMatrix(); - Mat image, depth; - readData(image, depth); + Mat gtImage, gtDepth; + readData(gtImage, gtDepth); OdometrySettings ods; ods.setCameraMatrix(K); Odometry odometry = Odometry(otype, ods, algtype); - OdometryFrame odf = odometry.createOdometryFrame(); - odf.setImage(image); - odf.setDepth(depth); + OdometryFrame odf(gtImage, gtDepth); odometry.prepareFrame(odf); - Mat points, mask; - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(mask, OdometryFramePyramidType::PYR_MASK, 0); + std::vector iters; + ods.getIterCounts(iters); + size_t nlevels = iters.size(); - OdometryFrame todf = odometry.createOdometryFrame(); - if (otype != OdometryType::DEPTH) + Mat points, mask, depth, gray, rgb, scaled; + + odf.getMask(mask); + int masknz = countNonZero(mask); + ASSERT_GT(masknz, 0); + + odf.getDepth(depth); + Mat patchedDepth = depth.clone(); + patchNaNs(patchedDepth, 0); + int depthnz = countNonZero(patchedDepth); + double depthNorm = cv::norm(depth, gtDepth, NORM_INF, mask); + ASSERT_LE(depthNorm, 0.0); + + Mat gtGray; + if (otype == OdometryType::RGB || otype == OdometryType::RGB_DEPTH) { - Mat img; - odf.getPyramidAt(img, OdometryFramePyramidType::PYR_IMAGE, 0); - todf.setPyramidLevel(1, OdometryFramePyramidType::PYR_IMAGE); - todf.setPyramidAt(img, OdometryFramePyramidType::PYR_IMAGE, 0); - } - todf.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD); - todf.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - todf.setPyramidLevel(1, OdometryFramePyramidType::PYR_MASK); - todf.setPyramidAt(mask, OdometryFramePyramidType::PYR_MASK, 0); + odf.getGrayImage(gray); + odf.getImage(rgb); + double rgbNorm = cv::norm(rgb, gtImage); + ASSERT_LE(rgbNorm, 0.0); - odometry.prepareFrame(todf); + if (gtImage.channels() == 3) + cvtColor(gtImage, gtGray, COLOR_BGR2GRAY); + else + gtGray = gtImage; + gtGray.convertTo(gtGray, CV_8U); + double grayNorm = cv::norm(gray, gtGray); + ASSERT_LE(grayNorm, 0.0); + } + + //TODO: remove it when scale issue is fixed + odf.getScaledDepth(scaled); + int scalednz = countNonZero(scaled); + EXPECT_EQ(scalednz, depthnz); + + std::vector gtPyrDepth, gtPyrMask; + //TODO: this depth calculation would become incorrect when we implement bilateral filtering, fixit + buildPyramid(gtDepth, gtPyrDepth, nlevels - 1); + for (const auto& gd : gtPyrDepth) + { + Mat pm = (gd > ods.getMinDepth()) & (gd < ods.getMaxDepth()); + gtPyrMask.push_back(pm); + } + + size_t npyr = odf.getPyramidLevels(); + ASSERT_EQ(npyr, nlevels); + Matx33f levelK = K; + for (size_t i = 0; i < nlevels; i++) + { + Mat depthi, cloudi, maski; + + odf.getPyramidAt(maski, OdometryFramePyramidType::PYR_MASK, i); + ASSERT_FALSE(maski.empty()); + double mnorm = cv::norm(maski, gtPyrMask[i]); + EXPECT_LE(mnorm, 16 * 255.0) << "Mask diff is too big at pyr level " << i; + + odf.getPyramidAt(depthi, OdometryFramePyramidType::PYR_DEPTH, i); + ASSERT_FALSE(depthi.empty()); + double dnorm = cv::norm(depthi, gtPyrDepth[i], NORM_INF, maski); + EXPECT_LE(dnorm, 8.e-7) << "Depth diff norm is too big at pyr level " << i; + + odf.getPyramidAt(cloudi, OdometryFramePyramidType::PYR_CLOUD, i); + ASSERT_FALSE(cloudi.empty()); + Mat gtCloud; + depthTo3d(depthi, levelK, gtCloud); + double cnorm = cv::norm(cloudi, gtCloud, NORM_INF, maski); + EXPECT_LE(cnorm, 0.0) << "Cloud diff norm is too big at pyr level " << i; + // downscale camera matrix for next pyramid level + levelK = 0.5f * levelK; + levelK(2, 2) = 1.f; + } + + if (otype == OdometryType::RGB || otype == OdometryType::RGB_DEPTH) + { + std::vector gtPyrImage; + buildPyramid(gtGray, gtPyrImage, nlevels - 1); + + for (size_t i = 0; i < nlevels; i++) + { + Mat rgbi, texi, dixi, diyi, maski; + odf.getPyramidAt(maski, OdometryFramePyramidType::PYR_MASK, i); + odf.getPyramidAt(rgbi, OdometryFramePyramidType::PYR_IMAGE, i); + ASSERT_FALSE(rgbi.empty()); + double rnorm = cv::norm(rgbi, gtPyrImage[i], NORM_INF); + EXPECT_LE(rnorm, 1.0) << "RGB diff is too big at pyr level " << i; + odf.getPyramidAt(texi, OdometryFramePyramidType::PYR_TEXMASK, i); + ASSERT_FALSE(texi.empty()); + int tnz = countNonZero(texi); + EXPECT_GE(tnz, 1000) << "Texture mask has too few valid pixels at pyr level " << i; + Mat gtDixi, gtDiyi; + Sobel(rgbi, gtDixi, CV_16S, 1, 0, ods.getSobelSize()); + odf.getPyramidAt(dixi, OdometryFramePyramidType::PYR_DIX, i); + ASSERT_FALSE(dixi.empty()); + double dixnorm = cv::norm(dixi, gtDixi, NORM_INF, maski); + EXPECT_LE(dixnorm, 0) << "dI/dx diff is too big at pyr level " << i; + Sobel(rgbi, gtDiyi, CV_16S, 0, 1, ods.getSobelSize()); + odf.getPyramidAt(diyi, OdometryFramePyramidType::PYR_DIY, i); + ASSERT_FALSE(diyi.empty()); + double diynorm = cv::norm(diyi, gtDiyi, NORM_INF, maski); + EXPECT_LE(diynorm, 0) << "dI/dy diff is too big at pyr level " << i; + } + } + + if (otype == OdometryType::DEPTH || otype == OdometryType::RGB_DEPTH) + { + Ptr normalComputer = odometry.getNormalsComputer(); + ASSERT_FALSE(normalComputer.empty()); + Mat normals; + odf.getNormals(normals); + std::vector gtPyrNormals; + buildPyramid(normals, gtPyrNormals, nlevels - 1); + for (size_t i = 0; i < nlevels; i++) + { + Mat gtNormal = gtPyrNormals[i]; + CV_Assert(gtNormal.type() == CV_32FC4); + for (int y = 0; y < gtNormal.rows; y++) + { + Vec4f *normals_row = gtNormal.ptr(y); + for (int x = 0; x < gtNormal.cols; x++) + { + Vec4f n4 = normals_row[x]; + Point3f n(n4[0], n4[1], n4[2]); + double nrm = cv::norm(n); + n *= 1.f / nrm; + normals_row[x] = Vec4f(n.x, n.y, n.z, 0); + } + } + + Mat normmaski; + odf.getPyramidAt(normmaski, OdometryFramePyramidType::PYR_NORMMASK, i); + ASSERT_FALSE(normmaski.empty()); + int nnm = countNonZero(normmaski); + EXPECT_GE(nnm, 1000) << "Normals mask has too few valid pixels at pyr level " << i; + + Mat ptsi; + odf.getPyramidAt(ptsi, OdometryFramePyramidType::PYR_CLOUD, i); + + Mat normi; + odf.getPyramidAt(normi, OdometryFramePyramidType::PYR_NORM, i); + ASSERT_FALSE(normi.empty()); + double nnorm = cv::norm(normi, gtNormal, NORM_INF, normmaski); + EXPECT_LE(nnorm, 1.8e-7) << "Normals diff is too big at pyr level " << i; + + if (i == 0) + { + double pnnorm = cv::norm(normals, normi, NORM_INF, normmaski); + EXPECT_GE(pnnorm, 0); + } + } + } } /****************************************************************************************\ * Tests registrations * \****************************************************************************************/ -TEST(RGBD_Odometry_Rgbd, algorithmic) +TEST(RGBD_Odometry_Rgb, algorithmic) { - OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.89); + OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99); test.run(); } @@ -330,14 +456,14 @@ TEST(RGBD_Odometry_RgbdICP, algorithmic) TEST(RGBD_Odometry_FastICP, algorithmic) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.87, 1.84e-5); test.run(); } -TEST(RGBD_Odometry_Rgbd, UMats) +TEST(RGBD_Odometry_Rgb, UMats) { - OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.89); + OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99); test.checkUMats(); } @@ -355,14 +481,15 @@ TEST(RGBD_Odometry_RgbdICP, UMats) TEST(RGBD_Odometry_FastICP, UMats) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); + // OpenCL version has slightly less accuracy than CPU version + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, 1.84e-5); test.checkUMats(); } -TEST(RGBD_Odometry_Rgbd, prepareFrame) +TEST(RGBD_Odometry_Rgb, prepareFrame) { - OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.89); + OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99); test.prepareFrameCheck(); } @@ -380,7 +507,7 @@ TEST(RGBD_Odometry_RgbdICP, prepareFrame) TEST(RGBD_Odometry_FastICP, prepareFrame) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, FLT_EPSILON); test.prepareFrameCheck(); } diff --git a/modules/3d/test/test_tsdf.cpp b/modules/3d/test/test_tsdf.cpp index eafcc53e84..12df205ccc 100644 --- a/modules/3d/test/test_tsdf.cpp +++ b/modules/3d/test/test_tsdf.cpp @@ -489,9 +489,7 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test Mat rgb = scene->rgb(poses[0]); Mat points, normals, tmpnormals, colors; - OdometryFrame odf; - odf.setDepth(depth); - odf.setImage(rgb); + OdometryFrame odf(rgb, depth); if (testSrcType == VolumeTestSrcType::MAT) { @@ -507,21 +505,10 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test if (testFunction == VolumeTestFunction::RAYCAST) { - if (testSrcType == VolumeTestSrcType::MAT) - { - if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); - else - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf); - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - if (volumeType == VolumeType::ColorTSDF) - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - } + if (volumeType == VolumeType::ColorTSDF) + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); + else + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); } else if (testFunction == VolumeTestFunction::FETCH_NORMALS) { @@ -568,9 +555,7 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test Mat rgb = scene->rgb(poses[0]); Mat points, normals, tmpnormals, colors; - OdometryFrame odf; - odf.setDepth(depth); - odf.setImage(rgb); + OdometryFrame odf(rgb, depth); if (testSrcType == VolumeTestSrcType::MAT) { @@ -586,21 +571,10 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test if (testFunction == VolumeTestFunction::RAYCAST) { - if (testSrcType == VolumeTestSrcType::MAT) - { - if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, points, normals, colors); - else - volume.raycast(poses[0].matrix, points, normals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, odf); - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - if (volumeType == VolumeType::ColorTSDF) - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - } + if (volumeType == VolumeType::ColorTSDF) + volume.raycast(poses[0].matrix, points, normals, colors); + else + volume.raycast(poses[0].matrix, points, normals); } else if (testFunction == VolumeTestFunction::FETCH_NORMALS) { @@ -648,9 +622,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType Mat points, normals, colors, newPoints, newNormals; int anfas, profile; - OdometryFrame odf; - odf.setDepth(depth); - odf.setImage(rgb); + OdometryFrame odf(rgb, depth); if (testSrcType == VolumeTestSrcType::MAT) { @@ -664,21 +636,10 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType volume.integrate(odf, poses[0].matrix); } - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); - else - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf); - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - if (volumeType == VolumeType::ColorTSDF) - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - } + if (volumeType == VolumeType::ColorTSDF) + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); + else + volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); patchNaNs(points); anfas = counterOfValid(points); @@ -694,21 +655,10 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType points.release(); normals.release(); - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals, colors); - else - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, odf); - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - if (volumeType == VolumeType::ColorTSDF) - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - } + if (volumeType == VolumeType::ColorTSDF) + volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals, colors); + else + volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals); patchNaNs(points); profile = counterOfValid(points); @@ -748,9 +698,7 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType Mat points, normals, colors, newPoints, newNormals; int anfas, profile; - OdometryFrame odf; - odf.setDepth(depth); - odf.setImage(rgb); + OdometryFrame odf(rgb, depth); if (testSrcType == VolumeTestSrcType::MAT) { @@ -764,21 +712,10 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType volume.integrate(odf, poses[0].matrix); } - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[0].matrix, points, normals, colors); - else - volume.raycast(poses[0].matrix, points, normals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[0].matrix, odf); - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - if (volumeType == VolumeType::ColorTSDF) - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - } + if (volumeType == VolumeType::ColorTSDF) + volume.raycast(poses[0].matrix, points, normals, colors); + else + volume.raycast(poses[0].matrix, points, normals); patchNaNs(points); anfas = counterOfValid(points); @@ -794,21 +731,10 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType points.release(); normals.release(); - if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats - { - if (volumeType == VolumeType::ColorTSDF) - volume.raycast(poses[17].matrix, points, normals, colors); - else - volume.raycast(poses[17].matrix, points, normals); - } - else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME) - { - volume.raycast(poses[17].matrix, odf); - odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0); - odf.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0); - if (volumeType == VolumeType::ColorTSDF) - odf.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0); - } + if (volumeType == VolumeType::ColorTSDF) + volume.raycast(poses[17].matrix, points, normals, colors); + else + volume.raycast(poses[17].matrix, points, normals); patchNaNs(points); profile = counterOfValid(points);