From 9d232fe7f29a5d485f0c0bc5ca444a5ace61afa0 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Sep 2025 07:36:13 +0200 Subject: [PATCH] Merge pull request #27801 from savuor:rv/fix_hashtsdf_expand Fixed HashTSDF expand bug #27801 HashTSDF should expand volume units during integration. It didn't happen in fact because of wrong Mat passing. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/3d/src/rgbd/hash_tsdf_functions.cpp | 17 +-- modules/3d/src/rgbd/hash_tsdf_functions.hpp | 4 +- modules/3d/test/test_tsdf.cpp | 140 +++++++++++++++++++- 3 files changed, 147 insertions(+), 14 deletions(-) diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.cpp b/modules/3d/src/rgbd/hash_tsdf_functions.cpp index d6cd210e24..15ad3197cc 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.cpp +++ b/modules/3d/src/rgbd/hash_tsdf_functions.cpp @@ -90,13 +90,13 @@ Point3f ocl_getNormalVoxel( void integrateHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, - InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits) + InputArray _depth, InputArray _pixNorms, InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits) { CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); Depth depth = _depth.getMat(); - Mat volUnitsData = _volUnitsData.getMat(); + Mat& volUnitsData = _volUnitsData.getMatRef(); Mat pixNorms = _pixNorms.getMat(); Matx44f _pose; @@ -184,11 +184,12 @@ void integrateHashTsdfVolumeUnit( vu.pose = subvolumePose; vu.index = lastVolIndex; - lastVolIndex++; - if (lastVolIndex > int(volUnitsData.size().height)) + if (lastVolIndex >= int(volUnitsData.size().height)) { - volUnitsData.resize((lastVolIndex - 1) * 2); + volUnitsData.resize(lastVolIndex * 2); + CV_LOG_DEBUG(NULL, "HashTSDF storage extended from " << lastVolIndex << " to " << lastVolIndex * 2 << " volume units"); } + lastVolIndex++; volUnitsData.row(vu.index).forEach([](VecTsdfVoxel &vv, const int * /* position */) { TsdfVoxel& v = reinterpret_cast(vv); @@ -480,15 +481,15 @@ void markActive( void ocl_integrateHashTsdfVolumeUnit( 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) + InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputOutputArray _volUnitsDataCopy, InputOutputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags) { CV_TRACE_FUNCTION(); UMat depth = _depth.getUMat(); CV_Assert(!depth.empty()); CV_Assert(lastVolIndex >= 0); UMat pixNorms = _pixNorms.getUMat(); - UMat volUnitsData = _volUnitsData.getUMat(); - Mat volUnitsDataCopy = _volUnitsDataCopy.getMat(); + UMat& volUnitsData = _volUnitsData.getUMatRef(); + Mat& volUnitsDataCopy = _volUnitsDataCopy.getMatRef(); UMat isActiveFlags = _isActiveFlags.getUMat(); UMat lastVisibleIndices = _lastVisibleIndices.getUMat(); diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.hpp b/modules/3d/src/rgbd/hash_tsdf_functions.hpp index 253fdaddf8..52c42675be 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.hpp +++ b/modules/3d/src/rgbd/hash_tsdf_functions.hpp @@ -287,7 +287,7 @@ typedef std::unordered_map VolumeUnitIndexes; void integrateHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, - InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits); + InputArray _depth, InputArray _pixNorms, InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits); void raycastHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, @@ -304,7 +304,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, bool enableGrowth, - InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags); + InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputOutputArray _volUnitsDataCopy, InputOutputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags); void ocl_raycastHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, diff --git a/modules/3d/test/test_tsdf.cpp b/modules/3d/test/test_tsdf.cpp index 20077e830e..d907b9f56c 100644 --- a/modules/3d/test/test_tsdf.cpp +++ b/modules/3d/test/test_tsdf.cpp @@ -302,9 +302,9 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im Range range(0, sz.height); const int nstripes = -1; - parallel_for_(range, [&](const Range&) + parallel_for_(range, [&](const Range& rows) { - for (int y = range.start; y < range.end; y++) + for (int y = rows.start; y < rows.end; y++) { Vec4b* imgRow = img[y]; const ptype* ptsRow = points[y]; @@ -365,9 +365,9 @@ void renderPointsNormalsColors(InputArray _points, InputArray, InputArray _color Range range(0, sz.height); const int nstripes = -1; - parallel_for_(range, [&](const Range&) + parallel_for_(range, [&](const Range& r) { - for (int y = range.start; y < range.end; y++) + for (int y = r.start; y < r.end; y++) { Vec4b* imgRow = img[y]; const ptype* clrRow = colors[y]; @@ -594,6 +594,119 @@ void boundingBoxGrowthTest(bool enableGrowth) } +struct CubesScene : Scene +{ + const Affine3f startPose = Affine3f(Vec3f(-1.5f, 0.f, 0.f), Vec3f(1.5f, 3.3f, -2.1f)); + + const int nFrames = 100; + const Vec3f shift = Vec3f(0.2f, 0.01f, 0.1f); + Size frameSize; + Matx33f intr; + float depthFactor; + + CubesScene(Size sz, Matx33f _intr, float _depthFactor) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor) + { } + + static float map(Point3f p, bool /*unused*/) + { + float plane = p.y + 0.5f; + + float step = 0.7f; + cv::Point3f boxPose {std::fmod(p.x, step)*(p.x < 0.f ? -1.f : 1.f) - step / 2.f, + p.y, + std::fmod(p.z, step)*(p.z < 0.f ? -1.f : 1.f) - step / 2.f}; + float boxSize = 0.3f; + float roundness = 0.01f; + cv::Point3f boxTmp; + boxTmp.x = std::max(std::abs(boxPose.x) - boxSize, 0.0f); + boxTmp.y = std::max(std::abs(boxPose.y) - boxSize, 0.0f); + boxTmp.z = std::max(std::abs(boxPose.z) - boxSize, 0.0f); + float roundBox = (float)cv::norm(boxTmp) - roundness; + + float sphereRadius = 0.4f; + float sphere = (float)cv::norm(boxPose) - sphereRadius; + + float boxMinusSphere = std::max(roundBox, -sphere); + + float res = std::min(boxMinusSphere, plane); + + return res; + } + + Mat_ depth(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor, false /*unused*/)); + + return frame; + } + + Mat_ rgb(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderColorInvoker(frame, pose, reproj, depthFactor, false /*unused*/)); + + return frame; + } + + std::vector getPoses() override + { + std::vector poses; + for (int i = 0; i < nFrames; i++) + { + Affine3f pose = startPose; + pose = pose.translate(shift * i); + poses.push_back(pose); + } + + return poses; + } +}; + +Ptr makeRepeatableScene(Size sz, Matx33f _intr, float _depthFactor) +{ + return makePtr(sz, _intr, _depthFactor); +} + +// For HashTSDF only +void hugeSceneGrowthTest() +{ + VolumeSettings vs(VolumeType::HashTSDF); + vs.setMaxDepth(10); + Volume volume(VolumeType::HashTSDF, vs); + + Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); + Matx33f intrIntegrate, intrRaycast; + vs.getCameraIntegrateIntrinsics(intrIntegrate); + vs.getCameraRaycastIntrinsics(intrRaycast); + float depthFactor = vs.getDepthFactor(); + Ptr scene = makeRepeatableScene(frameSize, intrIntegrate, depthFactor); + std::vector poses = scene->getPoses(); + + // this should exceed the standard size of 8192 volume units + // to grow volume more, use more poses + Mat depth = scene->depth(poses[0]); + UMat udepth; + depth.copyTo(udepth); + volume.integrate(udepth, poses[0].matrix); + if (cvtest::debugLevel > 0) + { + debugVolumeDraw(volume, poses[0], depth, depthFactor, "pts.obj"); + } + + // Reset check + + volume.reset(); +} + + template static Mat_ normalsErrorT(Mat_ srcNormals, Mat_ dstNormals) { @@ -1202,5 +1315,24 @@ TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) INSTANTIATE_TEST_CASE_P(Volume, BoundingBoxEnableGrowthTest, ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all())); + +class HugeSceneGrowthTest : public ::testing::TestWithParam +{ }; + +TEST_P(HugeSceneGrowthTest, boundingBoxEnableGrowth) +{ + auto p = GetParam(); + bool gpu = (p == PlatformType::GPU); + + OpenCLStatusRevert oclStatus; + + if (!gpu) + oclStatus.off(); + + hugeSceneGrowthTest(); +} + +INSTANTIATE_TEST_CASE_P(Volume, HugeSceneGrowthTest, PlatformTypeEnum::all()); + } } // namespace