From 2407ab4e61e5084e5c5d66b7f17da09b377cf840 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 22 Nov 2022 07:24:41 +0100 Subject: [PATCH] Merge pull request #22178 from savuor:hash_tsdf_fixes This PR contains: - a new property enableGrowth which controls should the HashTSDF be extended during integration by adding new volume units or not - a new method getBoundingBox which calculates the size of currently occupied data - a set of tests to check that new functionality - a fix for TSDF GPU reset (data is correctly zeroed now using floatToTsdf() function) minor changes --- modules/3d/include/opencv2/3d/volume.hpp | 28 ++- modules/3d/src/rgbd/hash_tsdf_functions.cpp | 226 ++++++++++---------- modules/3d/src/rgbd/hash_tsdf_functions.hpp | 5 +- modules/3d/src/rgbd/tsdf_functions.cpp | 11 +- modules/3d/src/rgbd/volume_impl.cpp | 148 ++++++++++++- modules/3d/src/rgbd/volume_impl.hpp | 50 +++++ modules/3d/test/ocl/test_tsdf.cpp | 1 + modules/3d/test/test_tsdf.cpp | 214 +++++++++++++++++- 8 files changed, 546 insertions(+), 137 deletions(-) diff --git a/modules/3d/include/opencv2/3d/volume.hpp b/modules/3d/include/opencv2/3d/volume.hpp index fcfc2de8b0..4e687d559e 100644 --- a/modules/3d/include/opencv2/3d/volume.hpp +++ b/modules/3d/include/opencv2/3d/volume.hpp @@ -23,7 +23,7 @@ public: * @param settings the custom settings for volume. */ Volume(VolumeType vtype, const VolumeSettings& settings); - ~Volume(); + virtual ~Volume(); /** @brief Integrates the input data to the volume. @@ -102,14 +102,38 @@ public: */ void reset(); + //TODO: remove this /** @brief return visible blocks in volume. */ int getVisibleBlocks() const; - /** @brief return number of vulmeunits in volume. + /** @brief return number of volume units in volume. */ size_t getTotalVolumeUnits() const; + enum BoundingBoxPrecision + { + VOLUME_UNIT = 0, + VOXEL = 1 + }; + /** @brief Gets bounding box in volume coordinates with given precision: + * VOLUME_UNIT - up to volume unit + * VOXEL - up to voxel (currently not supported) + * @return (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates + */ + virtual Vec6f getBoundingBox(int precision) const; + + /** + * @brief Enables or disables new volume unit allocation during integration. + * Makes sense for HashTSDF only. + */ + virtual void setEnableGrowth(bool v); + /** + * @brief Returns if new volume units are allocated during integration or not. + * Makes sense for HashTSDF only. + */ + virtual bool getEnableGrowth() const; + class Impl; private: Ptr impl; diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.cpp b/modules/3d/src/rgbd/hash_tsdf_functions.cpp index 45066ae9f6..c2a689233f 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.cpp +++ b/modules/3d/src/rgbd/hash_tsdf_functions.cpp @@ -89,11 +89,9 @@ Point3f ocl_getNormalVoxel( #endif void integrateHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits) { - //std::cout << "integrateHashTsdfVolumeUnit()" << std::endl; - CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); @@ -101,10 +99,16 @@ void integrateHashTsdfVolumeUnit( Mat volUnitsData = _volUnitsData.getMat(); Mat pixNorms = _pixNorms.getMat(); - //! Compute volumes to be allocated - const int depthStride = volumeUnitDegree; - const float invDepthFactor = 1.f / settings.getDepthFactor(); - const float truncDist = settings.getTsdfTruncateDistance(); + Matx44f _pose; + settings.getVolumePose(_pose); + const Affine3f pose = Affine3f(_pose); + const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); + + Matx33f intr; + settings.getCameraIntegrateIntrinsics(intr); + const Intr intrinsics(intr); + const Intr::Reprojector reproj(intrinsics.makeReprojector()); + const float maxDepth = settings.getMaxDepth(); const float voxelSize = settings.getVoxelSize(); @@ -112,89 +116,88 @@ void integrateHashTsdfVolumeUnit( settings.getVolumeResolution(resolution); const float volumeUnitSize = voxelSize * resolution[0]; - Matx33f intr; - settings.getCameraIntegrateIntrinsics(intr); - const Intr intrinsics(intr); - const Intr::Reprojector reproj(intrinsics.makeReprojector()); - - Matx44f _pose; - settings.getVolumePose(_pose); - const Affine3f pose = Affine3f(_pose); - const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); + if (enableGrowth) + { + //! Compute volumes to be allocated + const int depthStride = volumeUnitDegree; + const float invDepthFactor = 1.f / settings.getDepthFactor(); + const float truncDist = settings.getTsdfTruncateDistance(); - const Point3f truncPt(truncDist, truncDist, truncDist); - VolumeUnitIndexSet newIndices; - Mutex mutex; - Range allocateRange(0, depth.rows); + const Point3f truncPt(truncDist, truncDist, truncDist); + std::unordered_set newIndices; + Mutex mutex; + Range allocateRange(0, depth.rows); - auto AllocateVolumeUnitsInvoker = [&](const Range& range) { - VolumeUnitIndexSet localAccessVolUnits; - for (int y = range.start; y < range.end; y += depthStride) + auto AllocateVolumeUnitsInvoker = [&](const Range& range) { - const depthType* depthRow = depth[y]; - for (int x = 0; x < depth.cols; x += depthStride) + std::unordered_set localAccessVolUnits; + for (int y = range.start; y < range.end; y += depthStride) { - depthType z = depthRow[x] * invDepthFactor; + const depthType* depthRow = depth[y]; + for (int x = 0; x < depth.cols; x += depthStride) + { + depthType z = depthRow[x] * invDepthFactor; - if (z <= 0 || z > maxDepth) - continue; + if (z <= 0 || z > maxDepth) + continue; - Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); - Point3f volPoint = cam2vol * camPoint; - //! Find accessed TSDF volume unit for valid 3D vertex - Vec3i lower_bound = volumeToVolumeUnitIdx(volPoint - truncPt, volumeUnitSize); - Vec3i upper_bound = volumeToVolumeUnitIdx(volPoint + truncPt, volumeUnitSize); + Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); + Point3f volPoint = cam2vol * camPoint; + //! Find accessed TSDF volume unit for valid 3D vertex + Vec3i lower_bound = volumeToVolumeUnitIdx(volPoint - truncPt, volumeUnitSize); + Vec3i upper_bound = volumeToVolumeUnitIdx(volPoint + truncPt, volumeUnitSize); - for (int i = lower_bound[0]; i <= upper_bound[0]; i++) - for (int j = lower_bound[1]; j <= upper_bound[1]; j++) - for (int k = lower_bound[2]; k <= upper_bound[2]; k++) - { - const Vec3i tsdf_idx = Vec3i(i, j, k); - if (localAccessVolUnits.count(tsdf_idx) <= 0 && volumeUnits.count(tsdf_idx) <= 0) + for (int i = lower_bound[0]; i <= upper_bound[0]; i++) + for (int j = lower_bound[1]; j <= upper_bound[1]; j++) + for (int k = lower_bound[2]; k <= upper_bound[2]; k++) { - //! This volume unit will definitely be required for current integration - localAccessVolUnits.emplace(tsdf_idx); + const Vec3i tsdf_idx = Vec3i(i, j, k); + if (localAccessVolUnits.count(tsdf_idx) <= 0 && volumeUnits.count(tsdf_idx) <= 0) + { + //! This volume unit will definitely be required for current integration + localAccessVolUnits.emplace(tsdf_idx); + } } - } + } } - } - mutex.lock(); - for (const auto& tsdf_idx : localAccessVolUnits) - { - //! If the insert into the global set passes - if (!newIndices.count(tsdf_idx)) + mutex.lock(); + for (const auto& tsdf_idx : localAccessVolUnits) { - // Volume allocation can be performed outside of the lock - newIndices.emplace(tsdf_idx); + //! If the insert into the global set passes + if (!newIndices.count(tsdf_idx)) + { + // Volume allocation can be performed outside of the lock + newIndices.emplace(tsdf_idx); + } } - } - mutex.unlock(); - }; - parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); - //AllocateVolumeUnitsInvoker(allocateRange); + mutex.unlock(); + }; + parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); - //! Perform the allocation - for (auto idx : newIndices) - { - VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second; + //! Perform the allocation + for (auto idx : newIndices) + { + VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second; - Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix; + Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix; - vu.pose = subvolumePose; - vu.index = lastVolIndex; lastVolIndex++; - if (lastVolIndex > int(volUnitsData.size().height)) - { - volUnitsData.resize((lastVolIndex - 1) * 2); - } - volUnitsData.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) + vu.pose = subvolumePose; + vu.index = lastVolIndex; + lastVolIndex++; + if (lastVolIndex > int(volUnitsData.size().height)) + { + volUnitsData.resize((lastVolIndex - 1) * 2); + } + volUnitsData.row(vu.index).forEach([](VecTsdfVoxel &vv, const int * /* position */) { TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); - //! This volume unit will definitely be required for current integration - vu.lastVisibleIndex = frameId; - vu.isActive = true; + //! This volume unit will definitely be required for current integration + vu.lastVisibleIndex = frameId; + vu.isActive = true; + } } //! Get keys for all the allocated volume Units @@ -206,7 +209,8 @@ void integrateHashTsdfVolumeUnit( //! Mark volumes in the camera frustum as active Range inFrustumRange(0, (int)volumeUnits.size()); - parallel_for_(inFrustumRange, [&](const Range& range) { + parallel_for_(inFrustumRange, [&](const Range& range) + { const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); const Intr::Projector proj(intrinsics.makeProjector()); @@ -232,11 +236,12 @@ void integrateHashTsdfVolumeUnit( it->second.isActive = true; } } - }); + }); //! Integrate the correct volumeUnits - parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { + parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) + { for (int i = range.start; i < range.end; i++) { Vec3i tsdf_idx = totalVolUnits[i]; @@ -254,7 +259,7 @@ void integrateHashTsdfVolumeUnit( volumeUnit.isActive = false; } } - }); + }); } @@ -474,7 +479,7 @@ void markActive( void ocl_integrateHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth, InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags) { CV_TRACE_FUNCTION(); @@ -510,46 +515,49 @@ void ocl_integrateHashTsdfVolumeUnit( const Affine3f pose = Affine3f(_pose); Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix; - // Save length to fill new data in ranges - int sizeBefore = hashTable.last; - allocateVolumeUnits(depth, depthFactor, pose, cameraPose, intrinsics, hashTable, volumeUnitDegree, truncDist, maxDepth, volumeUnitSize); - int sizeAfter = hashTable.last; - //! Perform the allocation - - // Grow buffers - int buff_lvl = (int)(1 << bufferSizeDegree); - if (sizeAfter >= buff_lvl) + if (enableGrowth) { - bufferSizeDegree = (int)(log2(sizeAfter) + 1); // clz() would be better - int oldBuffSize = buff_lvl; - buff_lvl = (int)pow(2, bufferSizeDegree); + // Save length to fill new data in ranges + int sizeBefore = hashTable.last; + allocateVolumeUnits(depth, depthFactor, pose, cameraPose, intrinsics, hashTable, volumeUnitDegree, truncDist, maxDepth, volumeUnitSize); + int sizeAfter = hashTable.last; + //! Perform the allocation + + // Grow buffers + int buff_lvl = (int)(1 << bufferSizeDegree); + if (sizeAfter >= buff_lvl) + { + bufferSizeDegree = (int)(log2(sizeAfter) + 1); // clz() would be better + int oldBuffSize = buff_lvl; + buff_lvl = (int)pow(2, bufferSizeDegree); - volUnitsDataCopy.resize(buff_lvl); + volUnitsDataCopy.resize(buff_lvl); - Range oldr(0, oldBuffSize); - int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; - UMat newData(buff_lvl, volCubed, CV_8UC2); - volUnitsData.copyTo(newData.rowRange(oldr)); - volUnitsData = newData; + Range oldr(0, oldBuffSize); + int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; + UMat newData(buff_lvl, volCubed, CV_8UC2); + volUnitsData.copyTo(newData.rowRange(oldr)); + volUnitsData = newData; - UMat newLastVisibleIndices(buff_lvl, 1, CV_32S); - lastVisibleIndices.copyTo(newLastVisibleIndices.rowRange(oldr)); - lastVisibleIndices = newLastVisibleIndices; + UMat newLastVisibleIndices(buff_lvl, 1, CV_32S); + lastVisibleIndices.copyTo(newLastVisibleIndices.rowRange(oldr)); + lastVisibleIndices = newLastVisibleIndices; - UMat newIsActiveFlags(buff_lvl, 1, CV_8U); - isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr)); - isActiveFlags = newIsActiveFlags; - } + UMat newIsActiveFlags(buff_lvl, 1, CV_8U); + isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr)); + isActiveFlags = newIsActiveFlags; + } - // Fill data for new volume units - Range r(sizeBefore, sizeAfter); - if (r.start < r.end) - { - lastVisibleIndices.rowRange(r) = frameId; - isActiveFlags.rowRange(r) = 1; + // Fill data for new volume units + Range r(sizeBefore, sizeAfter); + if (r.start < r.end) + { + lastVisibleIndices.rowRange(r) = frameId; + isActiveFlags.rowRange(r) = 1; - TsdfVoxel emptyVoxel(floatToTsdf(0.0f), 0); - volUnitsData.rowRange(r) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); + TsdfVoxel emptyVoxel(floatToTsdf(0.0f), 0); + volUnitsData.rowRange(r) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); + } } //! Mark volumes in the camera frustum as active @@ -596,8 +604,6 @@ void ocl_integrateHashTsdfVolumeUnit( if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); - - //std::cout << "ocl_integrateHashTsdfVolumeUnit() end" << std::endl; } #endif diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.hpp b/modules/3d/src/rgbd/hash_tsdf_functions.hpp index de83574fe7..9da904e6d0 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.hpp +++ b/modules/3d/src/rgbd/hash_tsdf_functions.hpp @@ -283,11 +283,10 @@ public: int calcVolumeUnitDegree(Point3i volumeResolution); -typedef std::unordered_set VolumeUnitIndexSet; typedef std::unordered_map VolumeUnitIndexes; void integrateHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits); void raycastHashTsdfVolumeUnit( @@ -304,7 +303,7 @@ void fetchPointsNormalsFromHashTsdfVolumeUnit( #ifdef HAVE_OPENCL void ocl_integrateHashTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, + const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth, InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags); void ocl_raycastHashTsdfVolumeUnit( diff --git a/modules/3d/src/rgbd/tsdf_functions.cpp b/modules/3d/src/rgbd/tsdf_functions.cpp index df6717d4cd..e935dcae12 100644 --- a/modules/3d/src/rgbd/tsdf_functions.cpp +++ b/modules/3d/src/rgbd/tsdf_functions.cpp @@ -38,8 +38,6 @@ void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm) #ifdef HAVE_OPENCL void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm) { - //std::cout << "ocl_preCalculationPixNorm" << std::endl; - // calculating this on CPU then uploading to GPU is faster than calculating this on GPU Mat cpuPixNorm; preCalculationPixNorm(size, intrinsics, cpuPixNorm); @@ -51,7 +49,7 @@ void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm) // Integrate void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, - InputArray _depth, InputArray _pixNorms, InputArray _volume) + InputArray _depth, InputArray _pixNorms, InputArray _volume) { Matx44f volumePose; settings.getVolumePose(volumePose); @@ -59,12 +57,9 @@ void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& came } -void integrateTsdfVolumeUnit( - const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, - InputArray _depth, InputArray _pixNorms, InputArray _volume) +void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, + InputArray _depth, InputArray _pixNorms, InputArray _volume) { - //std::cout << "integrateTsdfVolumeUnit" << std::endl; - Depth depth = _depth.getMat(); Mat volume = _volume.getMat(); Mat pixNorms = _pixNorms.getMat(); diff --git a/modules/3d/src/rgbd/volume_impl.cpp b/modules/3d/src/rgbd/volume_impl.cpp index 6ebf3a1dcb..43d84cdeca 100644 --- a/modules/3d/src/rgbd/volume_impl.cpp +++ b/modules/3d/src/rgbd/volume_impl.cpp @@ -66,8 +66,8 @@ void TsdfVolume::integrate(InputArray _depth, InputArray _cameraPose) settings.getCameraIntegrateIntrinsics(intr); Intr intrinsics(intr); Vec6f newParams((float)depth.rows, (float)depth.cols, - intrinsics.fx, intrinsics.fy, - intrinsics.cx, intrinsics.cy); + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); if (!(frameParams == newParams)) { frameParams = newParams; @@ -143,7 +143,6 @@ void TsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) con ocl_fetchPointsNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals); else fetchPointsNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals); - #endif } @@ -164,7 +163,7 @@ void TsdfVolume::reset() }); #else if (useGPU) - gpu_volume.setTo(Scalar(0, 0)); + gpu_volume.setTo(Scalar(floatToTsdf(0.0f), 0)); else //TODO: use setTo(Scalar(0, 0)) cpu_volume.forEach([](VecTsdfVoxel& vv, const int* /* position */) @@ -178,6 +177,29 @@ int TsdfVolume::getVisibleBlocks() const { return 1; } size_t TsdfVolume::getTotalVolumeUnits() const { return 1; } +Vec6f TsdfVolume::getBoundingBox(int precision) const +{ + if (precision == Volume::BoundingBoxPrecision::VOXEL) + { + CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet"); + } + else + { + float sz = this->settings.getVoxelSize(); + Vec3f res; + this->settings.getVolumeResolution(res); + Vec3f volSize = res * sz; + return Vec6f(0, 0, 0, volSize[0], volSize[1], volSize[2]); + } +} + +void TsdfVolume::setEnableGrowth(bool /*v*/) { } + +bool TsdfVolume::getEnableGrowth() const +{ + return false; +} + // HASH_TSDF @@ -246,16 +268,18 @@ void HashTsdfVolume::integrate(InputArray _depth, InputArray _cameraPose) #endif } #ifndef HAVE_OPENCL - integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, pixNorms, volUnitsData, volumeUnits); + integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, enableGrowth, depth, pixNorms, volUnitsData, volumeUnits); lastFrameId++; #else if (useGPU) { - ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, depth, gpu_pixNorms, lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags); + ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, enableGrowth, depth, gpu_pixNorms, + lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags); } else { - integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits); + integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, enableGrowth, depth, + cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits); lastFrameId++; } #endif @@ -324,6 +348,7 @@ void HashTsdfVolume::reset() CV_TRACE_FUNCTION(); lastVolIndex = 0; lastFrameId = 0; + enableGrowth = true; #ifndef HAVE_OPENCL volUnitsData.forEach([](VecTsdfVoxel& vv, const int* /* position */) { @@ -364,6 +389,92 @@ void HashTsdfVolume::reset() int HashTsdfVolume::getVisibleBlocks() const { return 1; } size_t HashTsdfVolume::getTotalVolumeUnits() const { return 1; } +void HashTsdfVolume::setEnableGrowth(bool v) +{ + enableGrowth = v; +} + +bool HashTsdfVolume::getEnableGrowth() const +{ + return enableGrowth; +} + +Vec6f HashTsdfVolume::getBoundingBox(int precision) const +{ + if (precision == Volume::BoundingBoxPrecision::VOXEL) + { + CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet"); + } + else + { + Vec3i res; + this->settings.getVolumeResolution(res); + float voxelSize = this->settings.getVoxelSize(); + float side = res[0] * voxelSize; + + std::vector vi; +#ifndef HAVE_OPENCL + for (const auto& keyvalue : volumeUnits) + { + vi.push_back(keyvalue.first); + } +#else + if (useGPU) + { + for (int row = 0; row < hashTable.last; row++) + { + Vec4i idx4 = hashTable.data[row]; + vi.push_back(Vec3i(idx4[0], idx4[1], idx4[2])); + } + } + else + { + for (const auto& keyvalue : cpu_volumeUnits) + { + vi.push_back(keyvalue.first); + } + } +#endif + + if (vi.empty()) + { + return Vec6f(); + } + else + { + std::vector pts; + for (Vec3i idx : vi) + { + Point3f base = Point3f(idx[0], idx[1], idx[2]) * side; + pts.push_back(base); + pts.push_back(base + Point3f(side, 0, 0)); + pts.push_back(base + Point3f(0, side, 0)); + pts.push_back(base + Point3f(0, 0, side)); + pts.push_back(base + Point3f(side, side, 0)); + pts.push_back(base + Point3f(side, 0, side)); + pts.push_back(base + Point3f(0, side, side)); + pts.push_back(base + Point3f(side, side, side)); + } + + const float mval = std::numeric_limits::max(); + Vec6f bb(mval, mval, mval, -mval, -mval, -mval); + for (auto p : pts) + { + // pt in local coords + Point3f pg = p; + bb[0] = min(bb[0], pg.x); + bb[1] = min(bb[1], pg.y); + bb[2] = min(bb[2], pg.z); + bb[3] = max(bb[3], pg.x); + bb[4] = max(bb[4], pg.y); + bb[5] = max(bb[5], pg.z); + } + + return bb; + } + } +} + // COLOR_TSDF ColorTsdfVolume::ColorTsdfVolume(const VolumeSettings& _settings) : @@ -452,4 +563,27 @@ void ColorTsdfVolume::reset() int ColorTsdfVolume::getVisibleBlocks() const { return 1; } size_t ColorTsdfVolume::getTotalVolumeUnits() const { return 1; } +Vec6f ColorTsdfVolume::getBoundingBox(int precision) const +{ + if (precision == Volume::BoundingBoxPrecision::VOXEL) + { + CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet"); + } + else + { + float sz = this->settings.getVoxelSize(); + Vec3f res; + this->settings.getVolumeResolution(res); + Vec3f volSize = res * sz; + return Vec6f(0, 0, 0, volSize[0], volSize[1], volSize[2]); + } +} + +void ColorTsdfVolume::setEnableGrowth(bool /*v*/) { } + +bool ColorTsdfVolume::getEnableGrowth() const +{ + return false; +} + } diff --git a/modules/3d/src/rgbd/volume_impl.hpp b/modules/3d/src/rgbd/volume_impl.hpp index b0af2b5a54..01267b5fde 100644 --- a/modules/3d/src/rgbd/volume_impl.hpp +++ b/modules/3d/src/rgbd/volume_impl.hpp @@ -37,6 +37,10 @@ public: virtual int getVisibleBlocks() const = 0; virtual size_t getTotalVolumeUnits() const = 0; + virtual Vec6f getBoundingBox(int precision) const = 0; + virtual void setEnableGrowth(bool v) = 0; + virtual bool getEnableGrowth() const = 0; + public: const VolumeSettings& settings; #ifdef HAVE_OPENCL @@ -65,6 +69,19 @@ public: virtual int getVisibleBlocks() const override; virtual size_t getTotalVolumeUnits() const override; + // Gets bounding box in volume coordinates with given precision: + // VOLUME_UNIT - up to volume unit + // VOXEL - up to voxel + // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates + virtual Vec6f getBoundingBox(int precision) const override; + + // Enabels or disables new volume unit allocation during integration + // Applicable for HashTSDF only + virtual void setEnableGrowth(bool v) override; + // Returns if new volume units are allocated during integration or not + // Applicable for HashTSDF only + virtual bool getEnableGrowth() const override; + public: Vec6f frameParams; #ifndef HAVE_OPENCL @@ -105,11 +122,26 @@ public: virtual void reset() override; virtual int getVisibleBlocks() const override; virtual size_t getTotalVolumeUnits() const override; + + // Enabels or disables new volume unit allocation during integration + // Applicable for HashTSDF only + virtual void setEnableGrowth(bool v) override; + // Returns if new volume units are allocated during integration or not + // Applicable for HashTSDF only + virtual bool getEnableGrowth() const override; + + // Gets bounding box in volume coordinates with given precision: + // VOLUME_UNIT - up to volume unit + // VOXEL - up to voxel + // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates + virtual Vec6f getBoundingBox(int precision) const override; + public: int lastVolIndex; int lastFrameId; Vec6f frameParams; int volumeUnitDegree; + bool enableGrowth; #ifndef HAVE_OPENCL Mat volUnitsData; @@ -154,6 +186,20 @@ public: virtual void reset() override; virtual int getVisibleBlocks() const override; virtual size_t getTotalVolumeUnits() const override; + + // Gets bounding box in volume coordinates with given precision: + // VOLUME_UNIT - up to volume unit + // VOXEL - up to voxel + // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates + virtual Vec6f getBoundingBox(int precision) const override; + + // Enabels or disables new volume unit allocation during integration + // Applicable for HashTSDF only + virtual void setEnableGrowth(bool v) override; + // Returns if new volume units are allocated during integration or not + // Applicable for HashTSDF only + virtual bool getEnableGrowth() const override; + private: Vec4i volStrides; Vec6f frameParams; @@ -203,6 +249,10 @@ void Volume::reset() { this->impl->reset(); } int Volume::getVisibleBlocks() const { return this->impl->getVisibleBlocks(); } size_t Volume::getTotalVolumeUnits() const { return this->impl->getTotalVolumeUnits(); } +Vec6f Volume::getBoundingBox(int precision) const { return this->impl->getBoundingBox(precision); } +void Volume::setEnableGrowth(bool v) { this->impl->setEnableGrowth(v); } +bool Volume::getEnableGrowth() const { return this->impl->getEnableGrowth(); } + } diff --git a/modules/3d/test/ocl/test_tsdf.cpp b/modules/3d/test/ocl/test_tsdf.cpp index 792da2f2f9..f372e1049a 100644 --- a/modules/3d/test/ocl/test_tsdf.cpp +++ b/modules/3d/test/ocl/test_tsdf.cpp @@ -571,6 +571,7 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; } + TEST(TSDF_GPU, raycast_custom_framesize_normals_mat) { normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT); diff --git a/modules/3d/test/test_tsdf.cpp b/modules/3d/test/test_tsdf.cpp index 73676dd4c0..b08421c693 100644 --- a/modules/3d/test/test_tsdf.cpp +++ b/modules/3d/test/test_tsdf.cpp @@ -231,8 +231,8 @@ struct SemisphereScene : Scene pose = pose.rotate(startPose.rotation()); pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle); pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), - startPose.translation()[1], - startPose.translation()[2] * cos(angle))); + startPose.translation()[1], + startPose.translation()[2] * cos(angle))); poses.push_back(pose); } @@ -424,13 +424,13 @@ void normalsCheck(Mat normals) for (auto pvector = normals.begin(); pvector < normals.end(); pvector++) { vector = *pvector; - if (!cvIsNaN(vector[0])) + if (!(cvIsNaN(vector[0]) || cvIsNaN(vector[1]) || cvIsNaN(vector[2]))) { counter++; - float length = vector[0] * vector[0] + - vector[1] * vector[1] + - vector[2] * vector[2]; - ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + float l2 = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1.f - l2), 0.0001f) << "There is normal with length != 1"; } } ASSERT_GT(counter, 0) << "There are not normals"; @@ -537,6 +537,7 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test normalsCheck(normals); } + void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction testFunction, VolumeTestSrcType testSrcType) { VolumeSettings vs(volumeType); @@ -603,6 +604,7 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test normalsCheck(normals); } + void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType) { VolumeSettings vs(volumeType); @@ -679,6 +681,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; } + void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType) { VolumeSettings vs(volumeType); @@ -755,6 +758,117 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")"; } + +void debugVolumeDraw(const Volume &volume, Affine3f pose, Mat depth, float depthFactor, std::string objFname) +{ + Vec3f lightPose = Vec3f::all(0.f); + Mat points, normals; + volume.raycast(pose.matrix, points, normals); + + Mat ptsList, ptsList3, nrmList, nrmList3; + volume.fetchPointsNormals(ptsList, nrmList); + // transform 4 channels to 3 channels + cvtColor(ptsList, ptsList3, COLOR_BGRA2BGR); + cvtColor(ptsList, nrmList3, COLOR_BGRA2BGR); + savePointCloud(objFname, ptsList3, nrmList3); + + displayImage(depth, points, normals, depthFactor, lightPose); +} + + +// For fixed volumes which are TSDF and ColorTSDF +void staticBoundingBoxTest(VolumeType volumeType) +{ + VolumeSettings vs(volumeType); + Volume volume(volumeType, vs); + + Vec3i res; + vs.getVolumeResolution(res); + float voxelSize = vs.getVoxelSize(); + Matx44f pose; + vs.getVolumePose(pose); + Vec3f end = voxelSize * Vec3f(res); + Vec6f truebb(0, 0, 0, end[0], end[1], end[2]); + Vec6f bb = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT); + Vec6f diff = bb - truebb; + double normdiff = std::sqrt(diff.ddot(diff)); + ASSERT_LE(normdiff, std::numeric_limits::epsilon()); +} + + +// For HashTSDF only +void boundingBoxGrowthTest(bool enableGrowth) +{ + VolumeSettings vs(VolumeType::HashTSDF); + Volume volume(VolumeType::HashTSDF, vs); + + Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); + Matx33f intr; + vs.getCameraIntegrateIntrinsics(intr); + bool onlySemisphere = false; + float depthFactor = vs.getDepthFactor(); + Ptr scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); + std::vector poses = scene->getPoses(); + + Mat depth = scene->depth(poses[0]); + UMat udepth; + depth.copyTo(udepth); + + // depth is integrated with multiple weight + // TODO: add weight parameter to integrate() call (both scalar and array of 8u/32f) + const int nIntegrations = 1; + for (int i = 0; i < nIntegrations; i++) + volume.integrate(udepth, poses[0].matrix); + + Vec6f bb = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT); + Vec6f truebb(-0.9375f, 1.3125f, -0.8906f, 3.9375f, 2.6133f, 1.4004f); + Vec6f diff = bb - truebb; + double bbnorm = std::sqrt(diff.ddot(diff)); + + Vec3f vuRes; + vs.getVolumeResolution(vuRes); + double vuSize = vs.getVoxelSize() * vuRes[0]; + // it's OK to have such big difference since this is volume unit size-grained BB calculation + // Theoretical max difference can be sqrt(6) =(approx)= 2.4494 + EXPECT_LE(bbnorm, vuSize * 2.38); + + if (cvtest::debugLevel > 0) + { + debugVolumeDraw(volume, poses[0], depth, depthFactor, "pts.obj"); + } + + // Integrate another depth growth changed + + Mat depth2 = scene->depth(poses[0].translate(Vec3f(0, -0.25f, 0))); + UMat udepth2; + depth2.copyTo(udepth2); + + volume.setEnableGrowth(enableGrowth); + + for (int i = 0; i < nIntegrations; i++) + volume.integrate(udepth2, poses[0].matrix); + + Vec6f bb2 = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT); + + Vec6f truebb2 = truebb + Vec6f(0, -(1.3125f - 1.0723f), -(-0.8906f - (-1.4238f)), 0, 0, 0); + Vec6f diff2 = enableGrowth ? bb2 - truebb2 : bb2 - bb; + double bbnorm2 = std::sqrt(diff2.ddot(diff2)); + EXPECT_LE(bbnorm2, enableGrowth ? (vuSize * 2.3) : std::numeric_limits::epsilon()); + + if (cvtest::debugLevel > 0) + { + debugVolumeDraw(volume, poses[0], depth, depthFactor, enableGrowth ? "pts_growth.obj" : "pts_no_growth.obj"); + } + + // Reset check + + volume.reset(); + Vec6f bb3 = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT); + double bbnorm3 = std::sqrt(bb3.ddot(bb3)); + EXPECT_LE(bbnorm3, std::numeric_limits::epsilon()); +} + + static Mat nanMask(Mat img) { int depth = img.depth(); @@ -1013,6 +1127,17 @@ TEST(HashTSDF, valid_points_common_framesize_frame) valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME); } +class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam +{ }; + +TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) +{ + boundingBoxGrowthTest(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Bool()); + + TEST(HashTSDF, reproduce_volPoseRot) { regressionVolPoseRot(); @@ -1068,6 +1193,16 @@ TEST(ColorTSDF, valid_points_common_framesize_fetch) valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME); } +class StaticVolumeBoundingBox : public ::testing::TestWithParam +{ }; + +TEST_P(StaticVolumeBoundingBox, boundingBox) +{ + staticBoundingBoxTest(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); + #else TEST(TSDF_CPU, raycast_custom_framesize_normals_mat) { @@ -1287,12 +1422,77 @@ TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch) } +// used to store current OpenCL status (on/off) and revert it after test is done +// works even after exceptions thrown in test body +struct OpenCLStatusRevert +{ + OpenCLStatusRevert(bool oclStatus) : originalOpenCLStatus(oclStatus) { } + ~OpenCLStatusRevert() + { + cv::ocl::setUseOpenCL(originalOpenCLStatus); + } + bool originalOpenCLStatus; +}; + +class StaticVolumeBoundingBox : public ::testing::TestWithParam +{ }; + +TEST_P(StaticVolumeBoundingBox, boundingBoxCPU) +{ + OpenCLStatusRevert revert(cv::ocl::useOpenCL()); + + cv::ocl::setUseOpenCL(false); + + staticBoundingBoxTest(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); + + // OpenCL tests TEST(HashTSDF_GPU, reproduce_volPoseRot) { regressionVolPoseRot(); } +//TODO: use this when ColorTSDF gets GPU support +/* +class StaticVolumeBoundingBox : public ::testing::TestWithParam +{ }; + +TEST_P(StaticVolumeBoundingBox, GPU) +{ + staticBoundingBoxTest(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); +*/ + +// until that, we don't need parametrized test +TEST(TSDF, boundingBoxGPU) +{ + staticBoundingBoxTest(VolumeType::TSDF); +} + + +class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam> +{ }; + +TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) +{ + auto p = GetParam(); + bool gpu = std::get<0>(p); + bool enableGrowth = std::get<1>(p); + + OpenCLStatusRevert revert(cv::ocl::useOpenCL()); + + if (!gpu) + cv::ocl::setUseOpenCL(false); + + boundingBoxGrowthTest(enableGrowth); +} + +INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Combine(::testing::Bool(), ::testing::Bool())); #endif }